Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, motor-nurbs, updated. 2672320990d4ee6d23b80bdcabd1ed52d99118b9
|
From: Xavier L. <Ba...@us...> - 2010-02-23 21:14:30
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, motor-nurbs has been updated
via 2672320990d4ee6d23b80bdcabd1ed52d99118b9 (commit)
from 821b31d67ca783924aa55eba5cf94d0b3a7f6247 (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 2672320990d4ee6d23b80bdcabd1ed52d99118b9
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Feb 23 22:13:26 2010 +0100
New architecture of the trajectory system
The functions are grouped in a dedicated file
The interrupt function is now a dedicated one executed in main.c
-----------------------------------------------------------------------
Changes:
diff --git a/USB_Module/Motor_Controller/Firmware/PcInterface.h b/USB_Module/Motor_Controller/Firmware/PcInterface.h
index 530d113..8fd98e0 100644
--- a/USB_Module/Motor_Controller/Firmware/PcInterface.h
+++ b/USB_Module/Motor_Controller/Firmware/PcInterface.h
@@ -156,9 +156,6 @@ typedef struct _UP {
#define TRAJ_NOT_COMPLETED 0x00
#define TRAJ_COMPLETED 0x01
-#define TRAJ_TYPE_NONE 0x00
-#define TRAJ_TYPE_BEZIER3 0x01
-
#define TRAJ_STOP_MOTOR_OFF 256
#define TRAJ_STOP_ABRUPT 512
#define TRAJ_STOP_SMOOTH 1024
diff --git a/USB_Module/Motor_Controller/Firmware/lm629.c b/USB_Module/Motor_Controller/Firmware/lm629.c
index fc76286..0ee0b12 100644
--- a/USB_Module/Motor_Controller/Firmware/lm629.c
+++ b/USB_Module/Motor_Controller/Firmware/lm629.c
@@ -11,26 +11,6 @@
volatile DWORD_VAL posRight = 0;
volatile DWORD_VAL posLeft = 0;
-/* Variables pour le suivi de trajectoires
- * ces varibales permettent au syst de suivi de trajectoire de fonctionner
- * et d'anger des donn entre le code contenu dans plusieurs fichiers
-*/
-volatile BOOL gIsFollowingTrajectory = FALSE;
-volatile BOOL gIsTrajectoryNew = FALSE;
-volatile long gTrajectoryTime = 0;
-volatile BYTE gTrajectoryType = TRAJ_TYPE_NONE;
-/* Le tableau suivant contient les donn nssaires 'extion de la trajectoire
- * Pour TRAJ_TYPE_BEZIER3 :
- * - gTrajectoryData[0] : parame cx
- * - gTrajectoryData[1] : parame cy
- * - gTrajectoryData[2] : parame bx
- * - gTrajectoryData[3] : parame by
- * - gTrajectoryData[4] : parame ax
- * - gTrajectoryData[5] : parame ay
- * - gTrajectoryData[6] : vitesse de parcours de l'abscisse curviligne
-*/
-volatile float gTrajectoryData[10];
-
/**
* Change la direction du BUS de donn
* Cette fonction est utilisen interne et ne doit pas e appelsinon.
@@ -777,19 +757,6 @@ void goTo(short x, short y, short vel, short acc, BYTE mode, short d) {
}
}
-void goToBezier(short x0, short y0, short x1, short y1, short x2, short y2, short x3, short y3, short vel) {
- gIsFollowingTrajectory = FALSE;
- gTrajectoryData[0] = 3.0 * ((float)x1 - (float)x0);
- gTrajectoryData[1] = 3.0 * ((float)y1 - (float)y0);
- gTrajectoryData[2] = 3.0 * ((float)x2 - (float)x1) - gTrajectoryData[0];
- gTrajectoryData[3] = 3.0 * ((float)y2 - (float)y1) - gTrajectoryData[1];
- gTrajectoryData[4] = (float)x3 - (float)x0 - gTrajectoryData[2] - gTrajectoryData[0];
- gTrajectoryData[5] = (float)y3 - (float)y0 - gTrajectoryData[3] - gTrajectoryData[1];
- gTrajectoryData[6] = (float)vel;
- gTrajectoryType = TRAJ_TYPE_BEZIER3;
- gIsTrajectoryNew = TRUE;
-}
-
WORD getRelPosRight() {
DWORD_VAL pos;
diff --git a/USB_Module/Motor_Controller/Firmware/main.c b/USB_Module/Motor_Controller/Firmware/main.c
index b244776..a0637bc 100644
--- a/USB_Module/Motor_Controller/Firmware/main.c
+++ b/USB_Module/Motor_Controller/Firmware/main.c
@@ -90,6 +90,7 @@
#include "error.h"
#include "lm629.h"
#include "motor.h"
+#include "trajectory.h"
/* VARIABLES ******************************************************/
#pragma udata
@@ -233,7 +234,8 @@ char ResetSource(void);
s++;
}
- interruptMotor();
+ interruptCurrentMeasure();
+ interruptTrajectory();
// On rtorise l'interruption
PIR1bits.TMR1IF = 0;
diff --git a/USB_Module/Motor_Controller/Firmware/motor.c b/USB_Module/Motor_Controller/Firmware/motor.c
index 25e10a6..4d65ca1 100644
--- a/USB_Module/Motor_Controller/Firmware/motor.c
+++ b/USB_Module/Motor_Controller/Firmware/motor.c
@@ -28,14 +28,12 @@ extern volatile float gTrajectoryData[10];
* Cette fonction ne doit e appelque dans une interruption.
* Elle permet la mesure du courant dans le moteur.
*/
-void interruptMotor(void) {
+void interruptCurrentMeasure(void) {
static char state = 0;
static char Isens_idx0 = 0;
static char Isens_idx1 = 0;
static long Isens1_0[ISENS_AVR0];
static long Isens2_0[ISENS_AVR0];
- static float vx, vy, v = 0, t = 0, theta;
- static DWORD velR, velL;
char i;
@@ -111,37 +109,6 @@ void interruptMotor(void) {
gIsTrajectoryNew = FALSE;
gIsFollowingTrajectory = TRUE;
}
-
- // Suivi de trajectoire
- if (gIsFollowingTrajectory == TRUE) {
- gTrajectoryTime += 1;
- t = t + v*0.001*gTrajectoryData[6];
- if (t >= 1) {
- stop(MOTOR_BOTH, LM_LTRJ_STOP_ABRUPT);
- gIsFollowingTrajectory = FALSE;
- } else {
- if (gTrajectoryType == TRAJ_TYPE_BEZIER3) {
- // On drmine la nouvelle vitesse
- vx = 3*gTrajectoryData[4]*t*t + 2*gTrajectoryData[2]*t + gTrajectoryData[0];
- vy = 3*gTrajectoryData[5]*t*t + 2*gTrajectoryData[3]*t + gTrajectoryData[1];
- v = sqrt(vx*vx + vy*vy);
- vx = vx / v;
- vy = vy / v;
- theta = atan2(vy, vx);
- if (theta >= 0) {
- velR = gTrajectoryData[6];
- velL = (gTrajectoryData[6] + (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) / (gTrajectoryData[6] - (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) * velR;
- }
- if (theta >= 0) {
- velL = gTrajectoryData[6];
- velR = (gTrajectoryData[6] + (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) / (gTrajectoryData[6] - (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) * velL;
- }
- changeVelocity(MOTOR_LEFT, velL * COEF_LEFT_WHEEL * CONST_VEL, 0);
- changeVelocity(MOTOR_RIGHT, velR * COEF_LEFT_WHEEL * CONST_VEL, 0);
- }
- // pas d'autre type de trajectoire implntour le moment
- }
- }
}
/**
diff --git a/USB_Module/Motor_Controller/Firmware/motor.h b/USB_Module/Motor_Controller/Firmware/motor.h
index 1e71445..a5009ea 100644
--- a/USB_Module/Motor_Controller/Firmware/motor.h
+++ b/USB_Module/Motor_Controller/Firmware/motor.h
@@ -9,7 +9,6 @@
#include <adc.h>
#include "Compiler.h"
#include "HardwareProfile.h"
-#include "lm629.h"
// Parames graux
#define ISENS_R 0.02 ///< Valeur de la rstance de mesure du courant (en ohm)
@@ -85,7 +84,7 @@
); \
}
-void interruptMotor(void);
+void interruptCurrentMeasure(void);
long getIsens(char axis);
void enableMotor(char axis);
void disableMotor(char axis);
diff --git a/USB_Module/Motor_Controller/Firmware/trajectory.c b/USB_Module/Motor_Controller/Firmware/trajectory.c
new file mode 100644
index 0000000..fc9770c
--- /dev/null
+++ b/USB_Module/Motor_Controller/Firmware/trajectory.c
@@ -0,0 +1,97 @@
+/**
+ * @file trajectory.c
+ * G le syst de suivi de trajectoire.
+*/
+
+#ifndef TRAJECTORY_C
+#define TRAJECTORY_C
+
+/* Variables pour le suivi de trajectoires
+ * ces varibales permettent au syst de suivi de trajectoire de fonctionner
+ * et d'anger des donn entre le code contenu dans plusieurs fichiers
+*/
+volatile BOOL gIsFollowingTrajectory = FALSE; // Indique si une trajectoire est en cours de suivi
+volatile BOOL gIsTrajectoryNew = FALSE; // Indique qu'il faut drrer une nouvelle trajectoire
+volatile long gTrajectorySampleRate = 10; // Pode d'antillonnage de la commande de vitesse (en ms)
+volatile long gTrajectoryTime = 0; // Temps ulepuis le dt du suivi de trajectoire
+volatile BYTE gTrajectoryType = TRAJ_TYPE_NONE; // Type de trajectoire en cours
+/* Le tableau suivant contient les donn nssaires 'extion de la trajectoire
+ * pour TRAJ_TYPE_LINE :
+ * - gTrajectoryData[0] : vitesse
+ * - gTrajectoryData[1] : accration
+ * - gTrajectoryData[2] : distance arcourir
+ * Pour TRAJ_TYPE_BEZIER3 :
+ * - gTrajectoryData[0] : parame cx
+ * - gTrajectoryData[1] : parame cy
+ * - gTrajectoryData[2] : parame bx
+ * - gTrajectoryData[3] : parame by
+ * - gTrajectoryData[4] : parame ax
+ * - gTrajectoryData[5] : parame ay
+ * - gTrajectoryData[6] : vitesse de parcours de l'abscisse curviligne
+*/
+volatile float gTrajectoryData[10];
+
+/**
+ * Fonction d'interruption pour la gestion des trajectoires.
+ * Cette fonction ne doit e appelque dans une interruption.
+ * Elle permet le suivi de trajectoire par le robot.
+*/
+void interruptTrajectory(void) {
+ static float vx, vy, v = 0, t = 0, theta;
+ static DWORD velR, velL;
+
+
+ // Initialisation de la trajectoire
+ if (gIsTrajectoryNew == TRUE) {
+ v = 0.0;
+ t = 0.0;
+ gIsTrajectoryNew = FALSE;
+ gIsFollowingTrajectory = TRUE;
+ }
+
+ // Suivi de trajectoire
+ if (gIsFollowingTrajectory == TRUE) {
+ gTrajectoryTime += 1;
+ t = t + v*0.001*gTrajectoryData[6];
+ if (t >= 1) {
+ stop(MOTOR_BOTH, LM_LTRJ_STOP_ABRUPT);
+ gIsFollowingTrajectory = FALSE;
+ } else {
+ if (gTrajectoryType == TRAJ_TYPE_BEZIER3) {
+ // On drmine la nouvelle vitesse
+ vx = 3*gTrajectoryData[4]*t*t + 2*gTrajectoryData[2]*t + gTrajectoryData[0];
+ vy = 3*gTrajectoryData[5]*t*t + 2*gTrajectoryData[3]*t + gTrajectoryData[1];
+ v = sqrt(vx*vx + vy*vy);
+ vx = vx / v;
+ vy = vy / v;
+ theta = atan2(vy, vx);
+ if (theta >= 0) {
+ velR = gTrajectoryData[6];
+ velL = (gTrajectoryData[6] + (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) / (gTrajectoryData[6] - (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) * velR;
+ }
+ if (theta >= 0) {
+ velL = gTrajectoryData[6];
+ velR = (gTrajectoryData[6] + (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) / (gTrajectoryData[6] - (((float) WHEELS_DIST)/2.0 * theta / 1000.0)) * velL;
+ }
+ changeVelocity(MOTOR_LEFT, velL * COEF_LEFT_WHEEL * CONST_VEL, 0);
+ changeVelocity(MOTOR_RIGHT, velR * COEF_LEFT_WHEEL * CONST_VEL, 0);
+ }
+ // pas d'autre type de trajectoire implntour le moment
+ }
+ }
+}
+
+void goToBezier(short x0, short y0, short x1, short y1, short x2, short y2, short x3, short y3, short vel) {
+ gIsFollowingTrajectory = FALSE;
+ gTrajectoryData[0] = 3.0 * ((float)x1 - (float)x0);
+ gTrajectoryData[1] = 3.0 * ((float)y1 - (float)y0);
+ gTrajectoryData[2] = 3.0 * ((float)x2 - (float)x1) - gTrajectoryData[0];
+ gTrajectoryData[3] = 3.0 * ((float)y2 - (float)y1) - gTrajectoryData[1];
+ gTrajectoryData[4] = (float)x3 - (float)x0 - gTrajectoryData[2] - gTrajectoryData[0];
+ gTrajectoryData[5] = (float)y3 - (float)y0 - gTrajectoryData[3] - gTrajectoryData[1];
+ gTrajectoryData[6] = (float)vel;
+ gTrajectoryType = TRAJ_TYPE_BEZIER3;
+ gIsTrajectoryNew = TRUE;
+}
+
+#endif
\ No newline at end of file
diff --git a/USB_Module/Motor_Controller/Firmware/trajectory.h b/USB_Module/Motor_Controller/Firmware/trajectory.h
new file mode 100644
index 0000000..6192b78
--- /dev/null
+++ b/USB_Module/Motor_Controller/Firmware/trajectory.h
@@ -0,0 +1,20 @@
+/**
+ * @file trajectory.h
+ * G le syst de suivi de trajectoire
+*/
+
+#ifndef TRAJECTORY_H
+#define TRAJECTORY_H
+
+#include "lm629.h"
+
+// Constantes descriptives
+#define TRAJ_TYPE_NONE 0x00
+#define TRAJ_TYPE_LINE 0x01
+//#define TRAJ_TYPE_ARC 0x02
+//#define TRAJ_TYPE_BEZIER3 0x03
+
+void interruptTrajectory(void);
+void goToBezier(short x0, short y0, short x1, short y1, short x2, short y2, short x3, short y3, short vel);
+
+#endif
\ No newline at end of file
hooks/post-receive
--
krobot
|
