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@13:9c62e263f245, 2019-05-28 (annotated)

Committer:
Wael_H
Date:
Tue May 28 06:48:54 2019 +0000
Revision:
13:9c62e263f245
Parent:
9:2113adf37c66
Child:
16:05665faaa489
Robot cherche, attrape et lance une balle normalement

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