![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version 2.6.5 mode normal en cours(problème de décalage) voir incrément et coefficient codeur
Dependencies: mbed
Fork of Robot_a_cables_v2_6 by
main.cpp@9:0f63d4cb5613, 2018-05-18 (annotated)
- Committer:
- protongamer
- Date:
- Fri May 18 09:03:25 2018 +0000
- Revision:
- 9:0f63d4cb5613
- Parent:
- 8:1e6d3d9ae1d3
v2.6.5
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LouisReynier | 0:1ab5fdb4fa56 | 1 | #include "mbed.h" |
protongamer | 6:ffffa9dfadfc | 2 | #include "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 | 8:1e6d3d9ae1d3 | 10 | #define DEGRAD 0 |
protongamer | 8:1e6d3d9ae1d3 | 11 | #define NORMAL 1 |
protongamer | 1:4d70e593345f | 12 | |
protongamer | 9:0f63d4cb5613 | 13 | //#define ALPHA 425000 |
protongamer | 8:1e6d3d9ae1d3 | 14 | #define ALPHA 425000 |
protongamer | 3:86e21a36eecb | 15 | |
protongamer | 3:86e21a36eecb | 16 | DigitalOut MOTOR1_OP(LED1); |
protongamer | 3:86e21a36eecb | 17 | DigitalOut MOTOR2_OP(LED2); |
protongamer | 3:86e21a36eecb | 18 | DigitalOut DEB_MOD_DEG(LED3); |
protongamer | 3:86e21a36eecb | 19 | DigitalOut DEB_MOD_NOR(LED4); |
protongamer | 7:e87feff62bfd | 20 | DigitalOut MOD_DEG(p25); |
protongamer | 7:e87feff62bfd | 21 | DigitalOut MOD_NOR(p26); |
protongamer | 3:86e21a36eecb | 22 | DigitalIn SWH(p5); |
protongamer | 3:86e21a36eecb | 23 | DigitalIn SWV(p6); |
protongamer | 3:86e21a36eecb | 24 | DigitalIn BPO(p7); |
Ringslev | 5:d5a021bbe81b | 25 | AnalogIn joystick_ctr(p15); // Pin 2 (fil vert) Center Tap Reference |
Ringslev | 5:d5a021bbe81b | 26 | AnalogIn joystick_x(p16); // Pin 4 (fil jaune) |
Ringslev | 5:d5a021bbe81b | 27 | AnalogIn joystick_y(p17); // Pin 5 (fil bleu) |
protongamer | 3:86e21a36eecb | 28 | |
Ringslev | 5:d5a021bbe81b | 29 | CAN can2(p30, p29, 1000000); // on definit pin et debit |
Ringslev | 5:d5a021bbe81b | 30 | CANMessage msg; |
Ringslev | 5:d5a021bbe81b | 31 | Serial pc(USBTX, USBRX); |
protongamer | 4:3fd7c53d31c1 | 32 | |
Ringslev | 5:d5a021bbe81b | 33 | uint8_t mode = 0; |
Ringslev | 5:d5a021bbe81b | 34 | int x,y,ctr_x,ctr_y; // Variables de joystick |
Ringslev | 5:d5a021bbe81b | 35 | uint8_t ping = 0; //variable pour etat de processus |
Ringslev | 5:d5a021bbe81b | 36 | uint8_t done = 0; |
Ringslev | 5:d5a021bbe81b | 37 | char command[8]; //word to send command |
protongamer | 7:e87feff62bfd | 38 | uint16_t timer_process_read; //counter to make mbed read actual position |
protongamer | 7:e87feff62bfd | 39 | double dy = 1262, dx = 492.5;//coordinates values when origin is done |
protongamer | 6:ffffa9dfadfc | 40 | //these positions are in counters |
protongamer | 6:ffffa9dfadfc | 41 | int32_t p1, p2; //motors positions |
protongamer | 7:e87feff62bfd | 42 | int32_t p_actual, p_actual2, p_pom, p_pom2; //p_actual position read by can, p_pom position done when origin is made |
protongamer | 6:ffffa9dfadfc | 43 | |
protongamer | 6:ffffa9dfadfc | 44 | //longueur câbles |
protongamer | 6:ffffa9dfadfc | 45 | //lx = longueur entre les point des moteurs |
protongamer | 6:ffffa9dfadfc | 46 | //l1 = longueur moteur gauche à l'effecteur |
protongamer | 6:ffffa9dfadfc | 47 | //l2 = longueur moteur droite à l'effecteur |
protongamer | 7:e87feff62bfd | 48 | int32_t lx, l1, l2; |
protongamer | 8:1e6d3d9ae1d3 | 49 | float pVx, pVy; // variables pour afficher les vélocitées appliquées sur le moniteur |
protongamer | 6:ffffa9dfadfc | 50 | |
protongamer | 6:ffffa9dfadfc | 51 | |
protongamer | 6:ffffa9dfadfc | 52 | |
protongamer | 1:4d70e593345f | 53 | |
Ringslev | 5:d5a021bbe81b | 54 | void display_mode() |
Ringslev | 5:d5a021bbe81b | 55 | { |
protongamer | 8:1e6d3d9ae1d3 | 56 | if(mode == DEGRAD) // MODE DEGRADE |
Ringslev | 5:d5a021bbe81b | 57 | { |
Ringslev | 5:d5a021bbe81b | 58 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 59 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 60 | } |
protongamer | 8:1e6d3d9ae1d3 | 61 | else if(mode == NORMAL) // MODE NORMAL |
Ringslev | 5:d5a021bbe81b | 62 | { |
Ringslev | 5:d5a021bbe81b | 63 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 64 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 65 | } |
Ringslev | 5:d5a021bbe81b | 66 | } |
protongamer | 1:4d70e593345f | 67 | |
protongamer | 1:4d70e593345f | 68 | void send(int id, char octet_emis[], char RouD, char longueur ) |
protongamer | 1:4d70e593345f | 69 | { |
protongamer | 1:4d70e593345f | 70 | int emis_ok = 0 ; |
LouisReynier | 0:1ab5fdb4fa56 | 71 | |
protongamer | 4:3fd7c53d31c1 | 72 | MOTOR1_OP = 1; |
protongamer | 4:3fd7c53d31c1 | 73 | MOTOR1_OP = 0; |
protongamer | 4:3fd7c53d31c1 | 74 | |
protongamer | 1:4d70e593345f | 75 | if (RouD == 'D') |
protongamer | 1:4d70e593345f | 76 | { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANData, CANStandard )) ; |
protongamer | 3:86e21a36eecb | 77 | //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 | 78 | }// c'ets la valeur retournée par la fonction write |
protongamer | 1:4d70e593345f | 79 | else |
protongamer | 1:4d70e593345f | 80 | { emis_ok = can2.write(CANMessage(id, octet_emis, longueur, CANRemote, CANStandard )); |
protongamer | 3:86e21a36eecb | 81 | //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 | 82 | } |
protongamer | 1:4d70e593345f | 83 | //lcd.locate(0,10); |
protongamer | 1:4d70e593345f | 84 | //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 | 85 | /*if(emis_ok) { |
protongamer | 1:4d70e593345f | 86 | // ici octet emis n'a pas de sens car trame remote ! |
protongamer | 3:86e21a36eecb | 87 | //pc.printf("send \r \n"); |
protongamer | 3:86e21a36eecb | 88 | } */ |
LouisReynier | 0:1ab5fdb4fa56 | 89 | } |
protongamer | 1:4d70e593345f | 90 | |
Ringslev | 5:d5a021bbe81b | 91 | void initialisation() |
Ringslev | 5:d5a021bbe81b | 92 | { |
Ringslev | 5:d5a021bbe81b | 93 | pc.printf("starting...\r\n"); |
Ringslev | 5:d5a021bbe81b | 94 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 95 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 96 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 97 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 98 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 99 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 100 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 101 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 102 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 103 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 104 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 105 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 106 | MOD_DEG = 1; |
Ringslev | 5:d5a021bbe81b | 107 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 108 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 109 | MOD_DEG = 0; |
Ringslev | 5:d5a021bbe81b | 110 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 111 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 112 | |
protongamer | 6:ffffa9dfadfc | 113 | command[0] = DRIVER_R1; |
Ringslev | 5:d5a021bbe81b | 114 | command[1] = DRIVER_R2; |
Ringslev | 5:d5a021bbe81b | 115 | |
Ringslev | 5:d5a021bbe81b | 116 | while(ping == 0) // INIT MOTEUR DROIT |
Ringslev | 5:d5a021bbe81b | 117 | { |
Ringslev | 5:d5a021bbe81b | 118 | pc.printf("ping droite ...\r\n"); |
Ringslev | 5:d5a021bbe81b | 119 | send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
Ringslev | 5:d5a021bbe81b | 120 | MOD_NOR = 1; |
Ringslev | 5:d5a021bbe81b | 121 | if(can2.read(msg)) |
Ringslev | 5:d5a021bbe81b | 122 | { |
Ringslev | 5:d5a021bbe81b | 123 | for(int i = 0; i < msg.len; i++) |
Ringslev | 5:d5a021bbe81b | 124 | { |
Ringslev | 5:d5a021bbe81b | 125 | pc.printf("0x%x ",msg.data[i]); |
Ringslev | 5:d5a021bbe81b | 126 | ping = 1; |
Ringslev | 5:d5a021bbe81b | 127 | }//end of for |
Ringslev | 5:d5a021bbe81b | 128 | pc.printf("\r \n"); |
Ringslev | 5:d5a021bbe81b | 129 | }//can.read(msg) |
Ringslev | 5:d5a021bbe81b | 130 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 131 | MOD_NOR = 0; |
Ringslev | 5:d5a021bbe81b | 132 | wait(0.9); |
Ringslev | 5:d5a021bbe81b | 133 | } |
Ringslev | 5:d5a021bbe81b | 134 | |
Ringslev | 5:d5a021bbe81b | 135 | ping =0; |
Ringslev | 5:d5a021bbe81b | 136 | command[0] = DRIVER_L1; |
Ringslev | 5:d5a021bbe81b | 137 | command[1] = DRIVER_L2; |
Ringslev | 5:d5a021bbe81b | 138 | |
Ringslev | 5:d5a021bbe81b | 139 | while(ping == 0) // INIT MOTEUR GAUCHE |
Ringslev | 5:d5a021bbe81b | 140 | { |
Ringslev | 5:d5a021bbe81b | 141 | pc.printf("ping gauche ...\r\n"); |
Ringslev | 5:d5a021bbe81b | 142 | send(INITOP, command, 'D', 2); //send word to put pluto driver in operational mode |
protongamer | 3:86e21a36eecb | 143 | MOD_DEG = 1; |
protongamer | 3:86e21a36eecb | 144 | if(can2.read(msg)) { |
protongamer | 4:3fd7c53d31c1 | 145 | for(int i = 0; i < msg.len; i++){ |
protongamer | 4:3fd7c53d31c1 | 146 | pc.printf("0x%x ",msg.data[i]); |
protongamer | 4:3fd7c53d31c1 | 147 | ping = 1; |
protongamer | 4:3fd7c53d31c1 | 148 | }//end of for |
protongamer | 4:3fd7c53d31c1 | 149 | pc.printf("\r \n"); |
protongamer | 4:3fd7c53d31c1 | 150 | }//can.read(msg) |
protongamer | 3:86e21a36eecb | 151 | wait(0.1); |
Ringslev | 5:d5a021bbe81b | 152 | MOD_DEG = 0; |
protongamer | 3:86e21a36eecb | 153 | wait(0.9); |
Ringslev | 5:d5a021bbe81b | 154 | } |
Ringslev | 5:d5a021bbe81b | 155 | } // fin initialisation |
Ringslev | 5:d5a021bbe81b | 156 | |
protongamer | 6:ffffa9dfadfc | 157 | |
protongamer | 6:ffffa9dfadfc | 158 | |
protongamer | 6:ffffa9dfadfc | 159 | void read_position(){ |
protongamer | 6:ffffa9dfadfc | 160 | |
protongamer | 8:1e6d3d9ae1d3 | 161 | //timer_process_read++;//var used to set time for reading value |
protongamer | 8:1e6d3d9ae1d3 | 162 | l1 = LENGTH_L1_PO - (p1/2545); //Length 1 |
protongamer | 8:1e6d3d9ae1d3 | 163 | l2 = LENGTH_L2_PO - (p2/2545); //Length 2 |
protongamer | 8:1e6d3d9ae1d3 | 164 | lx = LENGTH_MOTOR; |
protongamer | 8:1e6d3d9ae1d3 | 165 | dx = (((double) l1*(double) l1) - ((double) l2*(double) l2) + ((double) lx*(double) lx))/(2*(double) lx); |
protongamer | 8:1e6d3d9ae1d3 | 166 | dy = sqrt(((double) l1*(double) l1) - (dx*dx)); |
protongamer | 8:1e6d3d9ae1d3 | 167 | |
protongamer | 8:1e6d3d9ae1d3 | 168 | if(BPO == 0){ |
protongamer | 7:e87feff62bfd | 169 | |
protongamer | 8:1e6d3d9ae1d3 | 170 | p_pom = p_actual; |
protongamer | 8:1e6d3d9ae1d3 | 171 | p_pom2 = p_actual2; |
protongamer | 8:1e6d3d9ae1d3 | 172 | mode = !mode; //Change mode |
protongamer | 8:1e6d3d9ae1d3 | 173 | wait(1.0); |
protongamer | 8:1e6d3d9ae1d3 | 174 | } |
protongamer | 6:ffffa9dfadfc | 175 | |
protongamer | 8:1e6d3d9ae1d3 | 176 | p1 = p_pom - p_actual; |
protongamer | 8:1e6d3d9ae1d3 | 177 | p2 = p_pom2 - p_actual2; |
protongamer | 8:1e6d3d9ae1d3 | 178 | |
protongamer | 8:1e6d3d9ae1d3 | 179 | //Left motor |
protongamer | 8:1e6d3d9ae1d3 | 180 | send(TPDO1_L, command, 'R', 0); //Ask position for second length |
protongamer | 8:1e6d3d9ae1d3 | 181 | if(can2.read(msg)){ |
protongamer | 8:1e6d3d9ae1d3 | 182 | p_actual = msg.data[3]; |
protongamer | 8:1e6d3d9ae1d3 | 183 | p_actual = p_actual << 8; |
protongamer | 8:1e6d3d9ae1d3 | 184 | p_actual = p_actual | msg.data[2]; |
protongamer | 8:1e6d3d9ae1d3 | 185 | p_actual = p_actual << 8; |
protongamer | 8:1e6d3d9ae1d3 | 186 | p_actual = p_actual | msg.data[1]; |
protongamer | 8:1e6d3d9ae1d3 | 187 | p_actual = p_actual << 8; |
protongamer | 8:1e6d3d9ae1d3 | 188 | p_actual = p_actual | msg.data[0]; |
protongamer | 8:1e6d3d9ae1d3 | 189 | } |
protongamer | 8:1e6d3d9ae1d3 | 190 | //timer_process_read = 1; |
protongamer | 9:0f63d4cb5613 | 191 | wait(0.005); |
protongamer | 8:1e6d3d9ae1d3 | 192 | send(TPDO1_R, command, 'R', 0); //Ask position for first length |
protongamer | 8:1e6d3d9ae1d3 | 193 | if(can2.read(msg)){ |
protongamer | 8:1e6d3d9ae1d3 | 194 | p_actual2 = msg.data[3]; |
protongamer | 8:1e6d3d9ae1d3 | 195 | p_actual2 = p_actual2 << 8; |
protongamer | 8:1e6d3d9ae1d3 | 196 | p_actual2 = p_actual2 | msg.data[2]; |
protongamer | 8:1e6d3d9ae1d3 | 197 | p_actual2 = p_actual2 << 8; |
protongamer | 8:1e6d3d9ae1d3 | 198 | p_actual2 = p_actual2 | msg.data[1]; |
protongamer | 8:1e6d3d9ae1d3 | 199 | p_actual2 = p_actual2 << 8; |
protongamer | 8:1e6d3d9ae1d3 | 200 | p_actual2 = p_actual2 | msg.data[0]; |
protongamer | 8:1e6d3d9ae1d3 | 201 | } |
protongamer | 8:1e6d3d9ae1d3 | 202 | |
protongamer | 6:ffffa9dfadfc | 203 | |
protongamer | 6:ffffa9dfadfc | 204 | }//fin de fonction |
protongamer | 6:ffffa9dfadfc | 205 | |
protongamer | 6:ffffa9dfadfc | 206 | |
protongamer | 6:ffffa9dfadfc | 207 | |
Ringslev | 5:d5a021bbe81b | 208 | void control_Moteur() |
Ringslev | 5:d5a021bbe81b | 209 | { |
Ringslev | 5:d5a021bbe81b | 210 | ///////////// CONTROLE MOTEUR ////////////////////////////////////////////////////////////////////////////// |
Ringslev | 5:d5a021bbe81b | 211 | // LECTURE Joystick + stockage valeurs dans x, y |
Ringslev | 5:d5a021bbe81b | 212 | ctr_x = int(-((joystick_ctr.read()*100)-50)); |
Ringslev | 5:d5a021bbe81b | 213 | ctr_y = int((joystick_ctr.read()*100)-50); |
Ringslev | 5:d5a021bbe81b | 214 | 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 | 215 | if(SWV){y = int((joystick_y.read()*100)-50)+ctr_y;}else{y=0;} // Detection y avec correction ctr |
protongamer | 8:1e6d3d9ae1d3 | 216 | float VX; // Calcule brut vélocité x, y |
protongamer | 8:1e6d3d9ae1d3 | 217 | float VY; |
protongamer | 8:1e6d3d9ae1d3 | 218 | long int VM_L; // Calcule des vélocités exactes de chaques moteurs |
protongamer | 8:1e6d3d9ae1d3 | 219 | long int VM_R; |
protongamer | 8:1e6d3d9ae1d3 | 220 | |
Ringslev | 5:d5a021bbe81b | 221 | if(mode ==0) |
Ringslev | 5:d5a021bbe81b | 222 | { |
Ringslev | 5:d5a021bbe81b | 223 | ///////////// MODE DEGRADE ///////////////////////////////////// |
protongamer | 8:1e6d3d9ae1d3 | 224 | VX = (float(x)/42)*425000; // Calcule brut vélocité x, y |
protongamer | 8:1e6d3d9ae1d3 | 225 | VY = (float(y)/42)*425000; |
protongamer | 8:1e6d3d9ae1d3 | 226 | VM_L = VX - VY; // Calcule des vélocités exactes de chaques moteurs |
protongamer | 8:1e6d3d9ae1d3 | 227 | VM_R = -VX - VY; |
Ringslev | 5:d5a021bbe81b | 228 | |
Ringslev | 5:d5a021bbe81b | 229 | command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian |
Ringslev | 5:d5a021bbe81b | 230 | command[1]= (VM_L & 0x0000FF00)>>8; |
Ringslev | 5:d5a021bbe81b | 231 | command[2]= (VM_L & 0x00FF0000)>>16; |
Ringslev | 5:d5a021bbe81b | 232 | command[3]= (VM_L & 0xFF000000)>>24; |
Ringslev | 5:d5a021bbe81b | 233 | send(RPDO1_L, command, 'D', 4); // Controle moteur gauche |
Ringslev | 5:d5a021bbe81b | 234 | |
Ringslev | 5:d5a021bbe81b | 235 | command[0]= (VM_R & 0x000000FF); |
Ringslev | 5:d5a021bbe81b | 236 | command[1]= (VM_R & 0x0000FF00)>>8; |
Ringslev | 5:d5a021bbe81b | 237 | command[2]= (VM_R & 0x00FF0000)>>16; |
Ringslev | 5:d5a021bbe81b | 238 | command[3]= (VM_R & 0xFF000000)>>24; |
Ringslev | 5:d5a021bbe81b | 239 | send(RPDO1_R, command, 'D', 4); // Controle moteur droit |
Ringslev | 5:d5a021bbe81b | 240 | //////////////////////////////////////////////////////////////// |
Ringslev | 5:d5a021bbe81b | 241 | } |
Ringslev | 5:d5a021bbe81b | 242 | else if(mode ==1) |
Ringslev | 5:d5a021bbe81b | 243 | { |
protongamer | 8:1e6d3d9ae1d3 | 244 | ///////////// MODE NORMAL ////////////////////////////////////// |
protongamer | 8:1e6d3d9ae1d3 | 245 | VX = x; |
protongamer | 8:1e6d3d9ae1d3 | 246 | VY = y; |
protongamer | 9:0f63d4cb5613 | 247 | VM_L = ((dx*VX)-(dy*VY)); |
protongamer | 9:0f63d4cb5613 | 248 | VM_L = (VM_L/l1); |
protongamer | 9:0f63d4cb5613 | 249 | VM_L = VM_L*(ALPHA/42); |
protongamer | 9:0f63d4cb5613 | 250 | VM_R = ((-(lx-dx)*VX)-(dy*VY)); |
protongamer | 9:0f63d4cb5613 | 251 | VM_R = (VM_R/l2); |
protongamer | 9:0f63d4cb5613 | 252 | VM_R = VM_R*(ALPHA/42); |
protongamer | 8:1e6d3d9ae1d3 | 253 | |
protongamer | 8:1e6d3d9ae1d3 | 254 | command[0]= (VM_L & 0x000000FF); // Masque + décalage pour translation big vers little endian |
protongamer | 8:1e6d3d9ae1d3 | 255 | command[1]= (VM_L & 0x0000FF00)>>8; |
protongamer | 8:1e6d3d9ae1d3 | 256 | command[2]= (VM_L & 0x00FF0000)>>16; |
protongamer | 8:1e6d3d9ae1d3 | 257 | command[3]= (VM_L & 0xFF000000)>>24; |
protongamer | 8:1e6d3d9ae1d3 | 258 | send(RPDO1_L, command, 'D', 4); // Controle moteur gauche |
protongamer | 8:1e6d3d9ae1d3 | 259 | |
protongamer | 8:1e6d3d9ae1d3 | 260 | command[0]= (VM_R & 0x000000FF); |
protongamer | 8:1e6d3d9ae1d3 | 261 | command[1]= (VM_R & 0x0000FF00)>>8; |
protongamer | 8:1e6d3d9ae1d3 | 262 | command[2]= (VM_R & 0x00FF0000)>>16; |
protongamer | 8:1e6d3d9ae1d3 | 263 | command[3]= (VM_R & 0xFF000000)>>24; |
protongamer | 8:1e6d3d9ae1d3 | 264 | send(RPDO1_R, command, 'D', 4); // Controle moteur droit |
Ringslev | 5:d5a021bbe81b | 265 | } |
protongamer | 8:1e6d3d9ae1d3 | 266 | pVx = VM_R; |
protongamer | 8:1e6d3d9ae1d3 | 267 | pVy = VM_L; |
Ringslev | 5:d5a021bbe81b | 268 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Ringslev | 5:d5a021bbe81b | 269 | } |
Ringslev | 5:d5a021bbe81b | 270 | |
Ringslev | 5:d5a021bbe81b | 271 | int main() { |
Ringslev | 5:d5a021bbe81b | 272 | |
Ringslev | 5:d5a021bbe81b | 273 | //define Pullup switch |
Ringslev | 5:d5a021bbe81b | 274 | SWH.mode(PullUp); |
Ringslev | 5:d5a021bbe81b | 275 | SWV.mode(PullUp); |
Ringslev | 5:d5a021bbe81b | 276 | BPO.mode(PullUp); |
Ringslev | 5:d5a021bbe81b | 277 | |
Ringslev | 5:d5a021bbe81b | 278 | initialisation(); // Mise en mode opérationnel des moteurs |
Ringslev | 5:d5a021bbe81b | 279 | |
protongamer | 6:ffffa9dfadfc | 280 | |
protongamer | 6:ffffa9dfadfc | 281 | |
protongamer | 8:1e6d3d9ae1d3 | 282 | |
Ringslev | 5:d5a021bbe81b | 283 | while(1) |
Ringslev | 5:d5a021bbe81b | 284 | { |
protongamer | 7:e87feff62bfd | 285 | |
protongamer | 7:e87feff62bfd | 286 | |
protongamer | 7:e87feff62bfd | 287 | pc.printf("L1 = %d", l1); |
protongamer | 7:e87feff62bfd | 288 | pc.printf("\t L2 = %d", l2); |
protongamer | 7:e87feff62bfd | 289 | pc.printf("\t x = %f", dx); |
protongamer | 8:1e6d3d9ae1d3 | 290 | pc.printf("\t y = %f", dy); |
protongamer | 8:1e6d3d9ae1d3 | 291 | pc.printf("\t VM_L = %f", pVx); |
protongamer | 8:1e6d3d9ae1d3 | 292 | pc.printf("\t VM_R = %f \r\n", pVy); |
protongamer | 4:3fd7c53d31c1 | 293 | |
protongamer | 7:e87feff62bfd | 294 | |
protongamer | 8:1e6d3d9ae1d3 | 295 | read_position(); |
Ringslev | 5:d5a021bbe81b | 296 | control_Moteur(); // Controle du meteur mode degradé et normal |
Ringslev | 5:d5a021bbe81b | 297 | display_mode(); // Controle des led affichant le mode |
protongamer | 1:4d70e593345f | 298 | |
protongamer | 1:4d70e593345f | 299 | }// fin while(1) |
protongamer | 1:4d70e593345f | 300 | |
protongamer | 1:4d70e593345f | 301 | } // fin main |
protongamer | 4:3fd7c53d31c1 | 302 | |
protongamer | 4:3fd7c53d31c1 | 303 | |
protongamer | 4:3fd7c53d31c1 | 304 | |
protongamer | 1:4d70e593345f | 305 | |
protongamer | 3:86e21a36eecb | 306 |