![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
added serial up, down, left and right control
Dependencies: mbed
Fork of Robot_a_cables_v3_6 by
Diff: main.cpp
- Revision:
- 10:fb3542f3c5e6
- Parent:
- 9:0f63d4cb5613
- Child:
- 11:0fa23c2dab76
diff -r 0f63d4cb5613 -r fb3542f3c5e6 main.cpp --- a/main.cpp Fri May 18 09:03:25 2018 +0000 +++ b/main.cpp Mon May 28 14:18:32 2018 +0000 @@ -1,5 +1,6 @@ #include "mbed.h" #include "parameters.h" +#include "rac.h" /* Projet BTS SN - Robot à câbles - Lycée Georges Cabanis Enzo Niro - Erwin Sazio @@ -10,15 +11,13 @@ #define DEGRAD 0 #define NORMAL 1 - //#define ALPHA 425000 - #define ALPHA 425000 + -DigitalOut MOTOR1_OP(LED1); -DigitalOut MOTOR2_OP(LED2); -DigitalOut DEB_MOD_DEG(LED3); -DigitalOut DEB_MOD_NOR(LED4); + + DigitalOut MOD_DEG(p25); DigitalOut MOD_NOR(p26); +DigitalOut CYCLE_TIME(p20); DigitalIn SWH(p5); DigitalIn SWV(p6); DigitalIn BPO(p7); @@ -26,16 +25,16 @@ AnalogIn joystick_x(p16); // Pin 4 (fil jaune) AnalogIn joystick_y(p17); // Pin 5 (fil bleu) +RAC robot; CAN can2(p30, p29, 1000000); // on definit pin et debit CANMessage msg; -Serial pc(USBTX, USBRX); +Serial pc(USBTX, USBRX, 115200); -uint8_t mode = 0; +bool mode = 0; // 0 = Degraded 1 = Normal 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 -uint16_t timer_process_read; //counter to make mbed read actual position double dy = 1262, dx = 492.5;//coordinates values when origin is done //these positions are in counters int32_t p1, p2; //motors positions @@ -46,7 +45,7 @@ //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 +double pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur @@ -65,28 +64,7 @@ } } -void send(int id, char octet_emis[], char RouD, char longueur ) -{ - int emis_ok = 0 ; - - MOTOR1_OP = 1; - MOTOR1_OP = 0; - - if (RouD == 'D') - { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ; - //pc.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); - }// c'ets la valeur retournée par la fonction write - else - { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard )); - //pc.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); - } - //lcd.locate(0,10); - //lcd.printf("id: %x, %c L = %d, \nData : %x:%x:%x:%x ... \n", id,RouD,longueur,octet_emis[0],octet_emis[1], octet_emis[2], octet_emis[3] ); - /*if(emis_ok) { - // ici octet emis n'a pas de sens car trame remote ! - //pc.printf("send \r \n"); - } */ -} + void initialisation() { @@ -110,36 +88,35 @@ MOD_NOR = 0; wait(0.1); - command[0] = DRIVER_R1; + 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 + robot.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); - } + 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; + 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 + robot.send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode MOD_DEG = 1; if(can2.read(msg)) { for(int i = 0; i < msg.len; i++){ @@ -155,118 +132,6 @@ } // fin initialisation - -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.005); - 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 - - - -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 - 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 ///////////////////////////////////// - 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; - 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 ////////////////////////////////////// - VX = x; - VY = y; - VM_L = ((dx*VX)-(dy*VY)); - VM_L = (VM_L/l1); - VM_L = VM_L*(ALPHA/42); - VM_R = ((-(lx-dx)*VX)-(dy*VY)); - VM_R = (VM_R/l2); - VM_R = VM_R*(ALPHA/42); - - 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; - ////////////////////////////////////////////////////////////////////////////////////////////////////////////// -} int main() { @@ -274,27 +139,37 @@ SWH.mode(PullUp); SWV.mode(PullUp); BPO.mode(PullUp); - + initialisation(); // Mise en mode opérationnel des moteurs - - - while(1) { - + /* 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 VM_L = %f", pVx); - pc.printf("\t VM_R = %f \r\n", pVy); - + pc.printf("\t L2 = %d", l2);*/ + if(mode == DEGRAD){ + pc.printf("mode = D"); + } + else if(mode == NORMAL){ + pc.printf("mode = N"); + } + pc.printf(" x = %f", dx); + pc.printf(" y = %f", dy); + pc.printf(" VM_L = %f", pVx); + pc.printf(" VM_R = %f \r\n", pVy); - read_position(); - control_Moteur(); // Controle du meteur mode degradé et normal + + + robot.readButtons(SWH, SWV, BPO); //read state buttons, important for others functions + + CYCLE_TIME = 0; + robot.readPosition(&dx, &dy, &mode); //read motors encoders and calculate coordinates position (x,y) + robot.setVelocity(joystick_x.read(),joystick_y.read(),joystick_ctr.read(), mode); //Motors control with the joystick and with the mode we use + robot.readVelocity(&pVx, &pVy);//read velocities applied on the robot display_mode(); // Controle des led affichant le mode + + CYCLE_TIME = 1; }// fin while(1)