RaC2018
added serial up, down, left and right control
Dependencies: mbed
Fork of
Robot_a_cables_v3_6
by 
RaC2018
Diff: main.cpp
- Revision:
- 8:1e6d3d9ae1d3
- Parent:
- 7:e87feff62bfd
- Child:
- 9:0f63d4cb5613
--- a/main.cpp Mon May 14 15:27:10 2018 +0000
+++ b/main.cpp Tue May 15 10:02:20 2018 +0000
@@ -7,7 +7,10 @@
*/
+ #define DEGRAD 0
+ #define NORMAL 1
+ #define ALPHA 425000
DigitalOut MOTOR1_OP(LED1);
DigitalOut MOTOR2_OP(LED2);
@@ -42,19 +45,19 @@
//l1 = longueur moteur gauche à l'effecteur
//l2 = longueur moteur droite à l'effecteur
int32_t lx, l1, l2;
-
+float pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur
void display_mode()
{
- if(mode == 0) // MODE DEGRADE
+ if(mode == DEGRAD) // MODE DEGRADE
{
MOD_DEG = 1;
MOD_NOR = 0;
}
- else if(mode == 1) // MODE NORMAL
+ else if(mode == NORMAL) // MODE NORMAL
{
MOD_DEG = 0;
MOD_NOR = 1;
@@ -154,16 +157,48 @@
void read_position(){
-
-
-
-
+ //timer_process_read++;//var used to set time for reading value
+ l1 = LENGTH_L1_PO - (p1/2545); //Length 1
+ l2 = LENGTH_L2_PO - (p2/2545); //Length 2
+ lx = LENGTH_MOTOR;
+ dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx);
+ dy = sqrt(((double) l1*(double) l1) - (dx*dx));
+
+ if(BPO == 0){
-
-
+ p_pom = p_actual;
+ p_pom2 = p_actual2;
+ mode = !mode; //Change mode
+ wait(1.0);
+ }
-
-
+ p1 = p_pom - p_actual;
+ p2 = p_pom2 - p_actual2;
+
+//Left motor
+ send(TPDO1_L, command, 'R', 0); //Ask position for second length
+ if(can2.read(msg)){
+ p_actual = msg.data[3];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[2];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[1];
+ p_actual = p_actual << 8;
+ p_actual = p_actual | msg.data[0];
+ }
+ //timer_process_read = 1;
+ wait(0.0005);
+ send(TPDO1_R, command, 'R', 0); //Ask position for first length
+ if(can2.read(msg)){
+ p_actual2 = msg.data[3];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[2];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[1];
+ p_actual2 = p_actual2 << 8;
+ p_actual2 = p_actual2 | msg.data[0];
+ }
+
}//fin de fonction
@@ -177,14 +212,18 @@
ctr_y = int((joystick_ctr.read()*100)-50);
if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;} // Detection x avec correction ctr de -42 à +42
if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr
-
+ float VX; // Calcule brut vélocité x, y
+ float VY;
+ long int VM_L; // Calcule des vélocités exactes de chaques moteurs
+ long int VM_R;
+
if(mode ==0)
{
///////////// MODE DEGRADE /////////////////////////////////////
- float VX = (float(x)/42)*425000; // Calcule brut vélocité x, y
- float VY = (float(y)/42)*425000;
- long int VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs
- long int VM_R = -VX - VY;
+ VX = (float(x)/42)*425000; // Calcule brut vélocité x, y
+ VY = (float(y)/42)*425000;
+ VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs
+ VM_R = -VX - VY;
command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian
command[1]= (VM_L & 0x0000FF00)>>8;
@@ -201,8 +240,28 @@
}
else if(mode ==1)
{
- // MODE NORMAL
+ ///////////// MODE NORMAL //////////////////////////////////////
+ VX = x;
+ VY = y;
+ VM_L = (ALPHA/42);
+ VM_L = (VM_L/l1)*((dx*VX)-(dy*VY));
+ VM_R = (ALPHA/42);
+ VM_R = (VM_R/l2)*((-(lx-dx)*VX)-(dy*VY));
+
+ command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian
+ command[1]= (VM_L & 0x0000FF00)>>8;
+ command[2]= (VM_L & 0x00FF0000)>>16;
+ command[3]= (VM_L & 0xFF000000)>>24;
+ send(RPDO1_L, command, 'D', 4); // Controle moteur gauche
+
+ command[0]= (VM_R & 0x000000FF);
+ command[1]= (VM_R & 0x0000FF00)>>8;
+ command[2]= (VM_R & 0x00FF0000)>>16;
+ command[3]= (VM_R & 0xFF000000)>>24;
+ send(RPDO1_R, command, 'D', 4); // Controle moteur droit
}
+ pVx = VM_R;
+ pVy = VM_L;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
@@ -217,93 +276,20 @@
- lx = LENGTH_MOTOR;
+
while(1)
{
- timer_process_read++;
- l1 = LENGTH_L1_PO - (p1/2545); //Length 1
- l2 = LENGTH_L2_PO - (p2/2545); //Length 2
- // dx = (pow((double) l1, 2) - pow((double) l2, 2) + pow((double) lx, 2))/(2*(double) lx);
- // dy = sqrt(pow((double) l1, 2) - pow(dx, 2));
- dx = (((double) l2*(double) l2) - ((double) l1*(double) l1) + ((double) lx*(double) lx))/(2*(double) lx);
- dy = sqrt(((double) l1*(double) l1) - (dx*dx));
-
- if(BPO == 0){
- timer_process_read = 0;
- p_pom = p_actual;
- p_pom2 = p_actual2;
- wait(1.0);
- }
-
-
-
-
-
-
-
-
-
-
-
- if(timer_process_read == 5){
- send(TPDO1_L, command, 'R', 0); //Ask position for second length
- if(can2.read(msg)){
- p_actual2 = msg.data[3];
- p_actual2 = p_actual2 << 8;
- p_actual2 = p_actual2 | msg.data[2];
- p_actual2 = p_actual2 << 8;
- p_actual2 = p_actual2 | msg.data[1];
- p_actual2 = p_actual2 << 8;
- p_actual2 = p_actual2 | msg.data[0];
- }
-
-
- }
-
- if(timer_process_read >= 10){
- send(TPDO1_R, command, 'R', 0); //Ask position for first length
- if(can2.read(msg)){
- p_actual = msg.data[3];
- p_actual = p_actual << 8;
- p_actual = p_actual | msg.data[2];
- p_actual = p_actual << 8;
- p_actual = p_actual | msg.data[1];
- p_actual = p_actual << 8;
- p_actual = p_actual | msg.data[0];
- }
- timer_process_read = 0;
- }
-
-
-
-
-
-
-
pc.printf("L1 = %d", l1);
pc.printf("\t L2 = %d", l2);
pc.printf("\t x = %f", dx);
- pc.printf("\t y = %f", dy);/*
- pc.printf("\t %d", p_actual);
- pc.printf("\t %d", p_actual2);
- pc.printf("\t %d", p_pom);
- pc.printf("\t %d\r\n", p_pom2);*/
- pc.printf("\t %d", p1);
- pc.printf("\t %d \r\n", p2);
-
-
-
- p1 = p_pom - p_actual;
- p2 = p_pom2 - p_actual2;
-
+ pc.printf("\t y = %f", dy);
+ pc.printf("\t VM_L = %f", pVx);
+ pc.printf("\t VM_R = %f \r\n", pVy);
-
-
-
- //read_position();
+ read_position();
control_Moteur(); // Controle du meteur mode degradé et normal
display_mode(); // Controle des led affichant le mode