![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version 2.5(pour le grand FEARHEAVEN) [spoil] oui [/spoil]
Dependencies: mbed
Fork of Robot_a_cables_v2_5 by
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