Enzo Niro
/
Robot_a_cables_v2_5
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Robot_a_cables_v2_0
by 
RaC2018
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "parameters.h" 00003 /* 00004 Projet BTS SN - Robot à câbles - Lycée Georges Cabanis 00005 Enzo Niro - Erwin Sazio 00006 Transmission par Bus CAN(protocol CANOpen) 00007 00008 */ 00009 00010 00011 00012 DigitalOut MOTOR1_OP(LED1); 00013 DigitalOut MOTOR2_OP(LED2); 00014 DigitalOut DEB_MOD_DEG(LED3); 00015 DigitalOut DEB_MOD_NOR(LED4); 00016 DigitalOut MOD_DEG(p25); 00017 DigitalOut MOD_NOR(p26); 00018 DigitalIn SWH(p5); 00019 DigitalIn SWV(p6); 00020 DigitalIn BPO(p7); 00021 AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference 00022 AnalogIn joystick_x(p16); // Pin 4 (fil jaune) 00023 AnalogIn joystick_y(p17); // Pin 5 (fil bleu) 00024 00025 CAN can2(p30, p29, 1000000); // on definit pin et debit 00026 CANMessage msg; 00027 Serial pc(USBTX, USBRX); 00028 00029 uint8_t mode = 0; 00030 int x,y,ctr_x,ctr_y; // Variables de joystick 00031 uint8_t ping = 0; //variable pour etat de processus 00032 uint8_t done = 0; 00033 char command[8]; //word to send command 00034 uint16_t timer_process_read; //counter to make mbed read actual position 00035 double dy = 1262, dx = 492.5;//coordinates values when origin is done 00036 //these positions are in counters 00037 int32_t p1, p2; //motors positions 00038 int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made 00039 00040 //longueur câbles 00041 //lx = longueur entre les point des moteurs 00042 //l1 = longueur moteur gauche à l'effecteur 00043 //l2 = longueur moteur droite à l'effecteur 00044 int32_t lx, l1, l2; 00045 00046 00047 00048 00049 00050 void display_mode() 00051 { 00052 if(mode == 0) // MODE DEGRADE 00053 { 00054 MOD_DEG = 1; 00055 MOD_NOR = 0; 00056 } 00057 else if(mode == 1) // MODE NORMAL 00058 { 00059 MOD_DEG = 0; 00060 MOD_NOR = 1; 00061 } 00062 } 00063 00064 void send(int id, char octet_emis[], char RouD, char longueur ) 00065 { 00066 int emis_ok = 0 ; 00067 00068 MOTOR1_OP = 1; 00069 MOTOR1_OP = 0; 00070 00071 if (RouD == 'D') 00072 { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ; 00073 //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] ); 00074 }// c'ets la valeur retournée par la fonction write 00075 else 00076 { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard )); 00077 //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] ); 00078 } 00079 //lcd.locate(0,10); 00080 //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] ); 00081 /*if(emis_ok) { 00082 // ici octet emis n'a pas de sens car trame remote ! 00083 //pc.printf("send \r \n"); 00084 } */ 00085 } 00086 00087 void initialisation() 00088 { 00089 pc.printf("starting...\r\n"); 00090 MOD_DEG = 1; 00091 MOD_NOR = 0; 00092 wait(0.1); 00093 MOD_DEG = 0; 00094 MOD_NOR = 1; 00095 wait(0.1); 00096 MOD_DEG = 1; 00097 MOD_NOR = 0; 00098 wait(0.1); 00099 MOD_DEG = 0; 00100 MOD_NOR = 1; 00101 wait(0.1); 00102 MOD_DEG = 1; 00103 MOD_NOR = 0; 00104 wait(0.1); 00105 MOD_DEG = 0; 00106 MOD_NOR = 0; 00107 wait(0.1); 00108 00109 command[0] = DRIVER_R1; 00110 command[1] = DRIVER_R2; 00111 00112 while(ping == 0) // INIT MOTEUR DROIT 00113 { 00114 pc.printf("ping droite ...\r\n"); 00115 send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode 00116 MOD_NOR = 1; 00117 if(can2.read(msg)) 00118 { 00119 for(int i = 0; i < msg.len; i++) 00120 { 00121 pc.printf("0x%x ",msg.data[i]); 00122 ping = 1; 00123 }//end of for 00124 pc.printf("\r \n"); 00125 }//can.read(msg) 00126 wait(0.1); 00127 MOD_NOR = 0; 00128 wait(0.9); 00129 } 00130 00131 ping =0; 00132 command[0] = DRIVER_L1; 00133 command[1] = DRIVER_L2; 00134 00135 while(ping == 0) // INIT MOTEUR GAUCHE 00136 { 00137 pc.printf("ping gauche ...\r\n"); 00138 send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode 00139 MOD_DEG = 1; 00140 if(can2.read(msg)) { 00141 for(int i = 0; i < msg.len; i++){ 00142 pc.printf("0x%x ",msg.data[i]); 00143 ping = 1; 00144 }//end of for 00145 pc.printf("\r \n"); 00146 }//can.read(msg) 00147 wait(0.1); 00148 MOD_DEG = 0; 00149 wait(0.9); 00150 } 00151 } // fin initialisation 00152 00153 00154 00155 void read_position(){ 00156 00157 00158 00159 00160 00161 00162 00163 00164 00165 00166 00167 00168 }//fin de fonction 00169 00170 00171 00172 void control_Moteur() 00173 { 00174 ///////////// CONTROLE MOTEUR ////////////////////////////////////////////////////////////////////////////// 00175 // LECTURE Joystick + stockage valeurs dans x, y 00176 ctr_x = int(-((joystick_ctr.read()*100)-50)); 00177 ctr_y = int((joystick_ctr.read()*100)-50); 00178 if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;} // Detection x avec correction ctr de -42 à +42 00179 if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr 00180 00181 if(mode ==0) 00182 { 00183 ///////////// MODE DEGRADE ///////////////////////////////////// 00184 float VX = (float(x)/42)*425000; // Calcule brut vélocité x, y 00185 float VY = (float(y)/42)*425000; 00186 long int VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs 00187 long int VM_R = -VX - VY; 00188 00189 command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian 00190 command[1]= (VM_L & 0x0000FF00)>>8; 00191 command[2]= (VM_L & 0x00FF0000)>>16; 00192 command[3]= (VM_L & 0xFF000000)>>24; 00193 send(RPDO1_L, command, 'D', 4); // Controle moteur gauche 00194 00195 command[0]= (VM_R & 0x000000FF); 00196 command[1]= (VM_R & 0x0000FF00)>>8; 00197 command[2]= (VM_R & 0x00FF0000)>>16; 00198 command[3]= (VM_R & 0xFF000000)>>24; 00199 send(RPDO1_R, command, 'D', 4); // Controle moteur droit 00200 //////////////////////////////////////////////////////////////// 00201 } 00202 else if(mode ==1) 00203 { 00204 // MODE NORMAL 00205 } 00206 ////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00207 } 00208 00209 int main() { 00210 00211 //define Pullup switch 00212 SWH.mode(PullUp); 00213 SWV.mode(PullUp); 00214 BPO.mode(PullUp); 00215 00216 initialisation(); // Mise en mode opérationnel des moteurs 00217 00218 00219 00220 lx = LENGTH_MOTOR; 00221 while(1) 00222 { 00223 00224 timer_process_read++; 00225 l1 = LENGTH_L1_PO - (p1/2545); //Length 1 00226 l2 = LENGTH_L2_PO - (p2/2545); //Length 2 00227 // dx = (pow((double) l1, 2) - pow((double) l2, 2) + pow((double) lx, 2))/(2*(double) lx); 00228 // dy = sqrt(pow((double) l1, 2) - pow(dx, 2)); 00229 dx = (((double) l2*(double) l2) - ((double) l1*(double) l1) + ((double) lx*(double) lx))/(2*(double) lx); 00230 dy = sqrt(((double) l1*(double) l1) - (dx*dx)); 00231 00232 if(BPO == 0){ 00233 timer_process_read = 0; 00234 p_pom = p_actual; 00235 p_pom2 = p_actual2; 00236 wait(1.0); 00237 } 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 if(timer_process_read == 5){ 00250 send(TPDO1_L, command, 'R', 0); //Ask position for second length 00251 if(can2.read(msg)){ 00252 p_actual2 = msg.data[3]; 00253 p_actual2 = p_actual2 << 8; 00254 p_actual2 = p_actual2 | msg.data[2]; 00255 p_actual2 = p_actual2 << 8; 00256 p_actual2 = p_actual2 | msg.data[1]; 00257 p_actual2 = p_actual2 << 8; 00258 p_actual2 = p_actual2 | msg.data[0]; 00259 } 00260 00261 00262 } 00263 00264 if(timer_process_read >= 10){ 00265 send(TPDO1_R, command, 'R', 0); //Ask position for first length 00266 if(can2.read(msg)){ 00267 p_actual = msg.data[3]; 00268 p_actual = p_actual << 8; 00269 p_actual = p_actual | msg.data[2]; 00270 p_actual = p_actual << 8; 00271 p_actual = p_actual | msg.data[1]; 00272 p_actual = p_actual << 8; 00273 p_actual = p_actual | msg.data[0]; 00274 } 00275 timer_process_read = 0; 00276 } 00277 00278 00279 00280 00281 00282 00283 00284 00285 pc.printf("L1 = %d", l1); 00286 pc.printf("\t L2 = %d", l2); 00287 pc.printf("\t x = %f", dx); 00288 pc.printf("\t y = %f", dy);/* 00289 pc.printf("\t %d", p_actual); 00290 pc.printf("\t %d", p_actual2); 00291 pc.printf("\t %d", p_pom); 00292 pc.printf("\t %d\r\n", p_pom2);*/ 00293 pc.printf("\t %d", p1); 00294 pc.printf("\t %d \r\n", p2); 00295 00296 00297 00298 p1 = p_pom - p_actual; 00299 p2 = p_pom2 - p_actual2; 00300 00301 00302 00303 00304 00305 00306 //read_position(); 00307 control_Moteur(); // Controle du meteur mode degradé et normal 00308 display_mode(); // Controle des led affichant le mode 00309 00310 }// fin while(1) 00311 00312 } // fin main 00313 00314 00315 00316 00317
Generated on Sat Aug 20 2022 17:22:44 by
1.7.2