FRC_equipe1 / Mbed 2 deprecated FRC_2019

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

CAN_asser.cpp@17:aae5361ddddf, 2019-06-04 (annotated)

Committer:
theolp
Date:
Tue Jun 04 17:25:49 2019 +0000
Revision:
17:aae5361ddddf
Parent:
16:05665faaa489
Child:
27:e103da412e2b
Odometrie position, dbug position, angles compris entre -180 et 180, fonction updateCAN. Bonne version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wael_H 7:753e901d441b 1 #include "mbed.h"
Wael_H 7:753e901d441b 2 #include "CAN_asser.h"
Wael_H 5:34048faec367 3
Wael_H 7:753e901d441b 4 CANMessage msg_can[256];
Wael_H 5:34048faec367 5 unsigned char pt_lecture_can = 0, pt_ecriture_can = 0;
Wael_H 5:34048faec367 6 int nb_msg_can = 0;
Wael_H 9:2113adf37c66 7
Wael_H 9:2113adf37c66 8 CAN can(PB_8, PB_9,1000000);
Wael_H 13:9c62e263f245 9 //Serial dbug(USBTX, USBRX, 115200); //PC
Wael_H 13:9c62e263f245 10 Serial dbug(PA_0, PA_1, 9600); //Module Bluetooth
Wael_H 13:9c62e263f245 11
Wael_H 13:9c62e263f245 12 /*//Bonus fin de partie
Wael_H 13:9c62e263f245 13 PwmOut bonus(PA_10);
Wael_H 13:9c62e263f245 14
Wael_H 13:9c62e263f245 15 //Disquette
Wael_H 13:9c62e263f245 16 PwmOut Servo_Ballon(PB_7);
Wael_H 13:9c62e263f245 17 PwmOut Mot_Ballon(PB_6);*/
Wael_H 13:9c62e263f245 18
Wael_H 13:9c62e263f245 19 /*dbug.printf("Gauche : %f\n\r",robot.getDist(Laser::Gauche));
Wael_H 13:9c62e263f245 20 dbug.printf("Droit : %f\n\r",robot.getDist(Laser::Droit));
Wael_H 13:9c62e263f245 21 sauter_lignes(25);
Wael_H 13:9c62e263f245 22 wait_ms(1500);*/
Wael_H 13:9c62e263f245 23
theolp 17:aae5361ddddf 24 void sauter_lignes(int nb_lignes)
Wael_H 13:9c62e263f245 25 {
Wael_H 13:9c62e263f245 26 for(unsigned i=0; i<nb_lignes; ++i)
Wael_H 13:9c62e263f245 27 dbug.printf("\n\r");
theolp 17:aae5361ddddf 28 }
Wael_H 7:753e901d441b 29
Wael_H 7:753e901d441b 30 void can_init(void)
Wael_H 7:753e901d441b 31 {
Wael_H 7:753e901d441b 32 can.attach(&isr_can, CAN::RxIrq);
Wael_H 7:753e901d441b 33 }
Wael_H 7:753e901d441b 34
Wael_H 5:34048faec367 35 void isr_can()
Wael_H 5:34048faec367 36 {
Wael_H 5:34048faec367 37 can.read(msg_can[pt_ecriture_can]);
Wael_H 5:34048faec367 38 nb_msg_can++;
Wael_H 5:34048faec367 39 pt_ecriture_can++;
Wael_H 5:34048faec367 40 if(nb_msg_can > 6)
Wael_H 5:34048faec367 41 {
Wael_H 7:753e901d441b 42 //dbug.printf("!!!!!! CAN BUFFER OVERFLOW !!!!!!!!\n");
Wael_H 5:34048faec367 43 pt_lecture_can++;
Wael_H 5:34048faec367 44 nb_msg_can--;
Wael_H 5:34048faec367 45 }
Wael_H 7:753e901d441b 46 }
Wael_H 7:753e901d441b 47
theolp 17:aae5361ddddf 48 void updateCAN(bool& flagFinDpl, short* pos)
Wael_H 5:34048faec367 49 {
Wael_H 5:34048faec367 50 while(nb_msg_can>0)
Wael_H 5:34048faec367 51 {
Wael_H 8:94ecfe411d02 52 if(msg_can[pt_lecture_can].id == 0x111)
Wael_H 8:94ecfe411d02 53 flagFinDpl = true;
theolp 17:aae5361ddddf 54 else if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION)
theolp 17:aae5361ddddf 55 get_pos(msg_can[pt_lecture_can], pos);
theolp 17:aae5361ddddf 56
Wael_H 9:2113adf37c66 57 nb_msg_can--;
Wael_H 9:2113adf37c66 58 pt_lecture_can++;
Wael_H 9:2113adf37c66 59 }
theolp 17:aae5361ddddf 60
theolp 17:aae5361ddddf 61 /* Test permettant de garder un angle compris entre -180 degrés et 180 degrés */
theolp 17:aae5361ddddf 62 if(pos[2] <= -1800)
theolp 17:aae5361ddddf 63 pos[2] = 3600 + pos[2];
theolp 17:aae5361ddddf 64 else if(pos[2] >= 1800)
theolp 17:aae5361ddddf 65 pos[2] = pos[2] - 3600;
Wael_H 7:753e901d441b 66 }
Wael_H 7:753e901d441b 67
Wael_H 5:34048faec367 68 void get_pos(CANMessage msg_pos, short* position)
Wael_H 5:34048faec367 69 {
Wael_H 5:34048faec367 70 memcpy((void *)((void *)&position[0]), &(msg_pos.data[0]), 2);
Wael_H 5:34048faec367 71 memcpy((void *)((void *)&position[1]), &(msg_pos.data[2]), 2);
Wael_H 5:34048faec367 72 memcpy((void *)((void *)&position[2]), &(msg_pos.data[4]), 2);
Wael_H 5:34048faec367 73 }
Wael_H 5:34048faec367 74
Wael_H 5:34048faec367 75 void SendCoefK (unsigned short id, double K)
Wael_H 5:34048faec367 76 {
Wael_H 5:34048faec367 77
Wael_H 5:34048faec367 78 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 79
Wael_H 5:34048faec367 80 msgTx.id=id;
Wael_H 5:34048faec367 81
Wael_H 5:34048faec367 82 msgTx.len=2;
Wael_H 5:34048faec367 83
Wael_H 5:34048faec367 84 msgTx.format=CANStandard;
Wael_H 5:34048faec367 85
Wael_H 5:34048faec367 86 msgTx.type=CANData;
Wael_H 5:34048faec367 87
Wael_H 5:34048faec367 88 // from sur 2 octets
Wael_H 5:34048faec367 89 for(int i=0;i<8;i++)
Wael_H 5:34048faec367 90 {
Wael_H 5:34048faec367 91 memcpy(&(msgTx.data[i]), (void *)((void *)&K + i), 1);
Wael_H 5:34048faec367 92 }
Wael_H 5:34048faec367 93
Wael_H 5:34048faec367 94 can.write(msgTx);
Wael_H 5:34048faec367 95 }
Wael_H 5:34048faec367 96 /*********************************************************************************************************/
Wael_H 5:34048faec367 97
Wael_H 5:34048faec367 98 /* FUNCTION NAME: SendRawId */
Wael_H 5:34048faec367 99 /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
Wael_H 5:34048faec367 100 /*********************************************************************************************************/
Wael_H 5:34048faec367 101 void SendRawId (unsigned short id)
Wael_H 5:34048faec367 102 {
Wael_H 5:34048faec367 103 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 104
Wael_H 5:34048faec367 105 msgTx.id=id;
Wael_H 5:34048faec367 106
Wael_H 5:34048faec367 107 msgTx.len=0;
Wael_H 5:34048faec367 108
Wael_H 5:34048faec367 109 can.write(msgTx);
Wael_H 5:34048faec367 110
Wael_H 5:34048faec367 111 wait_us(200);
Wael_H 5:34048faec367 112 }
Wael_H 5:34048faec367 113
Wael_H 5:34048faec367 114 /*********************************************************************************************/
Wael_H 5:34048faec367 115
Wael_H 5:34048faec367 116 /* FUNCTION NAME: SendAck */
Wael_H 5:34048faec367 117
Wael_H 5:34048faec367 118 /* DESCRIPTION : Envoyer un acknowledge */
Wael_H 5:34048faec367 119 /*********************************************************************************************/
Wael_H 5:34048faec367 120 void SendAck(unsigned short id, unsigned short from)
Wael_H 5:34048faec367 121 {
Wael_H 5:34048faec367 122
Wael_H 5:34048faec367 123 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 124
Wael_H 5:34048faec367 125 msgTx.id=id;
Wael_H 5:34048faec367 126
Wael_H 5:34048faec367 127 msgTx.len=2;
Wael_H 5:34048faec367 128
Wael_H 5:34048faec367 129 msgTx.format=CANStandard;
Wael_H 5:34048faec367 130
Wael_H 5:34048faec367 131 msgTx.type=CANData;
Wael_H 5:34048faec367 132
Wael_H 5:34048faec367 133 // from sur 2 octets
Wael_H 5:34048faec367 134
Wael_H 5:34048faec367 135 msgTx.data[0]=(unsigned char)from;
Wael_H 5:34048faec367 136
Wael_H 5:34048faec367 137 msgTx.data[1]=(unsigned char)(from>>8);
Wael_H 5:34048faec367 138
Wael_H 5:34048faec367 139
Wael_H 5:34048faec367 140 can.write(msgTx);
Wael_H 5:34048faec367 141 }
Wael_H 5:34048faec367 142
Wael_H 5:34048faec367 143 /*********************************************************************************************/
Wael_H 5:34048faec367 144
Wael_H 5:34048faec367 145 /* FUNCTION NAME: GoToPosition */
Wael_H 5:34048faec367 146 /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
Wael_H 5:34048faec367 147 /*********************************************************************************************/
Wael_H 5:34048faec367 148 void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
Wael_H 5:34048faec367 149 {
Wael_H 5:34048faec367 150
Wael_H 5:34048faec367 151 //id_to_expect=ACK_CONSIGNE;
Wael_H 5:34048faec367 152
Wael_H 5:34048faec367 153 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 154
Wael_H 5:34048faec367 155 msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
Wael_H 5:34048faec367 156
Wael_H 5:34048faec367 157 msgTx.len=7;
Wael_H 5:34048faec367 158
Wael_H 5:34048faec367 159 msgTx.format=CANStandard;
Wael_H 5:34048faec367 160
Wael_H 5:34048faec367 161 msgTx.type=CANData;
Wael_H 5:34048faec367 162
Wael_H 5:34048faec367 163 // x sur 2 octets
Wael_H 5:34048faec367 164
Wael_H 5:34048faec367 165 msgTx.data[0]=(unsigned char)x;
Wael_H 5:34048faec367 166
Wael_H 5:34048faec367 167 msgTx.data[1]=(unsigned char)(x>>8);
Wael_H 5:34048faec367 168
Wael_H 5:34048faec367 169 // y sur 2 octets
Wael_H 5:34048faec367 170
Wael_H 5:34048faec367 171 msgTx.data[2]=(unsigned char)y;
Wael_H 5:34048faec367 172
Wael_H 5:34048faec367 173 msgTx.data[3]=(unsigned char)(y>>8);
Wael_H 5:34048faec367 174
Wael_H 5:34048faec367 175 // theta signé sur 2 octets
Wael_H 5:34048faec367 176
Wael_H 5:34048faec367 177 msgTx.data[4]=(unsigned char)theta;
Wael_H 5:34048faec367 178
Wael_H 5:34048faec367 179 msgTx.data[5]=(unsigned char)(theta>>8);
Wael_H 5:34048faec367 180
Wael_H 5:34048faec367 181 msgTx.data[6]=sens;
Wael_H 5:34048faec367 182
Wael_H 5:34048faec367 183
Wael_H 5:34048faec367 184 can.write(msgTx);
Wael_H 5:34048faec367 185 }
Wael_H 5:34048faec367 186
Wael_H 5:34048faec367 187 /****************************************************************************************/
Wael_H 5:34048faec367 188
Wael_H 5:34048faec367 189 /* FUNCTION NAME: Rotate */
Wael_H 5:34048faec367 190
Wael_H 5:34048faec367 191 /* DESCRIPTION : Transmission CAN correspondant à une rotation */
Wael_H 5:34048faec367 192 /****************************************************************************************/
Wael_H 5:34048faec367 193 void Rotate (signed short angle)
Wael_H 5:34048faec367 194 {
Wael_H 5:34048faec367 195
Wael_H 5:34048faec367 196 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 197
Wael_H 5:34048faec367 198 msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
Wael_H 5:34048faec367 199
Wael_H 5:34048faec367 200 msgTx.len=2;
Wael_H 5:34048faec367 201
Wael_H 5:34048faec367 202 msgTx.format=CANStandard;
Wael_H 5:34048faec367 203
Wael_H 5:34048faec367 204 msgTx.type=CANData;
Wael_H 5:34048faec367 205
Wael_H 5:34048faec367 206 // Angle signé sur 2 octets
Wael_H 5:34048faec367 207
Wael_H 5:34048faec367 208 msgTx.data[0]=(unsigned char)angle;
Wael_H 5:34048faec367 209
Wael_H 5:34048faec367 210 msgTx.data[1]=(unsigned char)(angle>>8);
Wael_H 5:34048faec367 211
Wael_H 5:34048faec367 212
Wael_H 5:34048faec367 213 can.write(msgTx);
Wael_H 5:34048faec367 214 }
Wael_H 5:34048faec367 215
Wael_H 5:34048faec367 216
Wael_H 5:34048faec367 217 /*********************************************************************************************/
Wael_H 5:34048faec367 218
Wael_H 5:34048faec367 219 /* FUNCTION NAME: GoStraight */
Wael_H 5:34048faec367 220 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Wael_H 5:34048faec367 221
Wael_H 5:34048faec367 222 /* recalage : 0 => pas de recalage */
Wael_H 5:34048faec367 223
Wael_H 5:34048faec367 224 /* 1 => recalage en X */
Wael_H 5:34048faec367 225
Wael_H 5:34048faec367 226 /* 2 => Recalage en Y */
Wael_H 5:34048faec367 227
Wael_H 5:34048faec367 228 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Wael_H 5:34048faec367 229
Wael_H 5:34048faec367 230 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Wael_H 5:34048faec367 231
Wael_H 5:34048faec367 232 /* 0 => non */
Wael_H 5:34048faec367 233
Wael_H 5:34048faec367 234 /* 1 => oui */
Wael_H 5:34048faec367 235
Wael_H 5:34048faec367 236 /* 2 => dernière instruction de l'enchainement */
Wael_H 5:34048faec367 237 /*********************************************************************************************/
Wael_H 5:34048faec367 238 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Wael_H 5:34048faec367 239 {
Wael_H 5:34048faec367 240
Wael_H 5:34048faec367 241 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 242
Wael_H 5:34048faec367 243 msgTx.id=ASSERVISSEMENT_RECALAGE;
Wael_H 5:34048faec367 244
Wael_H 5:34048faec367 245 msgTx.len=6;
Wael_H 5:34048faec367 246
Wael_H 5:34048faec367 247 msgTx.format=CANStandard;
Wael_H 5:34048faec367 248
Wael_H 5:34048faec367 249 msgTx.type=CANData;
Wael_H 5:34048faec367 250
Wael_H 5:34048faec367 251 // x sur 2 octets
Wael_H 5:34048faec367 252
Wael_H 5:34048faec367 253 msgTx.data[0]=(unsigned char)distance;
Wael_H 5:34048faec367 254
Wael_H 5:34048faec367 255 msgTx.data[1]=(unsigned char)(distance>>8);
Wael_H 5:34048faec367 256
Wael_H 5:34048faec367 257 //Recalage sur 1 octet
Wael_H 5:34048faec367 258
Wael_H 5:34048faec367 259 msgTx.data[2]=recalage;
Wael_H 5:34048faec367 260
Wael_H 5:34048faec367 261 //Valeur du recalage sur 2 octets
Wael_H 5:34048faec367 262
Wael_H 5:34048faec367 263 msgTx.data[3]=(unsigned char)newValue;
Wael_H 5:34048faec367 264
Wael_H 5:34048faec367 265 msgTx.data[4]=(unsigned char)(newValue>>8);
Wael_H 5:34048faec367 266
Wael_H 5:34048faec367 267 //Enchainement sur 1 octet
Wael_H 5:34048faec367 268
Wael_H 5:34048faec367 269 msgTx.data[5]=isEnchainement;
Wael_H 5:34048faec367 270
Wael_H 5:34048faec367 271
Wael_H 5:34048faec367 272 can.write(msgTx);
Wael_H 5:34048faec367 273
Wael_H 5:34048faec367 274 //wait_ms(500);
Wael_H 5:34048faec367 275 }
Wael_H 5:34048faec367 276
Wael_H 5:34048faec367 277 /********************************************************************************************/
Wael_H 5:34048faec367 278
Wael_H 5:34048faec367 279 /* FUNCTION NAME: BendRadius */
Wael_H 5:34048faec367 280
Wael_H 5:34048faec367 281 /* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */
Wael_H 5:34048faec367 282 /********************************************************************************************/
Wael_H 5:34048faec367 283 void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
Wael_H 5:34048faec367 284 {
Wael_H 5:34048faec367 285
Wael_H 5:34048faec367 286 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 287
Wael_H 5:34048faec367 288 msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
Wael_H 5:34048faec367 289
Wael_H 5:34048faec367 290 msgTx.len=6;
Wael_H 5:34048faec367 291
Wael_H 5:34048faec367 292 msgTx.format=CANStandard;
Wael_H 5:34048faec367 293
Wael_H 5:34048faec367 294 msgTx.type=CANData;
Wael_H 5:34048faec367 295
Wael_H 5:34048faec367 296 // Rayon sur 2 octets
Wael_H 5:34048faec367 297
Wael_H 5:34048faec367 298 msgTx.data[0]=(unsigned char)rayon;
Wael_H 5:34048faec367 299
Wael_H 5:34048faec367 300 msgTx.data[1]=(unsigned char)(rayon>>8);
Wael_H 5:34048faec367 301
Wael_H 5:34048faec367 302 // Angle signé sur 2 octets
Wael_H 5:34048faec367 303
Wael_H 5:34048faec367 304 msgTx.data[2]=(unsigned char)angle;
Wael_H 5:34048faec367 305
Wael_H 5:34048faec367 306 msgTx.data[3]=(unsigned char)(angle>>8);
Wael_H 5:34048faec367 307
Wael_H 5:34048faec367 308 // Sens signé sur 1 octet
Wael_H 5:34048faec367 309
Wael_H 5:34048faec367 310 msgTx.data[4]=sens;
Wael_H 5:34048faec367 311
Wael_H 5:34048faec367 312 // Enchainement sur 1 octet
Wael_H 5:34048faec367 313
Wael_H 5:34048faec367 314 msgTx.data[5]=enchainement;
Wael_H 5:34048faec367 315
Wael_H 5:34048faec367 316
Wael_H 5:34048faec367 317 can.write(msgTx);
Wael_H 5:34048faec367 318 }
Wael_H 5:34048faec367 319
Wael_H 5:34048faec367 320 void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
Wael_H 5:34048faec367 321 {
Wael_H 5:34048faec367 322
Wael_H 5:34048faec367 323 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 324
Wael_H 5:34048faec367 325 msgTx.id=canId;
Wael_H 5:34048faec367 326
Wael_H 5:34048faec367 327 msgTx.format=CANStandard;
Wael_H 5:34048faec367 328
Wael_H 5:34048faec367 329 msgTx.type=CANData;
Wael_H 5:34048faec367 330
Wael_H 5:34048faec367 331 msgTx.len=6;
Wael_H 5:34048faec367 332
Wael_H 5:34048faec367 333
Wael_H 5:34048faec367 334 // x sur 2 octets
Wael_H 5:34048faec367 335
Wael_H 5:34048faec367 336 msgTx.data[0]=(unsigned char)x;
Wael_H 5:34048faec367 337
Wael_H 5:34048faec367 338 msgTx.data[1]=(unsigned char)(x>>8);
Wael_H 5:34048faec367 339
Wael_H 5:34048faec367 340 // y sur 2 octets
Wael_H 5:34048faec367 341
Wael_H 5:34048faec367 342 msgTx.data[2]=(unsigned char)y;
Wael_H 5:34048faec367 343
Wael_H 5:34048faec367 344 msgTx.data[3]=(unsigned char)(y>>8);
Wael_H 5:34048faec367 345
Wael_H 5:34048faec367 346 // theta signé sur 2 octets
Wael_H 5:34048faec367 347
Wael_H 5:34048faec367 348 msgTx.data[4]=(unsigned char)theta;
Wael_H 5:34048faec367 349
Wael_H 5:34048faec367 350 msgTx.data[5]=(unsigned char)(theta>>8);
Wael_H 5:34048faec367 351
Wael_H 5:34048faec367 352
Wael_H 5:34048faec367 353 can.write(msgTx);
Wael_H 5:34048faec367 354 }
Wael_H 5:34048faec367 355
Wael_H 5:34048faec367 356 /****************************************************************************************/
Wael_H 5:34048faec367 357
Wael_H 5:34048faec367 358 /* FUNCTION NAME: setAsservissementEtat */
Wael_H 5:34048faec367 359
Wael_H 5:34048faec367 360 /* DESCRIPTION : Activer ou désactiver l'asservissement */
Wael_H 5:34048faec367 361 /****************************************************************************************/
Wael_H 5:34048faec367 362 void setAsservissementEtat(unsigned char enable)
Wael_H 5:34048faec367 363 {
Wael_H 5:34048faec367 364
Wael_H 5:34048faec367 365 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 366
Wael_H 5:34048faec367 367 msgTx.id=ASSERVISSEMENT_ENABLE; // Tx rotation autour du centre du robot
Wael_H 5:34048faec367 368
Wael_H 5:34048faec367 369 msgTx.len=1;
Wael_H 5:34048faec367 370
Wael_H 5:34048faec367 371 msgTx.format=CANStandard;
Wael_H 5:34048faec367 372
Wael_H 5:34048faec367 373 msgTx.type=CANData;
Wael_H 5:34048faec367 374
Wael_H 5:34048faec367 375 // Angle signé sur 2 octets
Wael_H 5:34048faec367 376
Wael_H 5:34048faec367 377 msgTx.data[0]=(unsigned char)((enable==0)?0:1);
Wael_H 5:34048faec367 378
Wael_H 5:34048faec367 379
Wael_H 5:34048faec367 380 can.write(msgTx);
Wael_H 5:34048faec367 381 }
Wael_H 5:34048faec367 382
Wael_H 5:34048faec367 383
Wael_H 5:34048faec367 384 /****************************************************************************************/
Wael_H 5:34048faec367 385
Wael_H 5:34048faec367 386 /* FUNCTION NAME: SendSpeed */
Wael_H 5:34048faec367 387
Wael_H 5:34048faec367 388 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
Wael_H 5:34048faec367 389 /****************************************************************************************/
Wael_H 5:34048faec367 390 void SendSpeed (unsigned short vitesse, unsigned short acceleration)
Wael_H 5:34048faec367 391 {
Wael_H 5:34048faec367 392
Wael_H 5:34048faec367 393 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 394
Wael_H 5:34048faec367 395 msgTx.id=ASSERVISSEMENT_CONFIG;
Wael_H 5:34048faec367 396
Wael_H 5:34048faec367 397 msgTx.format=CANStandard;
Wael_H 5:34048faec367 398
Wael_H 5:34048faec367 399 msgTx.type=CANData;
Wael_H 5:34048faec367 400
Wael_H 5:34048faec367 401 msgTx.len=4;
Wael_H 5:34048faec367 402
Wael_H 5:34048faec367 403 msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
Wael_H 5:34048faec367 404
Wael_H 5:34048faec367 405 msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
Wael_H 5:34048faec367 406
Wael_H 5:34048faec367 407 msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
Wael_H 5:34048faec367 408
Wael_H 5:34048faec367 409
Wael_H 5:34048faec367 410 msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
Wael_H 5:34048faec367 411
Wael_H 5:34048faec367 412
Wael_H 5:34048faec367 413 can.write(msgTx);
Wael_H 5:34048faec367 414
Wael_H 5:34048faec367 415
Wael_H 5:34048faec367 416 }
Wael_H 5:34048faec367 417
Wael_H 5:34048faec367 418 /****************************************************************************************/
Wael_H 5:34048faec367 419
Wael_H 5:34048faec367 420 /* FUNCTION NAME: SendSpeedDecel */
Wael_H 5:34048faec367 421
Wael_H 5:34048faec367 422 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
Wael_H 5:34048faec367 423 /****************************************************************************************/
Wael_H 5:34048faec367 424
Wael_H 5:34048faec367 425 void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration)
Wael_H 5:34048faec367 426 {
Wael_H 5:34048faec367 427
Wael_H 5:34048faec367 428 CANMessage msgTx=CANMessage();
Wael_H 5:34048faec367 429
Wael_H 5:34048faec367 430 msgTx.id=ASSERVISSEMENT_CONFIG_DECEL;
Wael_H 5:34048faec367 431
Wael_H 5:34048faec367 432 msgTx.format=CANStandard;
Wael_H 5:34048faec367 433
Wael_H 5:34048faec367 434 msgTx.type=CANData;
Wael_H 5:34048faec367 435
Wael_H 5:34048faec367 436 msgTx.len=4;
Wael_H 5:34048faec367 437
Wael_H 5:34048faec367 438 msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
Wael_H 5:34048faec367 439
Wael_H 5:34048faec367 440 msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
Wael_H 5:34048faec367 441
Wael_H 5:34048faec367 442 msgTx.data[2]=(unsigned char)(deceleration&0x00FF);
Wael_H 5:34048faec367 443
Wael_H 5:34048faec367 444 msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8);
Wael_H 5:34048faec367 445
Wael_H 5:34048faec367 446 can.write(msgTx);
Wael_H 5:34048faec367 447
Wael_H 5:34048faec367 448
Wael_H 5:34048faec367 449 }