Enzo Niro / Mbed 2 deprecated Robot_a_cables_v2_5

version 2.5(pour le grand FEARHEAVEN) [spoil] oui [/spoil]

Dependencies:   mbed

Fork of Robot_a_cables_v2_0 by RaC2018

main.cpp@5:d5a021bbe81b, 2018-04-30 (annotated)

Committer:
Ringslev
Date:
Mon Apr 30 12:27:56 2018 +0000
Revision:
5:d5a021bbe81b
Parent:
4:3fd7c53d31c1
Child:
6:ffffa9dfadfc
Mode d?grad? fonctionnel; Mode normal en cours

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LouisReynier 0:1ab5fdb4fa56 1 #include "mbed.h"
protongamer 3:86e21a36eecb 2 #include "can_parameters.h"
protongamer 3:86e21a36eecb 3 /*
protongamer 3:86e21a36eecb 4 Projet BTS SN - Robot à câbles - Lycée Georges Cabanis
protongamer 3:86e21a36eecb 5 Enzo Niro - Erwin Sazio
protongamer 3:86e21a36eecb 6 Transmission par Bus CAN(protocol CANOpen)
protongamer 3:86e21a36eecb 7
protongamer 3:86e21a36eecb 8 */
LouisReynier 0:1ab5fdb4fa56 9
protongamer 1:4d70e593345f 10
protongamer 3:86e21a36eecb 11
protongamer 3:86e21a36eecb 12 DigitalOut MOTOR1_OP(LED1);
protongamer 3:86e21a36eecb 13 DigitalOut MOTOR2_OP(LED2);
protongamer 3:86e21a36eecb 14 DigitalOut DEB_MOD_DEG(LED3);
protongamer 3:86e21a36eecb 15 DigitalOut DEB_MOD_NOR(LED4);
protongamer 3:86e21a36eecb 16 DigitalOut MOD_DEG(p26);
protongamer 3:86e21a36eecb 17 DigitalOut MOD_NOR(p25);
protongamer 3:86e21a36eecb 18 DigitalIn SWH(p5);
protongamer 3:86e21a36eecb 19 DigitalIn SWV(p6);
protongamer 3:86e21a36eecb 20 DigitalIn BPO(p7);
Ringslev 5:d5a021bbe81b 21 AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference
Ringslev 5:d5a021bbe81b 22 AnalogIn joystick_x(p16); // Pin 4 (fil jaune)
Ringslev 5:d5a021bbe81b 23 AnalogIn joystick_y(p17); // Pin 5 (fil bleu)
protongamer 3:86e21a36eecb 24
Ringslev 5:d5a021bbe81b 25 CAN can2(p30, p29, 1000000); // on definit pin et debit
Ringslev 5:d5a021bbe81b 26 CANMessage msg;
Ringslev 5:d5a021bbe81b 27 Serial pc(USBTX, USBRX);
protongamer 4:3fd7c53d31c1 28
Ringslev 5:d5a021bbe81b 29 uint8_t mode = 0;
Ringslev 5:d5a021bbe81b 30 int x,y,ctr_x,ctr_y; // Variables de joystick
Ringslev 5:d5a021bbe81b 31 uint8_t ping = 0; //variable pour etat de processus
Ringslev 5:d5a021bbe81b 32 uint8_t done = 0;
Ringslev 5:d5a021bbe81b 33 char command[8]; //word to send command
Ringslev 5:d5a021bbe81b 34 uint64_t timer_process_read; //counter to make mbed read actual position
Ringslev 5:d5a021bbe81b 35 uint64_t timer_process_ping; //counter to make mbed read actual position
Ringslev 5:d5a021bbe81b 36 float dy = 525000;//valeur à la POM
Ringslev 5:d5a021bbe81b 37 int64_t d_old, d_new;
Ringslev 5:d5a021bbe81b 38 bool first_time = 0;
protongamer 1:4d70e593345f 39
Ringslev 5:d5a021bbe81b 40 void display_mode()
Ringslev 5:d5a021bbe81b 41 {
Ringslev 5:d5a021bbe81b 42 if(mode == 0) // MODE DEGRADE
Ringslev 5:d5a021bbe81b 43 {
Ringslev 5:d5a021bbe81b 44 MOD_DEG = 1;
Ringslev 5:d5a021bbe81b 45 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 46 }
Ringslev 5:d5a021bbe81b 47 else if(mode == 1) // MODE NORMAL
Ringslev 5:d5a021bbe81b 48 {
Ringslev 5:d5a021bbe81b 49 MOD_DEG = 0;
Ringslev 5:d5a021bbe81b 50 MOD_NOR = 1;
Ringslev 5:d5a021bbe81b 51 }
Ringslev 5:d5a021bbe81b 52 }
protongamer 1:4d70e593345f 53
protongamer 1:4d70e593345f 54 void send(int id, char octet_emis[], char RouD, char longueur )
protongamer 1:4d70e593345f 55 {
protongamer 1:4d70e593345f 56 int emis_ok = 0 ;
LouisReynier 0:1ab5fdb4fa56 57
protongamer 4:3fd7c53d31c1 58 MOTOR1_OP = 1;
protongamer 4:3fd7c53d31c1 59 MOTOR1_OP = 0;
protongamer 4:3fd7c53d31c1 60
protongamer 1:4d70e593345f 61 if (RouD == 'D')
protongamer 1:4d70e593345f 62 { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ;
protongamer 3:86e21a36eecb 63 //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] );
protongamer 1:4d70e593345f 64 }// c'ets la valeur retournée par la fonction write
protongamer 1:4d70e593345f 65 else
protongamer 1:4d70e593345f 66 { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard ));
protongamer 3:86e21a36eecb 67 //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] );
protongamer 1:4d70e593345f 68 }
protongamer 1:4d70e593345f 69 //lcd.locate(0,10);
protongamer 1:4d70e593345f 70 //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] );
protongamer 3:86e21a36eecb 71 /*if(emis_ok) {
protongamer 1:4d70e593345f 72 // ici octet emis n'a pas de sens car trame remote !
protongamer 3:86e21a36eecb 73 //pc.printf("send \r \n");
protongamer 3:86e21a36eecb 74 } */
LouisReynier 0:1ab5fdb4fa56 75 }
protongamer 1:4d70e593345f 76
Ringslev 5:d5a021bbe81b 77 void initialisation()
Ringslev 5:d5a021bbe81b 78 {
Ringslev 5:d5a021bbe81b 79 pc.printf("starting...\r\n");
Ringslev 5:d5a021bbe81b 80 MOD_DEG = 1;
Ringslev 5:d5a021bbe81b 81 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 82 wait(0.1);
Ringslev 5:d5a021bbe81b 83 MOD_DEG = 0;
Ringslev 5:d5a021bbe81b 84 MOD_NOR = 1;
Ringslev 5:d5a021bbe81b 85 wait(0.1);
Ringslev 5:d5a021bbe81b 86 MOD_DEG = 1;
Ringslev 5:d5a021bbe81b 87 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 88 wait(0.1);
Ringslev 5:d5a021bbe81b 89 MOD_DEG = 0;
Ringslev 5:d5a021bbe81b 90 MOD_NOR = 1;
Ringslev 5:d5a021bbe81b 91 wait(0.1);
Ringslev 5:d5a021bbe81b 92 MOD_DEG = 1;
Ringslev 5:d5a021bbe81b 93 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 94 wait(0.1);
Ringslev 5:d5a021bbe81b 95 MOD_DEG = 0;
Ringslev 5:d5a021bbe81b 96 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 97 wait(0.1);
Ringslev 5:d5a021bbe81b 98
Ringslev 5:d5a021bbe81b 99 command[0] = DRIVER_R1;
Ringslev 5:d5a021bbe81b 100 command[1] = DRIVER_R2;
Ringslev 5:d5a021bbe81b 101
Ringslev 5:d5a021bbe81b 102 while(ping == 0) // INIT MOTEUR DROIT
Ringslev 5:d5a021bbe81b 103 {
Ringslev 5:d5a021bbe81b 104 pc.printf("ping droite ...\r\n");
Ringslev 5:d5a021bbe81b 105 send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
Ringslev 5:d5a021bbe81b 106 MOD_NOR = 1;
Ringslev 5:d5a021bbe81b 107 if(can2.read(msg))
Ringslev 5:d5a021bbe81b 108 {
Ringslev 5:d5a021bbe81b 109 for(int i = 0; i < msg.len; i++)
Ringslev 5:d5a021bbe81b 110 {
Ringslev 5:d5a021bbe81b 111 pc.printf("0x%x ",msg.data[i]);
Ringslev 5:d5a021bbe81b 112 ping = 1;
Ringslev 5:d5a021bbe81b 113 }//end of for
Ringslev 5:d5a021bbe81b 114 pc.printf("\r \n");
Ringslev 5:d5a021bbe81b 115 }//can.read(msg)
Ringslev 5:d5a021bbe81b 116 wait(0.1);
Ringslev 5:d5a021bbe81b 117 MOD_NOR = 0;
Ringslev 5:d5a021bbe81b 118 wait(0.9);
Ringslev 5:d5a021bbe81b 119 }
Ringslev 5:d5a021bbe81b 120
Ringslev 5:d5a021bbe81b 121 ping =0;
Ringslev 5:d5a021bbe81b 122 command[0] = DRIVER_L1;
Ringslev 5:d5a021bbe81b 123 command[1] = DRIVER_L2;
Ringslev 5:d5a021bbe81b 124
Ringslev 5:d5a021bbe81b 125 while(ping == 0) // INIT MOTEUR GAUCHE
Ringslev 5:d5a021bbe81b 126 {
Ringslev 5:d5a021bbe81b 127 pc.printf("ping gauche ...\r\n");
Ringslev 5:d5a021bbe81b 128 send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
protongamer 3:86e21a36eecb 129 MOD_DEG = 1;
protongamer 3:86e21a36eecb 130 if(can2.read(msg)) {
protongamer 4:3fd7c53d31c1 131 for(int i = 0; i < msg.len; i++){
protongamer 4:3fd7c53d31c1 132 pc.printf("0x%x ",msg.data[i]);
protongamer 4:3fd7c53d31c1 133 ping = 1;
protongamer 4:3fd7c53d31c1 134 }//end of for
protongamer 4:3fd7c53d31c1 135 pc.printf("\r \n");
protongamer 4:3fd7c53d31c1 136 }//can.read(msg)
protongamer 3:86e21a36eecb 137 wait(0.1);
Ringslev 5:d5a021bbe81b 138 MOD_DEG = 0;
protongamer 3:86e21a36eecb 139 wait(0.9);
Ringslev 5:d5a021bbe81b 140 }
Ringslev 5:d5a021bbe81b 141 } // fin initialisation
Ringslev 5:d5a021bbe81b 142
Ringslev 5:d5a021bbe81b 143 void control_Moteur()
Ringslev 5:d5a021bbe81b 144 {
Ringslev 5:d5a021bbe81b 145 ///////////// CONTROLE MOTEUR //////////////////////////////////////////////////////////////////////////////
Ringslev 5:d5a021bbe81b 146 // LECTURE Joystick + stockage valeurs dans x, y
Ringslev 5:d5a021bbe81b 147 ctr_x = int(-((joystick_ctr.read()*100)-50));
Ringslev 5:d5a021bbe81b 148 ctr_y = int((joystick_ctr.read()*100)-50);
Ringslev 5:d5a021bbe81b 149 if(SWH){x = int(-((joystick_x.read()*100)-50))+ctr_x;}else{x=0;} // Detection x avec correction ctr de -42 à +42
Ringslev 5:d5a021bbe81b 150 if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr
Ringslev 5:d5a021bbe81b 151
Ringslev 5:d5a021bbe81b 152 if(mode ==0)
Ringslev 5:d5a021bbe81b 153 {
Ringslev 5:d5a021bbe81b 154 ///////////// MODE DEGRADE /////////////////////////////////////
Ringslev 5:d5a021bbe81b 155 float VX = (float(x)/42)*425000; // Calcule brut vélocité x, y
Ringslev 5:d5a021bbe81b 156 float VY = (float(y)/42)*425000;
Ringslev 5:d5a021bbe81b 157 long int VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs
Ringslev 5:d5a021bbe81b 158 long int VM_R = -VX - VY;
Ringslev 5:d5a021bbe81b 159
Ringslev 5:d5a021bbe81b 160 command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian
Ringslev 5:d5a021bbe81b 161 command[1]= (VM_L & 0x0000FF00)>>8;
Ringslev 5:d5a021bbe81b 162 command[2]= (VM_L & 0x00FF0000)>>16;
Ringslev 5:d5a021bbe81b 163 command[3]= (VM_L & 0xFF000000)>>24;
Ringslev 5:d5a021bbe81b 164 send(RPDO1_L, command, 'D', 4); // Controle moteur gauche
Ringslev 5:d5a021bbe81b 165
Ringslev 5:d5a021bbe81b 166 command[0]= (VM_R & 0x000000FF);
Ringslev 5:d5a021bbe81b 167 command[1]= (VM_R & 0x0000FF00)>>8;
Ringslev 5:d5a021bbe81b 168 command[2]= (VM_R & 0x00FF0000)>>16;
Ringslev 5:d5a021bbe81b 169 command[3]= (VM_R & 0xFF000000)>>24;
Ringslev 5:d5a021bbe81b 170 send(RPDO1_R, command, 'D', 4); // Controle moteur droit
Ringslev 5:d5a021bbe81b 171 ////////////////////////////////////////////////////////////////
Ringslev 5:d5a021bbe81b 172 }
Ringslev 5:d5a021bbe81b 173 else if(mode ==1)
Ringslev 5:d5a021bbe81b 174 {
Ringslev 5:d5a021bbe81b 175 // MODE NORMAL
Ringslev 5:d5a021bbe81b 176 }
Ringslev 5:d5a021bbe81b 177 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
Ringslev 5:d5a021bbe81b 178 }
Ringslev 5:d5a021bbe81b 179
Ringslev 5:d5a021bbe81b 180 int main() {
Ringslev 5:d5a021bbe81b 181
Ringslev 5:d5a021bbe81b 182 //define Pullup switch
Ringslev 5:d5a021bbe81b 183 SWH.mode(PullUp);
Ringslev 5:d5a021bbe81b 184 SWV.mode(PullUp);
Ringslev 5:d5a021bbe81b 185 BPO.mode(PullUp);
Ringslev 5:d5a021bbe81b 186
Ringslev 5:d5a021bbe81b 187 initialisation(); // Mise en mode opérationnel des moteurs
Ringslev 5:d5a021bbe81b 188
Ringslev 5:d5a021bbe81b 189 for(int working = 0; working < 50; working++)
Ringslev 5:d5a021bbe81b 190 {
Ringslev 5:d5a021bbe81b 191 send(TPDO1_R, command, 'R', 0); //Ask position
Ringslev 5:d5a021bbe81b 192 timer_process_read = 0;
Ringslev 5:d5a021bbe81b 193 d_new = msg.data[3];
Ringslev 5:d5a021bbe81b 194 d_new = d_new << 8;
Ringslev 5:d5a021bbe81b 195 d_new = d_new | msg.data[2];
Ringslev 5:d5a021bbe81b 196 d_new = d_new << 8;
Ringslev 5:d5a021bbe81b 197 d_new = d_new | msg.data[1];
Ringslev 5:d5a021bbe81b 198 d_new = d_new << 8;
Ringslev 5:d5a021bbe81b 199 d_new = d_new | msg.data[0];
Ringslev 5:d5a021bbe81b 200 pc.printf("%d \r\n",d_new);
Ringslev 5:d5a021bbe81b 201 }
Ringslev 5:d5a021bbe81b 202 d_old = d_new;
Ringslev 5:d5a021bbe81b 203
Ringslev 5:d5a021bbe81b 204 while(1)
Ringslev 5:d5a021bbe81b 205 {
Ringslev 5:d5a021bbe81b 206 //timer_process_ping++;
Ringslev 5:d5a021bbe81b 207 timer_process_read++;
Ringslev 5:d5a021bbe81b 208 pc.printf("%d \t",d_new);
Ringslev 5:d5a021bbe81b 209 pc.printf("%d \t",d_old);
Ringslev 5:d5a021bbe81b 210 //pc.printf("%d \t",d_new);
Ringslev 5:d5a021bbe81b 211 pc.printf("%f \r\n",dy);
protongamer 1:4d70e593345f 212
Ringslev 5:d5a021bbe81b 213 dy = 525000 + (d_new*1000)/2500;
protongamer 4:3fd7c53d31c1 214
protongamer 4:3fd7c53d31c1 215
Ringslev 5:d5a021bbe81b 216 //wait_ms(1);
protongamer 4:3fd7c53d31c1 217
Ringslev 5:d5a021bbe81b 218 /* if(timer_process_ping >= 5000){
protongamer 4:3fd7c53d31c1 219 send(TPDO2_R, command, 'R', 0); //Ask mod of motor
protongamer 4:3fd7c53d31c1 220 timer_process_ping = 0;
protongamer 4:3fd7c53d31c1 221 if(can2.read(msg)) {
protongamer 4:3fd7c53d31c1 222 for(int i = 0; i < msg.len; i++){
protongamer 4:3fd7c53d31c1 223 pc.printf("0x%x ",msg.data[i]);
protongamer 4:3fd7c53d31c1 224 }//end of for
protongamer 4:3fd7c53d31c1 225 pc.printf("\r \n");
protongamer 4:3fd7c53d31c1 226 }//can.read(msg)
protongamer 4:3fd7c53d31c1 227
protongamer 4:3fd7c53d31c1 228 if(msg.data[0] != 0x27 || msg.data[1] != 0x56){//if device is not in operational mode
protongamer 4:3fd7c53d31c1 229 ping = 0;
protongamer 4:3fd7c53d31c1 230 while(ping == 0){
protongamer 4:3fd7c53d31c1 231 pc.printf("ping...\r\n");
protongamer 4:3fd7c53d31c1 232 send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode
protongamer 4:3fd7c53d31c1 233 MOD_NOR = 1;
protongamer 4:3fd7c53d31c1 234 if(can2.read(msg)) {
protongamer 4:3fd7c53d31c1 235 for(int i = 0; i < msg.len; i++){
protongamer 4:3fd7c53d31c1 236 pc.printf("0x%x ",msg.data[i]);
protongamer 4:3fd7c53d31c1 237 ping = 1;
protongamer 4:3fd7c53d31c1 238 }//end of for
protongamer 4:3fd7c53d31c1 239 pc.printf("\r \n");
protongamer 4:3fd7c53d31c1 240 }//can.read(msg)
protongamer 4:3fd7c53d31c1 241 wait(0.1);
protongamer 4:3fd7c53d31c1 242 MOD_NOR = 0;
protongamer 4:3fd7c53d31c1 243 wait(0.9);
protongamer 4:3fd7c53d31c1 244
protongamer 4:3fd7c53d31c1 245 }
protongamer 4:3fd7c53d31c1 246
protongamer 4:3fd7c53d31c1 247 }
protongamer 4:3fd7c53d31c1 248
protongamer 4:3fd7c53d31c1 249 }*/
protongamer 4:3fd7c53d31c1 250
protongamer 4:3fd7c53d31c1 251 if(timer_process_read >= 10){
protongamer 4:3fd7c53d31c1 252 send(TPDO1_R, command, 'R', 0); //Ask position
protongamer 4:3fd7c53d31c1 253 timer_process_read = 0;
protongamer 4:3fd7c53d31c1 254 d_new = msg.data[3];
protongamer 4:3fd7c53d31c1 255 d_new = d_new << 8;
protongamer 4:3fd7c53d31c1 256 d_new = d_new | msg.data[2];
protongamer 4:3fd7c53d31c1 257 d_new = d_new << 8;
protongamer 4:3fd7c53d31c1 258 d_new = d_new | msg.data[1];
protongamer 4:3fd7c53d31c1 259 d_new = d_new << 8;
protongamer 4:3fd7c53d31c1 260 d_new = d_new | msg.data[0];
protongamer 4:3fd7c53d31c1 261
protongamer 4:3fd7c53d31c1 262 /* if(first_time == 0){
protongamer 4:3fd7c53d31c1 263 d_old = d_new;
protongamer 4:3fd7c53d31c1 264 first_time = 1;
protongamer 4:3fd7c53d31c1 265 }*/
protongamer 4:3fd7c53d31c1 266 }
protongamer 4:3fd7c53d31c1 267
protongamer 4:3fd7c53d31c1 268
protongamer 3:86e21a36eecb 269 if(can2.read(msg)) {
protongamer 3:86e21a36eecb 270 for(int i = 0; i < msg.len; i++){
protongamer 3:86e21a36eecb 271 pc.printf("0x%x ",msg.data[i]);
protongamer 3:86e21a36eecb 272 }//end of for
protongamer 3:86e21a36eecb 273 pc.printf("\r \n");
protongamer 4:3fd7c53d31c1 274 }//can.read(msg)
protongamer 3:86e21a36eecb 275
Ringslev 5:d5a021bbe81b 276 control_Moteur(); // Controle du meteur mode degradé et normal
Ringslev 5:d5a021bbe81b 277 display_mode(); // Controle des led affichant le mode
protongamer 1:4d70e593345f 278
protongamer 1:4d70e593345f 279 }// fin while(1)
protongamer 1:4d70e593345f 280
protongamer 1:4d70e593345f 281 } // fin main
protongamer 4:3fd7c53d31c1 282
protongamer 4:3fd7c53d31c1 283
protongamer 4:3fd7c53d31c1 284
protongamer 1:4d70e593345f 285
protongamer 3:86e21a36eecb 286