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