![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
Diff: main.cpp
- Revision:
- 10:e4e1228604fb
- Parent:
- 9:0f63d4cb5613
--- a/main.cpp Fri May 18 09:03:25 2018 +0000 +++ b/main.cpp Wed May 23 12:33:04 2018 +0000 @@ -10,15 +10,14 @@ #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 MOTOR_L_COM(LED1); +DigitalOut MOTOR_R_COM(LED3); DigitalOut MOD_DEG(p25); DigitalOut MOD_NOR(p26); +DigitalOut CYCLE_TIME(p20); DigitalIn SWH(p5); DigitalIn SWV(p6); DigitalIn BPO(p7); @@ -28,7 +27,7 @@ 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; int x,y,ctr_x,ctr_y; // Variables de joystick @@ -67,17 +66,15 @@ 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 )) ; + { 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 )); + { 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); @@ -110,7 +107,7 @@ MOD_NOR = 0; wait(0.1); - command[0] = DRIVER_R1; + command[0] = DRIVER_R1; command[1] = DRIVER_R2; while(ping == 0) // INIT MOTEUR DROIT @@ -120,19 +117,19 @@ 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; + MOTOR_R_COM = 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; @@ -145,6 +142,7 @@ for(int i = 0; i < msg.len; i++){ pc.printf("0x%x ",msg.data[i]); ping = 1; + MOTOR_L_COM = 1; }//end of for pc.printf("\r \n"); }//can.read(msg) @@ -157,24 +155,6 @@ 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 @@ -186,9 +166,10 @@ p_actual = p_actual | msg.data[1]; p_actual = p_actual << 8; p_actual = p_actual | msg.data[0]; + MOTOR_L_COM = 1; } //timer_process_read = 1; - wait(0.005); + wait(0.01); send(TPDO1_R, command, 'R', 0); //Ask position for first length if(can2.read(msg)){ p_actual2 = msg.data[3]; @@ -198,8 +179,28 @@ p_actual2 = p_actual2 | msg.data[1]; p_actual2 = p_actual2 << 8; p_actual2 = p_actual2 | msg.data[0]; + MOTOR_R_COM = 1; } - + + + if(BPO == 0){ + p_pom = p_actual; //Refresh first origin value + p_pom2 = p_actual2; //Refresh second origin value + mode = !mode; //Change mode + wait(1.0); + } + + //Calculate positions values in counters + p1 = p_pom - p_actual; + p2 = p_pom2 - p_actual2; + + + //Calculate Coordinates + l1 = LENGTH_L1_PO - (p1/REFERENCE); //Length 1 + l2 = LENGTH_L2_PO - (p2/REFERENCE); //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)); }//fin de fonction @@ -215,7 +216,7 @@ 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_L; long int VM_R; if(mode ==0) @@ -244,9 +245,11 @@ ///////////// MODE NORMAL ////////////////////////////////////// VX = x; VY = y; + //Left control VM_L = ((dx*VX)-(dy*VY)); VM_L = (VM_L/l1); VM_L = VM_L*(ALPHA/42); + //Right control VM_R = ((-(lx-dx)*VX)-(dy*VY)); VM_R = (VM_R/l2); VM_R = VM_R*(ALPHA/42); @@ -276,9 +279,6 @@ BPO.mode(PullUp); initialisation(); // Mise en mode opérationnel des moteurs - - - while(1) { @@ -291,10 +291,14 @@ pc.printf("\t VM_L = %f", pVx); pc.printf("\t VM_R = %f \r\n", pVy); - + CYCLE_TIME = 0; + MOTOR_L_COM = 0; + MOTOR_R_COM = 0; read_position(); control_Moteur(); // Controle du meteur mode degradé et normal display_mode(); // Controle des led affichant le mode + + CYCLE_TIME = 1; }// fin while(1)