Saša Radosavljevic / Mbed 2 deprecated Lecture_os_2

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.

Dependencies:   mbed

main.cpp@9:11b36174b15e, 2020-05-15 (annotated)

Committer:
higepi
Date:
Fri May 15 08:01:30 2020 +0000
Revision:
9:11b36174b15e
Parent:
8:18392048e2cc
Child:
10:72a688ace26d
Fonction de commande vitesse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:d5bd863b2036 1 #include "mbed.h"
bcostm 0:d5bd863b2036 2
higepi 3:ac6eead48a9d 3 //AX12 myax12 (PC_4, PC_5, 1,115200);
higepi 3:ac6eead48a9d 4 Serial myserial_ax12(PC_4,PC_5,115200);
higepi 4:4160376d7d69 5 Serial pc(USBTX,USBRX,115200);
higepi 6:a243c013e519 6 //Serial zigBee(PC_1,PC_0,115200);
higepi 4:4160376d7d69 7
higepi 4:4160376d7d69 8
higepi 4:4160376d7d69 9 DigitalOut my_led1(LED1);
higepi 4:4160376d7d69 10 DigitalOut my_led2(LED2);
higepi 4:4160376d7d69 11 DigitalOut my_led3(LED3);
bcostm 0:d5bd863b2036 12
higepi 9:11b36174b15e 13 void commande_vitesse(char ID, float speed);
higepi 9:11b36174b15e 14
higepi 6:a243c013e519 15 char tata[10] = {};
higepi 4:4160376d7d69 16 int position_1;
higepi 4:4160376d7d69 17 int position_2;
higepi 4:4160376d7d69 18 int position_3;
higepi 6:a243c013e519 19 int num_moteur = 1;
higepi 4:4160376d7d69 20 int vitesse;
higepi 4:4160376d7d69 21 char sens;
higepi 4:4160376d7d69 22
higepi 4:4160376d7d69 23 Ticker ticker_10ms;
higepi 4:4160376d7d69 24
higepi 4:4160376d7d69 25 //Définition des trames de requête pour le moteur 1
higepi 4:4160376d7d69 26 char trame_demande_pos_1[8]={0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
higepi 4:4160376d7d69 27 char trame_demande_pos_2[8]={0xFF,0xFF,0x02,0x4,0x2,0x24,2,0xD1};
higepi 4:4160376d7d69 28 char trame_demande_pos_3[8]={0xFF,0xFF,0x03,0x4,0x2,0x24,2,0xD0};
higepi 4:4160376d7d69 29 char trame_demande_vitesse[8]={0xFF,0xFF,0x01,0x4,0x2,0x28,2,0xCE};
higepi 4:4160376d7d69 30
higepi 4:4160376d7d69 31
higepi 4:4160376d7d69 32 #define acq_vitesse 0
higepi 4:4160376d7d69 33 #define acq_position 1
higepi 4:4160376d7d69 34
higepi 4:4160376d7d69 35 //Fonction d'interruption sur réception de charactère sur liaison série
higepi 3:ac6eead48a9d 36 void Rx_Irq_fonction(void)
bcostm 0:d5bd863b2036 37 {
higepi 4:4160376d7d69 38 my_led1 = !my_led1;
higepi 4:4160376d7d69 39
higepi 4:4160376d7d69 40 static int i_rx=0;
higepi 3:ac6eead48a9d 41 tata[i_rx]=myserial_ax12.getc();
higepi 4:4160376d7d69 42
higepi 4:4160376d7d69 43 if (tata[0]!=0xFF) // On vérifie que le premier et le deuxième octet sont biens 0xFF
higepi 3:ac6eead48a9d 44 i_rx=0;
higepi 3:ac6eead48a9d 45 else i_rx++;
higepi 4:4160376d7d69 46 if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0;
higepi 4:4160376d7d69 47
higepi 4:4160376d7d69 48 //Une fois qu'on sait que la trame est "juste" on récupère le reste
higepi 3:ac6eead48a9d 49 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7)
higepi 3:ac6eead48a9d 50 {
higepi 4:4160376d7d69 51 my_led3 = !my_led3;
higepi 3:ac6eead48a9d 52 i_rx=0;
higepi 4:4160376d7d69 53
higepi 6:a243c013e519 54 if(tata[4] == 0){ //si erreur est nulle
higepi 4:4160376d7d69 55 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
higepi 4:4160376d7d69 56 if (acq_position == 1) position_1 = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 57 /*if (acq_vitesse == 1){
higepi 4:4160376d7d69 58 sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
higepi 4:4160376d7d69 59 vitesse = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 60 }*/
higepi 4:4160376d7d69 61 }
higepi 3:ac6eead48a9d 62 tata[0]=0;
higepi 3:ac6eead48a9d 63 }
higepi 4:4160376d7d69 64
higepi 4:4160376d7d69 65 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7)
higepi 4:4160376d7d69 66 {
higepi 4:4160376d7d69 67 my_led3 = !my_led3;
higepi 4:4160376d7d69 68 i_rx=0;
higepi 4:4160376d7d69 69
higepi 6:a243c013e519 70 if(tata[4]==00){ //si erreur nulle
higepi 4:4160376d7d69 71 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
higepi 4:4160376d7d69 72 if (acq_position == 1) position_2 = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 73 /*if (acq_vitesse == 1){
higepi 4:4160376d7d69 74 sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
higepi 4:4160376d7d69 75 vitesse = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 76 }*/
higepi 4:4160376d7d69 77 }
higepi 4:4160376d7d69 78 tata[0]=0;
higepi 4:4160376d7d69 79 }
higepi 4:4160376d7d69 80
higepi 4:4160376d7d69 81 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7)
higepi 3:ac6eead48a9d 82 {
higepi 4:4160376d7d69 83 my_led3 = !my_led3;
higepi 4:4160376d7d69 84 i_rx=0;
higepi 4:4160376d7d69 85
higepi 6:a243c013e519 86 if(tata[4]==00){ //si erreur nulle
higepi 4:4160376d7d69 87 //On transforme les données en fonction de ce qu'on lit depuis les moteurs
higepi 4:4160376d7d69 88 if (acq_position == 1) position_3 = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 89 /*if (acq_vitesse == 1){
higepi 4:4160376d7d69 90 sens = ((tata[5]+((uint16_t)tata[6]<<8))&0x40)>>10;
higepi 4:4160376d7d69 91 vitesse = tata[5]+((uint16_t)tata[6]<<8);
higepi 4:4160376d7d69 92 }*/
higepi 4:4160376d7d69 93 }
higepi 4:4160376d7d69 94 tata[0]=0;
higepi 4:4160376d7d69 95 }
higepi 4:4160376d7d69 96
higepi 6:a243c013e519 97 if (i_rx>9)
higepi 4:4160376d7d69 98 {
higepi 4:4160376d7d69 99 /*for(int i=0;i<10;i++)
higepi 4:4160376d7d69 100 tata[i]=0;*/
higepi 3:ac6eead48a9d 101 i_rx=0;
higepi 3:ac6eead48a9d 102 }
higepi 3:ac6eead48a9d 103 }
higepi 3:ac6eead48a9d 104
higepi 3:ac6eead48a9d 105 void fonction_10ms(void)
higepi 3:ac6eead48a9d 106 {
higepi 5:d65b158dd93d 107 /*for(int i=0;i<=19;i++)
higepi 4:4160376d7d69 108 {
higepi 4:4160376d7d69 109 pc.printf("%X ",tata[i]);
higepi 4:4160376d7d69 110 }
higepi 5:d65b158dd93d 111 pc.printf("\n\r");*/
higepi 5:d65b158dd93d 112
higepi 4:4160376d7d69 113 for(int i=0;i<=7;i++)
higepi 3:ac6eead48a9d 114 {
higepi 4:4160376d7d69 115 //On envoi la trame que l'on souhaite
higepi 6:a243c013e519 116 if(num_moteur == 1)myserial_ax12.putc(trame_demande_pos_1[i]);
higepi 6:a243c013e519 117 if(num_moteur == 2)myserial_ax12.putc(trame_demande_pos_2[i]);
higepi 6:a243c013e519 118 if(num_moteur == 3)myserial_ax12.putc(trame_demande_pos_3[i]);
higepi 5:d65b158dd93d 119 /*if(acq_vitesse){
higepi 4:4160376d7d69 120 myserial_ax12.putc(trame_demande_vitesse[i]);
higepi 4:4160376d7d69 121 //pc.printf("\n\rvitesse = %d\n\rsens = %d",vitesse,sens);
higepi 5:d65b158dd93d 122 }*/
bcostm 0:d5bd863b2036 123 }
higepi 5:d65b158dd93d 124 num_moteur++;
higepi 8:18392048e2cc 125 if(num_moteur > 3)num_moteur = 1;
bcostm 0:d5bd863b2036 126 }
higepi 3:ac6eead48a9d 127
higepi 3:ac6eead48a9d 128 int main(){
higepi 3:ac6eead48a9d 129 //myax12.SetMode(1); //passage en Continuous rotation
higepi 4:4160376d7d69 130
higepi 9:11b36174b15e 131 //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
higepi 9:11b36174b15e 132 //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
higepi 9:11b36174b15e 133 //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};
higepi 3:ac6eead48a9d 134 float Angle;
higepi 3:ac6eead48a9d 135 short speed;
higepi 3:ac6eead48a9d 136 char data[10];
higepi 3:ac6eead48a9d 137 char s=0;
higepi 3:ac6eead48a9d 138 float temp;
higepi 5:d65b158dd93d 139 int position_old_1=0,i,a;
higepi 5:d65b158dd93d 140 int position_old_2=0;
higepi 5:d65b158dd93d 141 int position_old_3=0;
higepi 3:ac6eead48a9d 142
higepi 4:4160376d7d69 143 pc.printf("test lecture AX12\n\r");
higepi 4:4160376d7d69 144
higepi 3:ac6eead48a9d 145 //myax12.SetCRSpeed(0.05);
higepi 9:11b36174b15e 146 /*for(i=0;i<=8;i++)
higepi 3:ac6eead48a9d 147 {
higepi 5:d65b158dd93d 148 myserial_ax12.putc(trame_cmd_speed_1[i]);
higepi 5:d65b158dd93d 149 }
higepi 5:d65b158dd93d 150 for(i=0;i<=8;i++)
higepi 5:d65b158dd93d 151 {
higepi 5:d65b158dd93d 152 myserial_ax12.putc(trame_cmd_speed_2[i]);
higepi 5:d65b158dd93d 153 }
higepi 5:d65b158dd93d 154 for(i=0;i<=8;i++)
higepi 5:d65b158dd93d 155 {
higepi 5:d65b158dd93d 156 myserial_ax12.putc(trame_cmd_speed_3[i]);
higepi 9:11b36174b15e 157 }*/
higepi 9:11b36174b15e 158
higepi 9:11b36174b15e 159 commande_vitesse(1,0.05);
higepi 9:11b36174b15e 160 commande_vitesse(2,0.05);
higepi 9:11b36174b15e 161 commande_vitesse(3,0.05);
higepi 3:ac6eead48a9d 162
higepi 3:ac6eead48a9d 163 myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
higepi 4:4160376d7d69 164 ticker_10ms.attach(&fonction_10ms,1);
higepi 3:ac6eead48a9d 165
higepi 3:ac6eead48a9d 166 while(1) {
higepi 4:4160376d7d69 167
higepi 5:d65b158dd93d 168
higepi 7:f793a00a6e37 169 if (position_1 != position_old_1)
higepi 7:f793a00a6e37 170 {
higepi 7:f793a00a6e37 171 pc.printf("\n\rposition moteur 1 = %d :", position_1);
higepi 7:f793a00a6e37 172 //pc.printf("\n\rposition moteur 1 = %d\n\r mot recu :",position_1);
higepi 7:f793a00a6e37 173
higepi 7:f793a00a6e37 174 /*for(i=0;i<=7;i++)
higepi 7:f793a00a6e37 175 {
higepi 7:f793a00a6e37 176 pc.printf("%X ",tata[i]);
higepi 7:f793a00a6e37 177 }*/
higepi 7:f793a00a6e37 178 position_old_1 = position_1;
higepi 3:ac6eead48a9d 179 }
higepi 7:f793a00a6e37 180 if (position_2 != position_old_2)
higepi 7:f793a00a6e37 181 {
higepi 7:f793a00a6e37 182 pc.printf("\n\rposition moteur 2 = %d :", position_2);
higepi 7:f793a00a6e37 183
higepi 7:f793a00a6e37 184 position_old_2 = position_2;
higepi 7:f793a00a6e37 185 }
higepi 7:f793a00a6e37 186 if (position_3 != position_old_3)
higepi 7:f793a00a6e37 187 {
higepi 7:f793a00a6e37 188 pc.printf("\n\rposition moteur 3 = %d :", position_3);
higepi 7:f793a00a6e37 189
higepi 7:f793a00a6e37 190 position_old_3 = position_3;
higepi 7:f793a00a6e37 191 }
higepi 7:f793a00a6e37 192
higepi 4:4160376d7d69 193
higepi 4:4160376d7d69 194 }
higepi 3:ac6eead48a9d 195
higepi 3:ac6eead48a9d 196 }
higepi 3:ac6eead48a9d 197
higepi 9:11b36174b15e 198 void commande_vitesse(char ID, float speed){
higepi 9:11b36174b15e 199 char TxBuf[16];
higepi 9:11b36174b15e 200 char sum = 0;
higepi 9:11b36174b15e 201 char data[2];
higepi 9:11b36174b15e 202 int i;
higepi 9:11b36174b15e 203
higepi 9:11b36174b15e 204 int goal = (0x3ff * abs(speed));
higepi 9:11b36174b15e 205
higepi 9:11b36174b15e 206 // Set direction CW if we have a negative speed
higepi 9:11b36174b15e 207 if (speed < 0) {
higepi 9:11b36174b15e 208 goal |= (0x1 << 10);
higepi 9:11b36174b15e 209 }
higepi 9:11b36174b15e 210
higepi 9:11b36174b15e 211
higepi 9:11b36174b15e 212 TxBuf[0] = 0xFF;
higepi 9:11b36174b15e 213 TxBuf[1] = 0xFF;
higepi 9:11b36174b15e 214 TxBuf[2] = ID;
higepi 9:11b36174b15e 215 sum += TxBuf[2];
higepi 9:11b36174b15e 216
higepi 9:11b36174b15e 217 TxBuf[3] = 5;
higepi 9:11b36174b15e 218 sum += TxBuf[3];
higepi 9:11b36174b15e 219
higepi 9:11b36174b15e 220 TxBuf[4] = 0x03;
higepi 9:11b36174b15e 221 sum += TxBuf[4];
higepi 9:11b36174b15e 222
higepi 9:11b36174b15e 223 TxBuf[5] = 0x20;
higepi 9:11b36174b15e 224 sum += TxBuf[5];
higepi 9:11b36174b15e 225
higepi 9:11b36174b15e 226 TxBuf[6] = goal & 0xff; // bottom 8 bits
higepi 9:11b36174b15e 227 sum += TxBuf[6];
higepi 9:11b36174b15e 228 TxBuf[7] = goal >> 8; // top 8 bits
higepi 9:11b36174b15e 229 sum += TxBuf[7];
higepi 9:11b36174b15e 230
higepi 9:11b36174b15e 231 TxBuf[8] = 0xFF - sum;
higepi 9:11b36174b15e 232
higepi 9:11b36174b15e 233 for(i=0;i<=8;i++)
higepi 9:11b36174b15e 234 {
higepi 9:11b36174b15e 235 myserial_ax12.putc(TxBuf[i]);
higepi 9:11b36174b15e 236 }
higepi 9:11b36174b15e 237 }
higepi 9:11b36174b15e 238
higepi 4:4160376d7d69 239 //void set_speed(ID,speed)
higepi 4:4160376d7d69 240 //void demande_lecture_pos(ID)
higepi 3:ac6eead48a9d 241
higepi 3:ac6eead48a9d 242
higepi 3:ac6eead48a9d 243 /*
higepi 3:ac6eead48a9d 244 #include "mbed.h"
higepi 3:ac6eead48a9d 245
higepi 3:ac6eead48a9d 246 //AX12 myax12 (PC_4, PC_5, 1,115200);
higepi 3:ac6eead48a9d 247 Serial myserial_ax12(PC_4,PC_5,115200);
higepi 3:ac6eead48a9d 248 char tata[10];
higepi 3:ac6eead48a9d 249 int position;
higepi 3:ac6eead48a9d 250
higepi 3:ac6eead48a9d 251 void Rx_Irq_fonction(void)
higepi 3:ac6eead48a9d 252 {
higepi 3:ac6eead48a9d 253 static int i_rx;
higepi 3:ac6eead48a9d 254 tata[i_rx]=myserial_ax12.getc();
higepi 3:ac6eead48a9d 255 if (tata[0]!=0xFF)
higepi 3:ac6eead48a9d 256 i_rx=0;
higepi 3:ac6eead48a9d 257 else i_rx++;
higepi 3:ac6eead48a9d 258 if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7)
higepi 3:ac6eead48a9d 259 {
higepi 3:ac6eead48a9d 260 i_rx=0;
higepi 3:ac6eead48a9d 261 if(tata[4]==00) //si erreur est nulle
higepi 3:ac6eead48a9d 262 position = tata[5]+((uint16_t)tata[6]<<8);
higepi 3:ac6eead48a9d 263 tata[0]=0;
higepi 3:ac6eead48a9d 264 }
higepi 3:ac6eead48a9d 265 if (i_rx>9)
higepi 3:ac6eead48a9d 266 {
higepi 3:ac6eead48a9d 267 for(int i=0;i<10;i++)
higepi 3:ac6eead48a9d 268 tata[i]=0;
higepi 3:ac6eead48a9d 269 i_rx=0;
higepi 3:ac6eead48a9d 270 }
higepi 3:ac6eead48a9d 271 }
higepi 3:ac6eead48a9d 272
higepi 3:ac6eead48a9d 273 int main(){
higepi 3:ac6eead48a9d 274 //myax12.SetMode(1); //passage en Continuous rotation
higepi 3:ac6eead48a9d 275 char trame_demande_pos[8]={0xFF,0xFF,0x01,0x4,0x2,0x24,2,0xD2};
higepi 3:ac6eead48a9d 276 char trame_cmd_speed[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
higepi 3:ac6eead48a9d 277 float Angle;
higepi 3:ac6eead48a9d 278 short speed;
higepi 3:ac6eead48a9d 279 char data[10];
higepi 3:ac6eead48a9d 280 char s=0;
higepi 3:ac6eead48a9d 281 float temp;
higepi 3:ac6eead48a9d 282 int position_old=0,i,a;
higepi 3:ac6eead48a9d 283 //myax12.SetCRSpeed(0.05);
higepi 3:ac6eead48a9d 284
higepi 3:ac6eead48a9d 285 for(i=0;i<=8;i++)
higepi 3:ac6eead48a9d 286 {
higepi 3:ac6eead48a9d 287 myserial_ax12.putc(trame_cmd_speed[i]);
higepi 3:ac6eead48a9d 288 }
higepi 3:ac6eead48a9d 289
higepi 3:ac6eead48a9d 290 for(i=0;i<=7;i++)
higepi 3:ac6eead48a9d 291 {
higepi 3:ac6eead48a9d 292 myserial_ax12.putc(trame_demande_pos[i]);
higepi 3:ac6eead48a9d 293 }
higepi 3:ac6eead48a9d 294 myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
higepi 3:ac6eead48a9d 295
higepi 3:ac6eead48a9d 296
higepi 3:ac6eead48a9d 297 while(1) {
higepi 3:ac6eead48a9d 298 if (position != position_old)
higepi 3:ac6eead48a9d 299 {
higepi 3:ac6eead48a9d 300 printf("\n\rposition = %d\n\r mot recu :",position);
higepi 3:ac6eead48a9d 301 for(i=0;i<=7;i++)
higepi 3:ac6eead48a9d 302 {
higepi 3:ac6eead48a9d 303 printf("%X ",tata[i]);
higepi 3:ac6eead48a9d 304 }
higepi 3:ac6eead48a9d 305 position_old=position;
higepi 3:ac6eead48a9d 306 }
higepi 3:ac6eead48a9d 307 wait(0.01);
higepi 3:ac6eead48a9d 308 a++;
higepi 3:ac6eead48a9d 309 if(!(a % 20))
higepi 3:ac6eead48a9d 310 for(i=0;i<=7;i++)
higepi 3:ac6eead48a9d 311 {
higepi 3:ac6eead48a9d 312 myserial_ax12.putc(trame_demande_pos[i]);
higepi 3:ac6eead48a9d 313 }
higepi 3:ac6eead48a9d 314 }
higepi 3:ac6eead48a9d 315
higepi 3:ac6eead48a9d 316 }
higepi 3:ac6eead48a9d 317
higepi 3:ac6eead48a9d 318 void set_speed(ID,speed)*/