version 3.0
Dependencies: mbed
Fork of Robot_a_cables_v2_6_5 by
main.cpp
- Committer:
- protongamer
- Date:
- 2018-05-23
- Revision:
- 10:e4e1228604fb
- Parent:
- 9:0f63d4cb5613
File content as of revision 10:e4e1228604fb:
#include "mbed.h" #include "parameters.h" /* Projet BTS SN - Robot à câbles - Lycée Georges Cabanis Enzo Niro - Erwin Sazio Transmission par Bus CAN(protocol CANOpen) */ #define DEGRAD 0 #define NORMAL 1 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); 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, 115200); 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 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 int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made //longueur câbles //lx = longueur entre les point des moteurs //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 == DEGRAD) // MODE DEGRADE { MOD_DEG = 1; MOD_NOR = 0; } else if(mode == NORMAL) // MODE NORMAL { MOD_DEG = 0; MOD_NOR = 1; } } void send(int id, char octet_emis[], char RouD, char longueur ) { if (RouD == 'D') { 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 { 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() { 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; 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; 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; if(can2.read(msg)) { 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) wait(0.1); MOD_DEG = 0; wait(0.9); } } // fin initialisation void read_position(){ //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]; MOTOR_L_COM = 1; } //timer_process_read = 1; wait(0.01); 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]; 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 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; 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; //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); 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() { //define Pullup switch 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); 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) } // fin main