RaC2018
Include new libraries normal mode updated(offset reduced)
Dependencies: mbed
Fork of
Robot_a_cables_v2_6_5
by 
RaC2018
Diff: main.cpp
- Revision:
- 5:d5a021bbe81b
- Parent:
- 4:3fd7c53d31c1
- Child:
- 6:ffffa9dfadfc
--- a/main.cpp Mon Apr 23 08:44:10 2018 +0000
+++ b/main.cpp Mon Apr 30 12:27:56 2018 +0000
@@ -9,9 +9,6 @@
-
-
-
DigitalOut MOTOR1_OP(LED1);
DigitalOut MOTOR2_OP(LED2);
DigitalOut DEB_MOD_DEG(LED3);
@@ -21,14 +18,38 @@
DigitalIn SWH(p5);
DigitalIn SWV(p6);
DigitalIn BPO(p7);
+AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference
+AnalogIn joystick_x(p16); // Pin 4 (fil jaune)
+AnalogIn joystick_y(p17); // Pin 5 (fil bleu)
-
+CAN can2(p30, p29, 1000000); // on definit pin et debit
+CANMessage msg;
+Serial pc(USBTX, USBRX);
-CAN can1(p9, p10,1000000);// on definit pin et debit
-CAN can2(p30, p29, 1000000);
-Serial pc(USBTX, USBRX);
-//AnalogIn pot_1(p19); // potard pour selection
+uint8_t mode = 0;
+int x,y,ctr_x,ctr_y; // Variables de joystick
+uint8_t ping = 0; //variable pour etat de processus
+uint8_t done = 0;
+char command[8]; //word to send command
+uint64_t timer_process_read; //counter to make mbed read actual position
+uint64_t timer_process_ping; //counter to make mbed read actual position
+float dy = 525000;//valeur à la POM
+int64_t d_old, d_new;
+bool first_time = 0;
+void display_mode()
+{
+ if(mode == 0) // MODE DEGRADE
+ {
+ MOD_DEG = 1;
+ MOD_NOR = 0;
+ }
+ else if(mode == 1) // MODE NORMAL
+ {
+ MOD_DEG = 0;
+ MOD_NOR = 1;
+ }
+}
void send(int id, char octet_emis[], char RouD, char longueur )
{
@@ -53,49 +74,59 @@
} */
}
-
-
-
-uint64_t timer_process_read; //counter to make mbed read actual position
-uint64_t timer_process_ping; //counter to make mbed read actual position
-float dy = 525000;//valeur à la POM
-int64_t d_old, d_new;
-bool first_time = 0;
-
-int main() {
-
-
- CANMessage msg;
- uint8_t ping = 0; //variable pour etat de processus
- uint8_t done = 0;
- char command[8]; //word to send command
-
-
- //define Pullup switch
- SWH.mode(PullUp);
- SWV.mode(PullUp);
- BPO.mode(PullUp);
- pc.printf("starting...\r\n");
+void initialisation()
+{
+ pc.printf("starting...\r\n");
+ MOD_DEG = 1;
+ MOD_NOR = 0;
+ wait(0.1);
+ MOD_DEG = 0;
+ MOD_NOR = 1;
+ wait(0.1);
+ MOD_DEG = 1;
+ MOD_NOR = 0;
+ wait(0.1);
+ MOD_DEG = 0;
+ MOD_NOR = 1;
+ wait(0.1);
+ MOD_DEG = 1;
+ MOD_NOR = 0;
+ wait(0.1);
+ MOD_DEG = 0;
+ MOD_NOR = 0;
+ wait(0.1);
+
+ command[0] = DRIVER_R1;
+ command[1] = DRIVER_R2;
+
+ while(ping == 0) // INIT MOTEUR DROIT
+ {
+ pc.printf("ping droite ...\r\n");
+ send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
+ MOD_NOR = 1;
+ if(can2.read(msg))
+ {
+ for(int i = 0; i < msg.len; i++)
+ {
+ pc.printf("0x%x ",msg.data[i]);
+ ping = 1;
+ }//end of for
+ pc.printf("\r \n");
+ }//can.read(msg)
+ wait(0.1);
+ MOD_NOR = 0;
+ wait(0.9);
+ }
+
+ ping =0;
+ command[0] = DRIVER_L1;
+ command[1] = DRIVER_L2;
+
+ while(ping == 0) // INIT MOTEUR GAUCHE
+ {
+ pc.printf("ping gauche ...\r\n");
+ send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
MOD_DEG = 1;
- wait(0.1);
- MOD_DEG = 0;
- wait(0.1);
- MOD_DEG = 1;
- wait(0.1);
- MOD_DEG = 0;
- wait(0.1);
- MOD_DEG = 1;
- wait(0.1);
- MOD_DEG = 0;
- wait(0.1);
-
- command[0] = DRIVER_R1;
- command[1] = DRIVER_R2;
-
- while(ping == 0){
- pc.printf("ping...\r\n");
- send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
- MOD_NOR = 1;
if(can2.read(msg)) {
for(int i = 0; i < msg.len; i++){
pc.printf("0x%x ",msg.data[i]);
@@ -104,41 +135,87 @@
pc.printf("\r \n");
}//can.read(msg)
wait(0.1);
- MOD_NOR = 0;
+ MOD_DEG = 0;
wait(0.9);
+ }
+} // fin initialisation
+
+void control_Moteur()
+{
+ ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
+ // LECTURE Joystick + stockage valeurs dans x, y
+ ctr_x = int(-((joystick_ctr.read()*100)-50));
+ 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
+
+ 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;
+
+ 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
+ ////////////////////////////////////////////////////////////////
+ }
+ else if(mode ==1)
+ {
+ // MODE NORMAL
+ }
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////
+}
+
+int main() {
+
+ //define Pullup switch
+ SWH.mode(PullUp);
+ SWV.mode(PullUp);
+ BPO.mode(PullUp);
+
+ initialisation(); // Mise en mode opérationnel des moteurs
+
+ for(int working = 0; working < 50; working++)
+ {
+ send(TPDO1_R, command, 'R', 0); //Ask position
+ timer_process_read = 0;
+ d_new = msg.data[3];
+ d_new = d_new << 8;
+ d_new = d_new | msg.data[2];
+ d_new = d_new << 8;
+ d_new = d_new | msg.data[1];
+ d_new = d_new << 8;
+ d_new = d_new | msg.data[0];
+ pc.printf("%d \r\n",d_new);
+ }
+ d_old = d_new;
+
+ while(1)
+ {
+ //timer_process_ping++;
+ timer_process_read++;
+ pc.printf("%d \t",d_new);
+ pc.printf("%d \t",d_old);
+ //pc.printf("%d \t",d_new);
+ pc.printf("%f \r\n",dy);
- }
- MOD_NOR = 1;
- for(int working = 0; working < 50; working++){
- send(TPDO1_R, command, 'R', 0); //Ask position
- timer_process_read = 0;
- d_new = msg.data[3];
- d_new = d_new << 8;
- d_new = d_new | msg.data[2];
- d_new = d_new << 8;
- d_new = d_new | msg.data[1];
- d_new = d_new << 8;
- d_new = d_new | msg.data[0];
- pc.printf("%d \r\n",d_new);}
- d_old = d_new;
-
-
-
- while(1){
-
- //timer_process_ping++;
- timer_process_read++;
- pc.printf("%d \t",d_new);
- pc.printf("%d \t",d_old);
- //pc.printf("%d \t",d_new);
- pc.printf("%f \r\n",dy);
-
- dy = 525000 + (d_new*1000)/2500;
+ dy = 525000 + (d_new*1000)/2500;
- //wait_ms(1);
+ //wait_ms(1);
- /* if(timer_process_ping >= 5000){
+ /* if(timer_process_ping >= 5000){
send(TPDO2_R, command, 'R', 0); //Ask mod of motor
timer_process_ping = 0;
if(can2.read(msg)) {
@@ -195,52 +272,10 @@
}//end of for
pc.printf("\r \n");
}//can.read(msg)
-
- switch(BPO){
- case 0:
- if(done == 0){
- timer_process_read = 50;
- //timer_process_ping = 0;
- switch(SWV){
- case 0:
- command[0] = 0x00;
- command[1] = 0x8F;
- command[2] = 0x00;
- command[3] = 0x00;
- send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
- break;
-
- case 1:
- command[0] = 0xFF;
- command[1] = 0x70;
- command[2] = 0xFF;
- command[3] = 0xFF;
- send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
-
-
- break;
- }//end of switch SWV
- done = 1;
- }
-
- break;
-
- case 1:
- if(done == 1){
- command[0] = 0x00;
- command[1] = 0x00;
- command[2] = 0x00;
- command[3] = 0x00;
- send(RPDO1_R, command, 'D', 4); //send word to put pluto driver in operational mode
- done = 0;
- //send(TPDO1_R, command, 'R', 0); //send word to put pluto driver in operational mode
- }
- break;
-
- }
+ control_Moteur(); // Controle du meteur mode degradé et normal
+ display_mode(); // Controle des led affichant le mode
-
}// fin while(1)
} // fin main