Leopold Clement
/
PSL-VicTeam
faut que Val répare le truc
main.cpp@16:1bbf7e4249d7, 2020-06-10 (annotated)
- Committer:
- leocle
- Date:
- Wed Jun 10 12:52:54 2020 +0000
- Revision:
- 16:1bbf7e4249d7
- Parent:
- 15:ccfe333bcf76
- Child:
- 17:9ba65b880f06
correction de la fonction send_roue;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
leocle | 0:8762773cddf9 | 1 | /* #include <mbed_events.h> */ |
leocle | 0:8762773cddf9 | 2 | #include "mbed.h" |
leocle | 14:b5e4219b01d9 | 3 | |
jlesoudeer | 13:dfbfefe2f0a8 | 4 | #define adrr_queue 2 |
jlesoudeer | 13:dfbfefe2f0a8 | 5 | #define adrr_gauche 1 |
jlesoudeer | 13:dfbfefe2f0a8 | 6 | #define adrr_droite 3 |
leocle | 0:8762773cddf9 | 7 | |
leocle | 16:1bbf7e4249d7 | 8 | #define hex_queue 0x02 |
leocle | 16:1bbf7e4249d7 | 9 | #define hex_gauche 0x01 |
leocle | 16:1bbf7e4249d7 | 10 | #define hex_droite 0x03 |
leocle | 14:b5e4219b01d9 | 11 | |
leocle | 0:8762773cddf9 | 12 | |
leocle | 0:8762773cddf9 | 13 | DigitalOut alivenessLED(LED1); |
leocle | 0:8762773cddf9 | 14 | DigitalOut testLed(LED2); |
leocle | 0:8762773cddf9 | 15 | |
jlesoudeer | 13:dfbfefe2f0a8 | 16 | Serial coach(D1, D0); |
jlesoudeer | 13:dfbfefe2f0a8 | 17 | /*Serial coach(USBTX, USBRX);*/ |
leocle | 6:b32a24905cd6 | 18 | Serial myserial_ax12(PC_4,PC_5,115200); // Modifier le baud rate en fonction de votre utilisation des moteurs |
leocle | 0:8762773cddf9 | 19 | |
leocle | 0:8762773cddf9 | 20 | const float rapport_vitesse = 0.019383809*0.026315; |
leocle | 0:8762773cddf9 | 21 | const float root3 = 1.7321; |
leocle | 6:b32a24905cd6 | 22 | /*a=moteur de queue |
leocle | 5:86f9ec683ad5 | 23 | AX12 myax12a (PC_4, PC_5, 2,115200); |
leocle | 6:b32a24905cd6 | 24 | b : moteur de gauche |
leocle | 5:86f9ec683ad5 | 25 | AX12 myax12b (PC_4, PC_5, 1,115200); |
leocle | 6:b32a24905cd6 | 26 | c : moteur de droite |
leocle | 6:b32a24905cd6 | 27 | AX12 myax12c (PC_4, PC_5, 3,115200);*/ |
leocle | 0:8762773cddf9 | 28 | |
leocle | 0:8762773cddf9 | 29 | int i = 0; |
leocle | 0:8762773cddf9 | 30 | char last_read; |
leocle | 1:c5e9c436f8d0 | 31 | int indice_a_modifie = 1; |
leocle | 2:223d6363315b | 32 | char message_0[50] = {}; |
leocle | 2:223d6363315b | 33 | char message_1[50] = {}; |
leocle | 1:c5e9c436f8d0 | 34 | char message_vide = '0'; |
leocle | 0:8762773cddf9 | 35 | float front, lat, rot, T_front, T_lat = 0; |
leocle | 2:223d6363315b | 36 | int flag_msg = 0; |
leocle | 2:223d6363315b | 37 | int flag_a_traite = 0; |
leocle | 1:c5e9c436f8d0 | 38 | |
leocle | 6:b32a24905cd6 | 39 | |
leocle | 6:b32a24905cd6 | 40 | DigitalOut my_led1(LED1); |
leocle | 6:b32a24905cd6 | 41 | DigitalOut my_led2(LED2); |
leocle | 6:b32a24905cd6 | 42 | DigitalOut my_led3(LED3); |
leocle | 6:b32a24905cd6 | 43 | DigitalIn bp(USER_BUTTON); |
leocle | 6:b32a24905cd6 | 44 | |
leocle | 6:b32a24905cd6 | 45 | void SetCRSpeed(int ID, float speed); |
leocle | 6:b32a24905cd6 | 46 | void SetMode(int ID, int mode); |
leocle | 6:b32a24905cd6 | 47 | void SetCWLimit (int ID, int degrees); |
leocle | 6:b32a24905cd6 | 48 | void SetCCWLimit (int ID, int degrees); |
leocle | 6:b32a24905cd6 | 49 | void Write(int ID, int start, int bytes, char* data); |
leocle | 6:b32a24905cd6 | 50 | |
leocle | 6:b32a24905cd6 | 51 | char tata[10] = {}; |
leocle | 6:b32a24905cd6 | 52 | uint16_t position_1; |
leocle | 6:b32a24905cd6 | 53 | uint16_t position_2; |
leocle | 6:b32a24905cd6 | 54 | uint16_t position_3; |
leocle | 6:b32a24905cd6 | 55 | int num_moteur = 1; |
leocle | 6:b32a24905cd6 | 56 | int vitesse; |
leocle | 6:b32a24905cd6 | 57 | char sens; |
leocle | 6:b32a24905cd6 | 58 | int temps_ms = 0; |
leocle | 6:b32a24905cd6 | 59 | //pour connaitre le nombre de tours |
leocle | 6:b32a24905cd6 | 60 | int tour_1 = 0; |
leocle | 6:b32a24905cd6 | 61 | int tour_2 = 0; |
leocle | 6:b32a24905cd6 | 62 | int tour_3 = 0; |
leocle | 6:b32a24905cd6 | 63 | |
leocle | 6:b32a24905cd6 | 64 | Ticker ticker_1ms; |
leocle | 6:b32a24905cd6 | 65 | |
leocle | 6:b32a24905cd6 | 66 | uint16_t position_1_old; |
leocle | 6:b32a24905cd6 | 67 | uint16_t position_2_old; |
leocle | 6:b32a24905cd6 | 68 | uint16_t position_3_old; |
leocle | 6:b32a24905cd6 | 69 | int16_t delta_position_1; |
leocle | 6:b32a24905cd6 | 70 | int16_t delta_position_2; |
leocle | 6:b32a24905cd6 | 71 | int16_t delta_position_3; |
leocle | 6:b32a24905cd6 | 72 | |
leocle | 6:b32a24905cd6 | 73 | //Définition des trames de requête |
leocle | 14:b5e4219b01d9 | 74 | char trame_demande_pos_1[8]= {0xFF,0xFF,hex_queue,0x4,0x2,0x24,2,0xD2}; |
leocle | 14:b5e4219b01d9 | 75 | char trame_demande_pos_2[8]= {0xFF,0xFF,hex_gauche,0x4,0x2,0x24,2,0xD1}; |
leocle | 14:b5e4219b01d9 | 76 | char trame_demande_pos_3[8]= {0xFF,0xFF,hex_droite,0x4,0x2,0x24,2,0xD0}; |
leocle | 6:b32a24905cd6 | 77 | |
leocle | 6:b32a24905cd6 | 78 | |
leocle | 6:b32a24905cd6 | 79 | //Fonction d'interruption sur réception de caractère sur liaison série |
leocle | 6:b32a24905cd6 | 80 | void Rx_Irq_fonction(void) |
leocle | 6:b32a24905cd6 | 81 | { |
leocle | 6:b32a24905cd6 | 82 | uint16_t position_lu = 0; |
leocle | 6:b32a24905cd6 | 83 | my_led1 = !my_led1; |
leocle | 6:b32a24905cd6 | 84 | |
leocle | 6:b32a24905cd6 | 85 | static int i_rx=0; |
leocle | 6:b32a24905cd6 | 86 | tata[i_rx]=myserial_ax12.getc(); |
leocle | 6:b32a24905cd6 | 87 | |
leocle | 6:b32a24905cd6 | 88 | if (tata[0]!=0xFF) // On vérifie que le premier et le deuxième octet sont biens 0xFF |
leocle | 6:b32a24905cd6 | 89 | i_rx=0; |
leocle | 6:b32a24905cd6 | 90 | else i_rx++; |
leocle | 6:b32a24905cd6 | 91 | if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0; |
leocle | 6:b32a24905cd6 | 92 | |
leocle | 6:b32a24905cd6 | 93 | //Une fois qu'on sait que la trame est "juste" on récupère le reste |
leocle | 6:b32a24905cd6 | 94 | if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) { |
leocle | 6:b32a24905cd6 | 95 | my_led3 = !my_led3; |
leocle | 6:b32a24905cd6 | 96 | i_rx=0; |
leocle | 6:b32a24905cd6 | 97 | |
leocle | 6:b32a24905cd6 | 98 | if(tata[4] == 0) { //si erreur est nulle |
leocle | 6:b32a24905cd6 | 99 | //On transforme les données en fonction de ce qu'on lit depuis les moteurs |
leocle | 6:b32a24905cd6 | 100 | position_lu = tata[5]+((uint16_t)tata[6]<<8); |
leocle | 6:b32a24905cd6 | 101 | if((0<= position_lu)&&(position_lu<=4095)) { |
leocle | 6:b32a24905cd6 | 102 | position_1 = position_lu; |
leocle | 6:b32a24905cd6 | 103 | } |
leocle | 6:b32a24905cd6 | 104 | } |
leocle | 6:b32a24905cd6 | 105 | for(int i=0; i<10; i++) { |
leocle | 6:b32a24905cd6 | 106 | tata[i]=0; |
leocle | 6:b32a24905cd6 | 107 | } |
leocle | 6:b32a24905cd6 | 108 | } |
leocle | 6:b32a24905cd6 | 109 | |
leocle | 6:b32a24905cd6 | 110 | if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) { |
leocle | 6:b32a24905cd6 | 111 | my_led3 = !my_led3; |
leocle | 6:b32a24905cd6 | 112 | i_rx=0; |
leocle | 6:b32a24905cd6 | 113 | |
leocle | 6:b32a24905cd6 | 114 | if(tata[4]==00) { //si erreur nulle |
leocle | 6:b32a24905cd6 | 115 | //On transforme les données en fonction de ce qu'on lit depuis les moteurs |
leocle | 6:b32a24905cd6 | 116 | position_lu = tata[5]+((uint16_t)tata[6]<<8); |
leocle | 6:b32a24905cd6 | 117 | if((0<= position_lu)&&(position_lu<=4095)) { |
leocle | 6:b32a24905cd6 | 118 | position_2 = position_lu; |
leocle | 6:b32a24905cd6 | 119 | } |
leocle | 6:b32a24905cd6 | 120 | |
leocle | 6:b32a24905cd6 | 121 | } |
leocle | 6:b32a24905cd6 | 122 | for(int i=0; i<10; i++) { |
leocle | 6:b32a24905cd6 | 123 | tata[i]=0; |
leocle | 6:b32a24905cd6 | 124 | } |
leocle | 6:b32a24905cd6 | 125 | } |
leocle | 6:b32a24905cd6 | 126 | |
leocle | 6:b32a24905cd6 | 127 | if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) { |
leocle | 6:b32a24905cd6 | 128 | my_led3 = !my_led3; |
leocle | 6:b32a24905cd6 | 129 | i_rx=0; |
leocle | 6:b32a24905cd6 | 130 | |
leocle | 6:b32a24905cd6 | 131 | if(tata[4]==00) { //si erreur nulle |
leocle | 6:b32a24905cd6 | 132 | //On transforme les données en fonction de ce qu'on lit depuis les moteurs |
leocle | 6:b32a24905cd6 | 133 | position_lu = tata[5]+((uint16_t)tata[6]<<8); |
leocle | 6:b32a24905cd6 | 134 | if((0<= position_lu)&&(position_lu<=4095)) { |
leocle | 6:b32a24905cd6 | 135 | position_3 = position_lu; |
leocle | 6:b32a24905cd6 | 136 | } |
leocle | 6:b32a24905cd6 | 137 | |
leocle | 6:b32a24905cd6 | 138 | } |
leocle | 6:b32a24905cd6 | 139 | for(int i=0; i<10; i++) { |
leocle | 6:b32a24905cd6 | 140 | tata[i]=0; |
leocle | 6:b32a24905cd6 | 141 | } |
leocle | 6:b32a24905cd6 | 142 | } |
leocle | 6:b32a24905cd6 | 143 | |
leocle | 6:b32a24905cd6 | 144 | if (i_rx>9) { |
leocle | 6:b32a24905cd6 | 145 | for(int i=0; i<10; i++) |
leocle | 6:b32a24905cd6 | 146 | tata[i]=0; |
leocle | 6:b32a24905cd6 | 147 | i_rx=0; |
leocle | 6:b32a24905cd6 | 148 | } |
leocle | 6:b32a24905cd6 | 149 | } |
leocle | 6:b32a24905cd6 | 150 | |
leocle | 6:b32a24905cd6 | 151 | void fonction_1ms(void) |
leocle | 6:b32a24905cd6 | 152 | { |
leocle | 6:b32a24905cd6 | 153 | temps_ms++; |
leocle | 6:b32a24905cd6 | 154 | } |
leocle | 6:b32a24905cd6 | 155 | |
leocle | 6:b32a24905cd6 | 156 | |
leocle | 6:b32a24905cd6 | 157 | |
leocle | 1:c5e9c436f8d0 | 158 | void asser(float Vx, float Vy, float gammaz) |
leocle | 1:c5e9c436f8d0 | 159 | { |
leocle | 0:8762773cddf9 | 160 | float wa = rapport_vitesse*(Vx + 100 * gammaz); |
leocle | 0:8762773cddf9 | 161 | float wb = rapport_vitesse*( -0.5 * Vx - (root3 / 2) * Vy + 100 * gammaz); |
leocle | 0:8762773cddf9 | 162 | float wc = rapport_vitesse*( -0.5 * Vx + (root3 / 2) * Vy + 100 * gammaz); |
leocle | 12:7d61181d7ac7 | 163 | /*moteur a */ |
leocle | 12:7d61181d7ac7 | 164 | SetCRSpeed(adrr_queue, wa); |
leocle | 16:1bbf7e4249d7 | 165 | wait(0.1) |
leocle | 12:7d61181d7ac7 | 166 | /*moteur b */ |
leocle | 12:7d61181d7ac7 | 167 | SetCRSpeed(adrr_gauche, wb); |
leocle | 16:1bbf7e4249d7 | 168 | wait(0.1) |
leocle | 12:7d61181d7ac7 | 169 | /* moteur c */ |
leocle | 12:7d61181d7ac7 | 170 | SetCRSpeed(adrr_droite, wc); |
leocle | 16:1bbf7e4249d7 | 171 | wait(0.1) |
leocle | 1:c5e9c436f8d0 | 172 | } |
leocle | 0:8762773cddf9 | 173 | |
leocle | 1:c5e9c436f8d0 | 174 | void send_stat() |
leocle | 1:c5e9c436f8d0 | 175 | { |
leocle | 16:1bbf7e4249d7 | 176 | coach.printf("S%f,%f,%f,%f,%f \r\n", front, lat, rot, T_front, T_lat ); |
leocle | 1:c5e9c436f8d0 | 177 | } |
leocle | 0:8762773cddf9 | 178 | |
leocle | 6:b32a24905cd6 | 179 | void send_roues() |
leocle | 6:b32a24905cd6 | 180 | { |
leocle | 16:1bbf7e4249d7 | 181 | coach.printf("\n\rR %5d ; %5d ; %5d ; %5d ; %5d ; %5d\r\n", position_1, delta_position_1, position_2, delta_position_2, position_3, delta_position_3); |
leocle | 6:b32a24905cd6 | 182 | } |
leocle | 6:b32a24905cd6 | 183 | |
leocle | 2:223d6363315b | 184 | void mise_a_zero_msg_0() |
leocle | 1:c5e9c436f8d0 | 185 | { |
leocle | 1:c5e9c436f8d0 | 186 | for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères |
leocle | 2:223d6363315b | 187 | message_0[i] = message_vide; |
leocle | 1:c5e9c436f8d0 | 188 | } |
leocle | 2:223d6363315b | 189 | } |
leocle | 2:223d6363315b | 190 | |
leocle | 2:223d6363315b | 191 | |
leocle | 2:223d6363315b | 192 | void mise_a_zero_msg_1() |
leocle | 2:223d6363315b | 193 | { |
leocle | 2:223d6363315b | 194 | for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères |
leocle | 2:223d6363315b | 195 | message_1[i] = message_vide; |
leocle | 2:223d6363315b | 196 | } |
leocle | 1:c5e9c436f8d0 | 197 | } |
leocle | 1:c5e9c436f8d0 | 198 | |
leocle | 2:223d6363315b | 199 | void callback_serial() |
leocle | 1:c5e9c436f8d0 | 200 | { |
leocle | 2:223d6363315b | 201 | last_read = coach.getc(); |
leocle | 2:223d6363315b | 202 | if (last_read == 'S') { |
leocle | 2:223d6363315b | 203 | send_stat(); |
leocle | 6:b32a24905cd6 | 204 | } else if (last_read == 'R') { |
leocle | 6:b32a24905cd6 | 205 | send_roues(); |
leocle | 2:223d6363315b | 206 | } else if (last_read == ';') { |
leocle | 3:c2c22b4f984b | 207 | coach.puts("EOL\n"); |
leocle | 3:c2c22b4f984b | 208 | /*coach.puts(message_0); |
leocle | 3:c2c22b4f984b | 209 | coach.puts(message_1); */ |
leocle | 2:223d6363315b | 210 | if (flag_msg == 0) { |
leocle | 2:223d6363315b | 211 | message_0[indice_a_modifie] = last_read; |
leocle | 2:223d6363315b | 212 | mise_a_zero_msg_1(); |
leocle | 2:223d6363315b | 213 | flag_msg = 1; |
leocle | 2:223d6363315b | 214 | } else { |
leocle | 2:223d6363315b | 215 | message_1[indice_a_modifie] = last_read; |
leocle | 2:223d6363315b | 216 | mise_a_zero_msg_0(); |
leocle | 2:223d6363315b | 217 | flag_msg = 0; |
leocle | 2:223d6363315b | 218 | } |
leocle | 2:223d6363315b | 219 | flag_a_traite = 1; |
leocle | 2:223d6363315b | 220 | indice_a_modifie = 0; |
leocle | 2:223d6363315b | 221 | } else { |
leocle | 2:223d6363315b | 222 | if (flag_msg == 0) { |
leocle | 2:223d6363315b | 223 | message_0[indice_a_modifie] = last_read; |
leocle | 2:223d6363315b | 224 | } else { |
leocle | 2:223d6363315b | 225 | message_1[indice_a_modifie] = last_read; |
leocle | 2:223d6363315b | 226 | } |
leocle | 2:223d6363315b | 227 | indice_a_modifie++; |
leocle | 2:223d6363315b | 228 | } |
leocle | 2:223d6363315b | 229 | } |
leocle | 1:c5e9c436f8d0 | 230 | |
leocle | 2:223d6363315b | 231 | void reception_com() |
leocle | 2:223d6363315b | 232 | { |
leocle | 2:223d6363315b | 233 | if (flag_a_traite == 1) { |
leocle | 3:c2c22b4f984b | 234 | coach.puts("traitement\n"); |
leocle | 2:223d6363315b | 235 | if (flag_msg == 0) { |
leocle | 2:223d6363315b | 236 | sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); |
leocle | 2:223d6363315b | 237 | } else { |
leocle | 2:223d6363315b | 238 | sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); |
leocle | 1:c5e9c436f8d0 | 239 | } |
leocle | 2:223d6363315b | 240 | flag_a_traite = 0; |
leocle | 1:c5e9c436f8d0 | 241 | } |
leocle | 1:c5e9c436f8d0 | 242 | } |
leocle | 0:8762773cddf9 | 243 | |
leocle | 2:223d6363315b | 244 | |
leocle | 1:c5e9c436f8d0 | 245 | int main() |
leocle | 1:c5e9c436f8d0 | 246 | { |
leocle | 11:75b6dde00307 | 247 | |
jlesoudeer | 8:922178e5d286 | 248 | coach.baud(115200); |
leocle | 10:1675ab60cf43 | 249 | coach.puts("slt\n\r"); |
leocle | 7:49ef78d60837 | 250 | coach.attach(&callback_serial, Serial::RxIrq); |
leocle | 7:49ef78d60837 | 251 | |
leocle | 6:b32a24905cd6 | 252 | //myax12.SetMode(1); //passage en Continuous rotation |
leocle | 6:b32a24905cd6 | 253 | |
leocle | 6:b32a24905cd6 | 254 | //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6}; |
leocle | 6:b32a24905cd6 | 255 | //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5}; |
leocle | 6:b32a24905cd6 | 256 | //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4}; |
leocle | 6:b32a24905cd6 | 257 | |
leocle | 6:b32a24905cd6 | 258 | //pour regarder les 3 moteurs les uns à la suite des autres |
jlesoudeer | 8:922178e5d286 | 259 | |
leocle | 6:b32a24905cd6 | 260 | int etat=1; |
leocle | 6:b32a24905cd6 | 261 | int fm_etat=1; |
leocle | 6:b32a24905cd6 | 262 | |
leocle | 12:7d61181d7ac7 | 263 | SetMode(adrr_queue , 1); |
leocle | 6:b32a24905cd6 | 264 | wait(0.1); |
leocle | 12:7d61181d7ac7 | 265 | SetMode(adrr_droite, 1); |
leocle | 6:b32a24905cd6 | 266 | wait(0.1); |
leocle | 12:7d61181d7ac7 | 267 | SetMode(adrr_gauche, 1); |
leocle | 6:b32a24905cd6 | 268 | |
leocle | 6:b32a24905cd6 | 269 | /* SetCRSpeed(1,0.05); |
leocle | 6:b32a24905cd6 | 270 | wait(0.1); |
leocle | 6:b32a24905cd6 | 271 | SetCRSpeed(2,0.05); |
leocle | 6:b32a24905cd6 | 272 | wait(0.1); |
leocle | 6:b32a24905cd6 | 273 | SetCRSpeed(3,0.05); */ |
leocle | 6:b32a24905cd6 | 274 | |
leocle | 6:b32a24905cd6 | 275 | char a; |
leocle | 6:b32a24905cd6 | 276 | |
leocle | 6:b32a24905cd6 | 277 | //On passe le mode retour en 1 : retour seulement sur demande de lecture |
leocle | 6:b32a24905cd6 | 278 | a=1; |
leocle | 12:7d61181d7ac7 | 279 | Write(adrr_queue,16,1,&a); |
leocle | 12:7d61181d7ac7 | 280 | Write(adrr_droite,16,1,&a); |
leocle | 12:7d61181d7ac7 | 281 | Write(adrr_gauche,16,1,&a); |
leocle | 6:b32a24905cd6 | 282 | |
leocle | 6:b32a24905cd6 | 283 | |
leocle | 6:b32a24905cd6 | 284 | //On passe le délai de retour à 250µs. C'est bien de le diminuer ensuite. |
leocle | 6:b32a24905cd6 | 285 | a=250; |
leocle | 12:7d61181d7ac7 | 286 | Write(adrr_queue,5,1,&a); |
leocle | 12:7d61181d7ac7 | 287 | Write(adrr_droite,5,1,&a); |
leocle | 12:7d61181d7ac7 | 288 | Write(adrr_gauche,5,1,&a); |
leocle | 6:b32a24905cd6 | 289 | |
leocle | 6:b32a24905cd6 | 290 | /* On coupe de le couple du moteur pour pouvoir le faire tourner à la main |
leocle | 6:b32a24905cd6 | 291 | a=0; |
leocle | 6:b32a24905cd6 | 292 | Write(2,24,1,&a); |
leocle | 6:b32a24905cd6 | 293 | Write(1,24,1,&a); |
leocle | 6:b32a24905cd6 | 294 | Write(3,24,1,&a); */ |
leocle | 6:b32a24905cd6 | 295 | |
leocle | 6:b32a24905cd6 | 296 | myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq); |
leocle | 6:b32a24905cd6 | 297 | ticker_1ms.attach(&fonction_1ms,0.001); |
leocle | 6:b32a24905cd6 | 298 | etat=2; |
leocle | 6:b32a24905cd6 | 299 | |
jlesoudeer | 8:922178e5d286 | 300 | coach.printf("fin_initialisation"); |
jlesoudeer | 8:922178e5d286 | 301 | position_1 = 0; |
jlesoudeer | 8:922178e5d286 | 302 | position_2 = 0; |
jlesoudeer | 8:922178e5d286 | 303 | position_3 = 0; |
jlesoudeer | 8:922178e5d286 | 304 | position_1_old = 0; |
jlesoudeer | 8:922178e5d286 | 305 | position_2_old = 0; |
jlesoudeer | 8:922178e5d286 | 306 | position_3_old = 0; |
jlesoudeer | 8:922178e5d286 | 307 | delta_position_1 = 0; |
jlesoudeer | 8:922178e5d286 | 308 | delta_position_2 = 0; |
jlesoudeer | 8:922178e5d286 | 309 | delta_position_3 = 0; |
leocle | 16:1bbf7e4249d7 | 310 | |
leocle | 1:c5e9c436f8d0 | 311 | while (true) { |
leocle | 6:b32a24905cd6 | 312 | //on regarde la position des moteurs |
leocle | 6:b32a24905cd6 | 313 | switch(etat) { |
leocle | 6:b32a24905cd6 | 314 | case 2 : |
leocle | 6:b32a24905cd6 | 315 | if(fm_etat) { |
leocle | 6:b32a24905cd6 | 316 | position_1_old = position_1; |
leocle | 6:b32a24905cd6 | 317 | for(int i=0; i<=7; i++) { |
leocle | 6:b32a24905cd6 | 318 | //On envoi la trame que l'on souhaite |
leocle | 6:b32a24905cd6 | 319 | |
leocle | 6:b32a24905cd6 | 320 | myserial_ax12.putc(trame_demande_pos_1[i]); |
leocle | 6:b32a24905cd6 | 321 | //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures |
leocle | 6:b32a24905cd6 | 322 | } |
leocle | 6:b32a24905cd6 | 323 | fm_etat=0; |
leocle | 6:b32a24905cd6 | 324 | } |
leocle | 6:b32a24905cd6 | 325 | |
leocle | 6:b32a24905cd6 | 326 | if(temps_ms>=3) { |
leocle | 6:b32a24905cd6 | 327 | etat=3; |
leocle | 6:b32a24905cd6 | 328 | fm_etat=1; |
leocle | 6:b32a24905cd6 | 329 | } |
leocle | 6:b32a24905cd6 | 330 | break; |
leocle | 6:b32a24905cd6 | 331 | |
leocle | 6:b32a24905cd6 | 332 | case 3 : |
leocle | 6:b32a24905cd6 | 333 | |
leocle | 6:b32a24905cd6 | 334 | if(fm_etat) { |
leocle | 6:b32a24905cd6 | 335 | delta_position_1 = (int16_t)position_1 - (int16_t)position_1_old; |
leocle | 6:b32a24905cd6 | 336 | if(delta_position_1 > 2048) { |
leocle | 6:b32a24905cd6 | 337 | delta_position_1 = delta_position_1 - 4096; |
leocle | 6:b32a24905cd6 | 338 | } |
leocle | 6:b32a24905cd6 | 339 | |
leocle | 6:b32a24905cd6 | 340 | if(delta_position_1 < -2048) { |
leocle | 6:b32a24905cd6 | 341 | delta_position_1 = delta_position_1 + 4096; |
leocle | 6:b32a24905cd6 | 342 | } |
leocle | 6:b32a24905cd6 | 343 | position_2_old = position_2; |
leocle | 6:b32a24905cd6 | 344 | for(int i=0; i<=7; i++) { |
leocle | 6:b32a24905cd6 | 345 | //On envoi la trame que l'on souhaite |
leocle | 6:b32a24905cd6 | 346 | myserial_ax12.putc(trame_demande_pos_2[i]); |
leocle | 6:b32a24905cd6 | 347 | //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures |
leocle | 6:b32a24905cd6 | 348 | |
leocle | 6:b32a24905cd6 | 349 | } |
leocle | 6:b32a24905cd6 | 350 | fm_etat=0; |
leocle | 6:b32a24905cd6 | 351 | } |
leocle | 6:b32a24905cd6 | 352 | |
leocle | 6:b32a24905cd6 | 353 | if(temps_ms>=6) { |
leocle | 6:b32a24905cd6 | 354 | etat=4; |
leocle | 6:b32a24905cd6 | 355 | fm_etat=1; |
leocle | 6:b32a24905cd6 | 356 | } |
leocle | 6:b32a24905cd6 | 357 | break; |
leocle | 6:b32a24905cd6 | 358 | |
leocle | 6:b32a24905cd6 | 359 | case 4 : |
leocle | 6:b32a24905cd6 | 360 | if(fm_etat) { |
leocle | 6:b32a24905cd6 | 361 | delta_position_2 = (int16_t)position_2 - (int16_t)position_2_old; |
leocle | 6:b32a24905cd6 | 362 | if(delta_position_2 > 2048) { |
leocle | 6:b32a24905cd6 | 363 | delta_position_2 = delta_position_2 - 4096; |
leocle | 6:b32a24905cd6 | 364 | } |
leocle | 6:b32a24905cd6 | 365 | |
leocle | 6:b32a24905cd6 | 366 | if(delta_position_2 < -2048) { |
leocle | 6:b32a24905cd6 | 367 | delta_position_2 = delta_position_2 + 4096; |
leocle | 6:b32a24905cd6 | 368 | } |
leocle | 6:b32a24905cd6 | 369 | position_3_old = position_3; |
leocle | 6:b32a24905cd6 | 370 | for(int i=0; i<=7; i++) { |
leocle | 6:b32a24905cd6 | 371 | //On envoi la trame que l'on souhaite |
leocle | 6:b32a24905cd6 | 372 | myserial_ax12.putc(trame_demande_pos_3[i]); |
leocle | 6:b32a24905cd6 | 373 | |
leocle | 6:b32a24905cd6 | 374 | } |
leocle | 6:b32a24905cd6 | 375 | fm_etat=0; |
leocle | 6:b32a24905cd6 | 376 | } |
leocle | 6:b32a24905cd6 | 377 | |
leocle | 6:b32a24905cd6 | 378 | if(temps_ms>=9) { |
leocle | 6:b32a24905cd6 | 379 | etat=5; |
leocle | 6:b32a24905cd6 | 380 | fm_etat=1; |
leocle | 6:b32a24905cd6 | 381 | } |
leocle | 6:b32a24905cd6 | 382 | break; |
leocle | 6:b32a24905cd6 | 383 | |
leocle | 6:b32a24905cd6 | 384 | case 5 : |
jlesoudeer | 8:922178e5d286 | 385 | if(fm_etat == 1) { |
jlesoudeer | 8:922178e5d286 | 386 | delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old; |
jlesoudeer | 8:922178e5d286 | 387 | if(delta_position_3 > 2048) { |
jlesoudeer | 8:922178e5d286 | 388 | delta_position_3 = delta_position_3 - 4096; |
jlesoudeer | 8:922178e5d286 | 389 | } |
leocle | 6:b32a24905cd6 | 390 | |
jlesoudeer | 8:922178e5d286 | 391 | if(delta_position_3 < -2048) { |
jlesoudeer | 8:922178e5d286 | 392 | delta_position_3 = delta_position_3 + 4096; |
jlesoudeer | 8:922178e5d286 | 393 | } |
jlesoudeer | 8:922178e5d286 | 394 | send_roues(); |
leocle | 6:b32a24905cd6 | 395 | } |
leocle | 6:b32a24905cd6 | 396 | |
jlesoudeer | 8:922178e5d286 | 397 | fm_etat =0; |
jlesoudeer | 8:922178e5d286 | 398 | |
jlesoudeer | 8:922178e5d286 | 399 | if (flag_a_traite == 1) { |
jlesoudeer | 8:922178e5d286 | 400 | etat = 6; |
jlesoudeer | 8:922178e5d286 | 401 | temps_ms = 0; |
jlesoudeer | 8:922178e5d286 | 402 | fm_etat =1; |
leocle | 6:b32a24905cd6 | 403 | } |
jlesoudeer | 8:922178e5d286 | 404 | break; |
jlesoudeer | 8:922178e5d286 | 405 | |
jlesoudeer | 8:922178e5d286 | 406 | case 6 : |
jlesoudeer | 8:922178e5d286 | 407 | if(fm_etat ==1){ |
jlesoudeer | 8:922178e5d286 | 408 | coach.puts("traitement\n"); |
jlesoudeer | 8:922178e5d286 | 409 | if (flag_msg == 0) { |
jlesoudeer | 8:922178e5d286 | 410 | sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); |
jlesoudeer | 8:922178e5d286 | 411 | } else { |
jlesoudeer | 8:922178e5d286 | 412 | sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat ); |
jlesoudeer | 8:922178e5d286 | 413 | } |
jlesoudeer | 8:922178e5d286 | 414 | flag_a_traite = 0; |
leocle | 6:b32a24905cd6 | 415 | temps_ms = 0; |
jlesoudeer | 8:922178e5d286 | 416 | asser(front, lat, rot); |
jlesoudeer | 8:922178e5d286 | 417 | fm_etat = 0; |
jlesoudeer | 8:922178e5d286 | 418 | } |
jlesoudeer | 8:922178e5d286 | 419 | if(temps_ms>= 5) { |
jlesoudeer | 8:922178e5d286 | 420 | |
jlesoudeer | 8:922178e5d286 | 421 | etat = 2; |
jlesoudeer | 8:922178e5d286 | 422 | fm_etat =1; |
jlesoudeer | 8:922178e5d286 | 423 | temps_ms = 0; |
jlesoudeer | 8:922178e5d286 | 424 | } |
leocle | 6:b32a24905cd6 | 425 | break; |
leocle | 6:b32a24905cd6 | 426 | } |
leocle | 1:c5e9c436f8d0 | 427 | } |
leocle | 0:8762773cddf9 | 428 | |
leocle | 2:223d6363315b | 429 | } |
leocle | 6:b32a24905cd6 | 430 | |
leocle | 6:b32a24905cd6 | 431 | void SetCRSpeed(int ID, float speed) |
leocle | 6:b32a24905cd6 | 432 | { |
leocle | 6:b32a24905cd6 | 433 | |
leocle | 6:b32a24905cd6 | 434 | // bit 10 = direction, 0 = CCW, 1=CW |
leocle | 6:b32a24905cd6 | 435 | // bits 9-0 = Speed |
leocle | 6:b32a24905cd6 | 436 | char data[2]; |
leocle | 6:b32a24905cd6 | 437 | |
leocle | 6:b32a24905cd6 | 438 | int goal = (0x3ff * abs(speed)); |
leocle | 6:b32a24905cd6 | 439 | |
leocle | 6:b32a24905cd6 | 440 | // Set direction CW if we have a negative speed |
leocle | 6:b32a24905cd6 | 441 | if (speed < 0) { |
leocle | 6:b32a24905cd6 | 442 | goal |= (0x1 << 10); |
leocle | 6:b32a24905cd6 | 443 | } |
leocle | 6:b32a24905cd6 | 444 | |
leocle | 6:b32a24905cd6 | 445 | data[0] = goal & 0xff; // bottom 8 bits |
leocle | 6:b32a24905cd6 | 446 | data[1] = goal >> 8; // top 8 bits |
leocle | 6:b32a24905cd6 | 447 | |
leocle | 6:b32a24905cd6 | 448 | // write the packet, return the error code |
leocle | 6:b32a24905cd6 | 449 | Write(ID, 0x20, 2, data); |
leocle | 6:b32a24905cd6 | 450 | } |
leocle | 6:b32a24905cd6 | 451 | |
leocle | 6:b32a24905cd6 | 452 | void SetMode(int ID, int mode) |
leocle | 6:b32a24905cd6 | 453 | { |
leocle | 6:b32a24905cd6 | 454 | |
leocle | 6:b32a24905cd6 | 455 | if (mode == 1) { // set CR |
leocle | 6:b32a24905cd6 | 456 | SetCWLimit(ID,0); |
leocle | 6:b32a24905cd6 | 457 | SetCCWLimit(ID,0); |
leocle | 6:b32a24905cd6 | 458 | SetCRSpeed(ID,0.0); |
leocle | 6:b32a24905cd6 | 459 | } else { |
leocle | 6:b32a24905cd6 | 460 | SetCWLimit(ID,0); |
leocle | 6:b32a24905cd6 | 461 | SetCCWLimit(ID,360); |
leocle | 6:b32a24905cd6 | 462 | SetCRSpeed(ID,0.0); |
leocle | 6:b32a24905cd6 | 463 | } |
leocle | 6:b32a24905cd6 | 464 | } |
leocle | 6:b32a24905cd6 | 465 | |
leocle | 6:b32a24905cd6 | 466 | void SetCWLimit (int ID, int degrees) |
leocle | 6:b32a24905cd6 | 467 | { |
leocle | 6:b32a24905cd6 | 468 | |
leocle | 6:b32a24905cd6 | 469 | char data[2]; |
leocle | 6:b32a24905cd6 | 470 | short limit = (4095 * degrees) / 360; |
leocle | 6:b32a24905cd6 | 471 | |
leocle | 6:b32a24905cd6 | 472 | data[0] = limit & 0xff; // bottom 8 bits |
leocle | 6:b32a24905cd6 | 473 | data[1] = limit >> 8; // top 8 bits |
leocle | 6:b32a24905cd6 | 474 | |
leocle | 6:b32a24905cd6 | 475 | Write(ID, 0x06, 2, data); |
leocle | 6:b32a24905cd6 | 476 | } |
leocle | 6:b32a24905cd6 | 477 | |
leocle | 6:b32a24905cd6 | 478 | void SetCCWLimit (int ID, int degrees) |
leocle | 6:b32a24905cd6 | 479 | { |
leocle | 6:b32a24905cd6 | 480 | |
leocle | 6:b32a24905cd6 | 481 | char data[2]; |
leocle | 6:b32a24905cd6 | 482 | short limit = (4095 * degrees) / 360; |
leocle | 6:b32a24905cd6 | 483 | |
leocle | 6:b32a24905cd6 | 484 | data[0] = limit & 0xff; // bottom 8 bits |
leocle | 6:b32a24905cd6 | 485 | data[1] = limit >> 8; // top 8 bits |
leocle | 6:b32a24905cd6 | 486 | |
leocle | 6:b32a24905cd6 | 487 | Write(ID, 0x08, 2, data); |
leocle | 6:b32a24905cd6 | 488 | } |
leocle | 6:b32a24905cd6 | 489 | |
leocle | 6:b32a24905cd6 | 490 | void Write(int ID, int start, int bytes, char* data) |
leocle | 6:b32a24905cd6 | 491 | { |
leocle | 6:b32a24905cd6 | 492 | char TxBuf[16]; |
leocle | 6:b32a24905cd6 | 493 | char sum = 0; |
leocle | 6:b32a24905cd6 | 494 | |
leocle | 6:b32a24905cd6 | 495 | TxBuf[0] = 0xff; |
leocle | 6:b32a24905cd6 | 496 | TxBuf[1] = 0xff; |
leocle | 6:b32a24905cd6 | 497 | |
leocle | 6:b32a24905cd6 | 498 | // ID |
leocle | 6:b32a24905cd6 | 499 | TxBuf[2] = ID; |
leocle | 6:b32a24905cd6 | 500 | sum += TxBuf[2]; |
leocle | 6:b32a24905cd6 | 501 | |
leocle | 6:b32a24905cd6 | 502 | |
leocle | 6:b32a24905cd6 | 503 | // packet Length |
leocle | 6:b32a24905cd6 | 504 | TxBuf[3] = 3+bytes; |
leocle | 6:b32a24905cd6 | 505 | sum += TxBuf[3]; |
leocle | 6:b32a24905cd6 | 506 | |
leocle | 6:b32a24905cd6 | 507 | // Instruction |
leocle | 6:b32a24905cd6 | 508 | TxBuf[4]=0x03; |
leocle | 6:b32a24905cd6 | 509 | sum += TxBuf[4]; |
leocle | 6:b32a24905cd6 | 510 | |
leocle | 6:b32a24905cd6 | 511 | // Start Address |
leocle | 6:b32a24905cd6 | 512 | TxBuf[5] = start; |
leocle | 6:b32a24905cd6 | 513 | sum += TxBuf[5]; |
leocle | 6:b32a24905cd6 | 514 | |
leocle | 6:b32a24905cd6 | 515 | // data |
leocle | 6:b32a24905cd6 | 516 | for (char i=0; i<bytes ; i++) { |
leocle | 6:b32a24905cd6 | 517 | TxBuf[6+i] = data[i]; |
leocle | 6:b32a24905cd6 | 518 | sum += TxBuf[6+i]; |
leocle | 6:b32a24905cd6 | 519 | |
leocle | 6:b32a24905cd6 | 520 | } |
leocle | 6:b32a24905cd6 | 521 | |
leocle | 6:b32a24905cd6 | 522 | // checksum |
leocle | 6:b32a24905cd6 | 523 | TxBuf[6+bytes] = 0xFF - sum; |
leocle | 6:b32a24905cd6 | 524 | |
leocle | 6:b32a24905cd6 | 525 | // Transmit the packet in one burst with no pausing |
leocle | 6:b32a24905cd6 | 526 | for (int i = 0; i < (7 + bytes) ; i++) { |
leocle | 6:b32a24905cd6 | 527 | myserial_ax12.putc(TxBuf[i]); |
leocle | 6:b32a24905cd6 | 528 | } |
leocle | 6:b32a24905cd6 | 529 | |
leocle | 6:b32a24905cd6 | 530 | wait(0.00002); |
leocle | 6:b32a24905cd6 | 531 | |
leocle | 6:b32a24905cd6 | 532 | } |