CRAC Team / Mbed 2 deprecated code_carte_IHM_avec_modifs

code avec modifs, programme mit dans les robots pour les derniers matchs

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Asservissement/Asservissement.cpp@37:ee2c72e76d9c, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 16:19:46 2021 +0000
Revision:
37:ee2c72e76d9c
Parent:
21:d137ec53c3a9 
ouais

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabrieltetar 21:d137ec53c3a9 1 #include "global.h"
gabrieltetar 0:41cc45429aba 2
gabrieltetar 0:41cc45429aba 3 /*********************************************************************************************************/
gabrieltetar 0:41cc45429aba 4 /* FUNCTION NAME: SendRawId */
gabrieltetar 0:41cc45429aba 5 /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
gabrieltetar 0:41cc45429aba 6 /*********************************************************************************************************/
gabrieltetar 0:41cc45429aba 7 void SendRawId (unsigned short id)
gabrieltetar 0:41cc45429aba 8 {
gabrieltetar 0:41cc45429aba 9 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 10 msgTx.id=id;
gabrieltetar 0:41cc45429aba 11 msgTx.len=0;
gabrieltetar 0:41cc45429aba 12 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 13 wait_us(200);
gabrieltetar 0:41cc45429aba 14 }
gabrieltetar 0:41cc45429aba 15
gabrieltetar 0:41cc45429aba 16 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 17 /* FUNCTION NAME: SendAck */
gabrieltetar 0:41cc45429aba 18 /* DESCRIPTION : Envoyer un acknowledge */
gabrieltetar 0:41cc45429aba 19 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 20 void SendAck(unsigned short id, unsigned short from)
gabrieltetar 0:41cc45429aba 21 {
gabrieltetar 0:41cc45429aba 22 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 23 msgTx.id=id;
gabrieltetar 0:41cc45429aba 24 msgTx.len=2;
goldmas 37:ee2c72e76d9c 25 // msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 26 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 27 // from sur 2 octets
gabrieltetar 0:41cc45429aba 28 msgTx.data[0]=(unsigned char)from;
gabrieltetar 0:41cc45429aba 29 msgTx.data[1]=(unsigned char)(from>>8);
gabrieltetar 0:41cc45429aba 30
gabrieltetar 0:41cc45429aba 31 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 32 }
gabrieltetar 0:41cc45429aba 33
gabrieltetar 0:41cc45429aba 34
gabrieltetar 0:41cc45429aba 35 void Send2Short(unsigned short id, unsigned short d1, unsigned short d2)
gabrieltetar 0:41cc45429aba 36 {
gabrieltetar 0:41cc45429aba 37 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 38 msgTx.id=id;
gabrieltetar 0:41cc45429aba 39 msgTx.len=4;
goldmas 37:ee2c72e76d9c 40 // msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 41 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 42 // from sur 2 octets
gabrieltetar 0:41cc45429aba 43 msgTx.data[0]=(unsigned char)d1;
gabrieltetar 0:41cc45429aba 44 msgTx.data[1]=(unsigned char)(d1>>8);
gabrieltetar 0:41cc45429aba 45 msgTx.data[2]=(unsigned char)d2;
gabrieltetar 0:41cc45429aba 46 msgTx.data[3]=(unsigned char)(d2>>8);
gabrieltetar 0:41cc45429aba 47
gabrieltetar 0:41cc45429aba 48 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 49 }
gabrieltetar 0:41cc45429aba 50
gabrieltetar 0:41cc45429aba 51 void SendMsgCan(unsigned short id, unsigned char* data, int len)
gabrieltetar 0:41cc45429aba 52 {
gabrieltetar 0:41cc45429aba 53 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 54 msgTx.id=id;
gabrieltetar 0:41cc45429aba 55 msgTx.len=len;
goldmas 37:ee2c72e76d9c 56 // msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 57 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 58 // from sur 2 octets
gabrieltetar 0:41cc45429aba 59 for(int i = 0; i<len; i++)
gabrieltetar 0:41cc45429aba 60 {
gabrieltetar 0:41cc45429aba 61 msgTx.data[i]=data[i];
gabrieltetar 0:41cc45429aba 62 }
gabrieltetar 0:41cc45429aba 63
gabrieltetar 0:41cc45429aba 64 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 65 }
gabrieltetar 0:41cc45429aba 66 void SendCharCan(unsigned short id, unsigned char data)
gabrieltetar 0:41cc45429aba 67 {
gabrieltetar 0:41cc45429aba 68 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 69 msgTx.id=id;
gabrieltetar 0:41cc45429aba 70 msgTx.len=1;
goldmas 37:ee2c72e76d9c 71 // msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 72 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 73 msgTx.data[0]=data;
gabrieltetar 0:41cc45429aba 74
gabrieltetar 0:41cc45429aba 75 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 76 }
gabrieltetar 0:41cc45429aba 77
gabrieltetar 0:41cc45429aba 78
gabrieltetar 0:41cc45429aba 79
gabrieltetar 0:41cc45429aba 80 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 81 /* FUNCTION NAME: GoToPosition */
gabrieltetar 0:41cc45429aba 82 /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
gabrieltetar 0:41cc45429aba 83 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 84 void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
gabrieltetar 0:41cc45429aba 85 {
gabrieltetar 0:41cc45429aba 86 //id_to_expect=ACK_CONSIGNE;
gabrieltetar 0:41cc45429aba 87
gabrieltetar 0:41cc45429aba 88 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 89 msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
gabrieltetar 0:41cc45429aba 90 msgTx.len=7;
gabrieltetar 0:41cc45429aba 91 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 92 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 93 // x sur 2 octets
gabrieltetar 0:41cc45429aba 94 msgTx.data[0]=(unsigned char)x;
gabrieltetar 0:41cc45429aba 95 msgTx.data[1]=(unsigned char)(x>>8);
gabrieltetar 0:41cc45429aba 96 // y sur 2 octets
gabrieltetar 0:41cc45429aba 97 msgTx.data[2]=(unsigned char)y;
gabrieltetar 0:41cc45429aba 98 msgTx.data[3]=(unsigned char)(y>>8);
gabrieltetar 0:41cc45429aba 99 // theta signé sur 2 octets
gabrieltetar 0:41cc45429aba 100 msgTx.data[4]=(unsigned char)theta;
gabrieltetar 0:41cc45429aba 101 msgTx.data[5]=(unsigned char)(theta>>8);
gabrieltetar 0:41cc45429aba 102 msgTx.data[6]=sens;
gabrieltetar 0:41cc45429aba 103
gabrieltetar 0:41cc45429aba 104 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 105 }
gabrieltetar 0:41cc45429aba 106
gabrieltetar 0:41cc45429aba 107 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 108 /* FUNCTION NAME: Rotate */
gabrieltetar 0:41cc45429aba 109 /* DESCRIPTION : Transmission CAN correspondant à une rotation */
gabrieltetar 0:41cc45429aba 110 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 111 void Rotate (signed short angle)
gabrieltetar 0:41cc45429aba 112 {
gabrieltetar 0:41cc45429aba 113 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 114 msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
gabrieltetar 0:41cc45429aba 115 msgTx.len=2;
gabrieltetar 0:41cc45429aba 116 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 117 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 118 // Angle signé sur 2 octets
gabrieltetar 0:41cc45429aba 119 msgTx.data[0]=(unsigned char)angle;
gabrieltetar 0:41cc45429aba 120 msgTx.data[1]=(unsigned char)(angle>>8);
gabrieltetar 0:41cc45429aba 121
gabrieltetar 0:41cc45429aba 122 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 123 }
gabrieltetar 0:41cc45429aba 124
gabrieltetar 0:41cc45429aba 125
gabrieltetar 0:41cc45429aba 126 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 127 /* FUNCTION NAME: GoStraight */
gabrieltetar 0:41cc45429aba 128 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
gabrieltetar 0:41cc45429aba 129 /* recalage : 0 => pas de recalage */
gabrieltetar 0:41cc45429aba 130 /* 1 => recalage en X */
gabrieltetar 0:41cc45429aba 131 /* 2 => Recalage en Y */
gabrieltetar 0:41cc45429aba 132 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
gabrieltetar 0:41cc45429aba 133 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
gabrieltetar 0:41cc45429aba 134 /* 0 => non */
gabrieltetar 0:41cc45429aba 135 /* 1 => oui */
gabrieltetar 0:41cc45429aba 136 /* 2 => dernière instruction de l'enchainement */
gabrieltetar 0:41cc45429aba 137 /*********************************************************************************************/
gabrieltetar 0:41cc45429aba 138 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
gabrieltetar 0:41cc45429aba 139 {
gabrieltetar 0:41cc45429aba 140 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 141 msgTx.id=ASSERVISSEMENT_RECALAGE;
gabrieltetar 0:41cc45429aba 142 msgTx.len=6;
gabrieltetar 0:41cc45429aba 143 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 144 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 145 // x sur 2 octets
gabrieltetar 0:41cc45429aba 146 msgTx.data[0]=(unsigned char)distance;
gabrieltetar 0:41cc45429aba 147 msgTx.data[1]=(unsigned char)(distance>>8);
gabrieltetar 0:41cc45429aba 148 //Recalage sur 1 octet
gabrieltetar 0:41cc45429aba 149 msgTx.data[2]=recalage;
gabrieltetar 0:41cc45429aba 150 //Valeur du recalage sur 2 octets
gabrieltetar 0:41cc45429aba 151 msgTx.data[3]=(unsigned char)newValue;
gabrieltetar 0:41cc45429aba 152 msgTx.data[4]=(unsigned char)(newValue>>8);
gabrieltetar 0:41cc45429aba 153 //Enchainement sur 1 octet
gabrieltetar 0:41cc45429aba 154 msgTx.data[5]=isEnchainement;
gabrieltetar 0:41cc45429aba 155
gabrieltetar 0:41cc45429aba 156 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 157 //wait_ms(500);
gabrieltetar 0:41cc45429aba 158 }
gabrieltetar 0:41cc45429aba 159
gabrieltetar 0:41cc45429aba 160 /********************************************************************************************/
gabrieltetar 0:41cc45429aba 161 /* FUNCTION NAME: BendRadius */
gabrieltetar 0:41cc45429aba 162 /* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */
gabrieltetar 0:41cc45429aba 163 /********************************************************************************************/
gabrieltetar 0:41cc45429aba 164 void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
gabrieltetar 0:41cc45429aba 165 {
gabrieltetar 0:41cc45429aba 166 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 167 msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
gabrieltetar 0:41cc45429aba 168 msgTx.len=6;
gabrieltetar 0:41cc45429aba 169 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 170 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 171 // Rayon sur 2 octets
gabrieltetar 0:41cc45429aba 172 msgTx.data[0]=(unsigned char)rayon;
gabrieltetar 0:41cc45429aba 173 msgTx.data[1]=(unsigned char)(rayon>>8);
gabrieltetar 0:41cc45429aba 174 // Angle signé sur 2 octets
gabrieltetar 0:41cc45429aba 175 msgTx.data[2]=(unsigned char)angle;
gabrieltetar 0:41cc45429aba 176 msgTx.data[3]=(unsigned char)(angle>>8);
gabrieltetar 0:41cc45429aba 177 // Sens signé sur 1 octet
gabrieltetar 0:41cc45429aba 178 msgTx.data[4]=sens;
gabrieltetar 0:41cc45429aba 179 // Enchainement sur 1 octet
gabrieltetar 0:41cc45429aba 180 msgTx.data[5]=enchainement;
gabrieltetar 0:41cc45429aba 181
gabrieltetar 0:41cc45429aba 182 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 183 }
gabrieltetar 0:41cc45429aba 184
gabrieltetar 0:41cc45429aba 185 void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
gabrieltetar 0:41cc45429aba 186 {
gabrieltetar 0:41cc45429aba 187 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 188 msgTx.id=canId;
gabrieltetar 0:41cc45429aba 189 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 190 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 191 msgTx.len=6;
gabrieltetar 0:41cc45429aba 192
gabrieltetar 0:41cc45429aba 193 // x sur 2 octets
gabrieltetar 0:41cc45429aba 194 msgTx.data[0]=(unsigned char)x;
gabrieltetar 0:41cc45429aba 195 msgTx.data[1]=(unsigned char)(x>>8);
gabrieltetar 0:41cc45429aba 196 // y sur 2 octets
gabrieltetar 0:41cc45429aba 197 msgTx.data[2]=(unsigned char)y;
gabrieltetar 0:41cc45429aba 198 msgTx.data[3]=(unsigned char)(y>>8);
gabrieltetar 0:41cc45429aba 199 // theta signé sur 2 octets
gabrieltetar 0:41cc45429aba 200 msgTx.data[4]=(unsigned char)theta;
gabrieltetar 0:41cc45429aba 201 msgTx.data[5]=(unsigned char)(theta>>8);
gabrieltetar 0:41cc45429aba 202
gabrieltetar 0:41cc45429aba 203 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 204 }
gabrieltetar 0:41cc45429aba 205
gabrieltetar 0:41cc45429aba 206 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 207 /* FUNCTION NAME: setAsservissementEtat */
gabrieltetar 0:41cc45429aba 208 /* DESCRIPTION : Activer ou désactiver l'asservissement */
gabrieltetar 0:41cc45429aba 209 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 210 void setAsservissementEtat(unsigned char enable)
gabrieltetar 0:41cc45429aba 211 {
gabrieltetar 0:41cc45429aba 212 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 213 msgTx.id=ASSERVISSEMENT_ENABLE; // Tx rotation autour du centre du robot
gabrieltetar 0:41cc45429aba 214 msgTx.len=1;
gabrieltetar 0:41cc45429aba 215 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 216 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 217 // Angle signé sur 2 octets
gabrieltetar 0:41cc45429aba 218 msgTx.data[0]=(unsigned char)((enable==0)?0:1);
gabrieltetar 0:41cc45429aba 219
gabrieltetar 0:41cc45429aba 220 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 221 }
gabrieltetar 0:41cc45429aba 222
gabrieltetar 0:41cc45429aba 223
gabrieltetar 0:41cc45429aba 224 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 225 /* FUNCTION NAME: SendSpeed accel decel */
gabrieltetar 0:41cc45429aba 226 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
gabrieltetar 0:41cc45429aba 227 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 228 void SendSpeedAccelDecel (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration)
gabrieltetar 0:41cc45429aba 229 {
gabrieltetar 0:41cc45429aba 230 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 231 msgTx.id=ASSERVISSEMENT_CONFIG;
gabrieltetar 0:41cc45429aba 232 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 233 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 234 msgTx.len=8;
gabrieltetar 0:41cc45429aba 235 msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
gabrieltetar 0:41cc45429aba 236 msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 237
gabrieltetar 0:41cc45429aba 238 msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
gabrieltetar 0:41cc45429aba 239 msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 240
gabrieltetar 0:41cc45429aba 241 msgTx.data[4]=(unsigned char)(deceleration&0x00FF);
gabrieltetar 0:41cc45429aba 242 msgTx.data[5]=(unsigned char)((deceleration&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 243
gabrieltetar 0:41cc45429aba 244 msgTx.data[6]=(unsigned char)(acceleration&0x00FF);//cloto
gabrieltetar 0:41cc45429aba 245 msgTx.data[7]=(unsigned char)((acceleration&0xFF00)>>8);//cloto
gabrieltetar 0:41cc45429aba 246
gabrieltetar 0:41cc45429aba 247 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 248
gabrieltetar 0:41cc45429aba 249 }
gabrieltetar 0:41cc45429aba 250
gabrieltetar 0:41cc45429aba 251 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 252 /* FUNCTION NAME: SendSpeed */
gabrieltetar 0:41cc45429aba 253 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
gabrieltetar 0:41cc45429aba 254 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 255 void SendSpeed (unsigned short vitesse)
gabrieltetar 0:41cc45429aba 256 {
gabrieltetar 0:41cc45429aba 257 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 258 msgTx.id=ASSERVISSEMENT_CONFIG_VIT;
gabrieltetar 0:41cc45429aba 259 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 260 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 261 msgTx.len=2;
gabrieltetar 0:41cc45429aba 262 msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
gabrieltetar 0:41cc45429aba 263 msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 264
gabrieltetar 0:41cc45429aba 265 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 266
gabrieltetar 0:41cc45429aba 267 }
gabrieltetar 0:41cc45429aba 268
gabrieltetar 0:41cc45429aba 269 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 270 /* FUNCTION NAME: SendSpeed */
gabrieltetar 0:41cc45429aba 271 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
gabrieltetar 0:41cc45429aba 272 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 273 void SendAccel(unsigned short acceleration,unsigned short deceleration)
gabrieltetar 0:41cc45429aba 274 {
gabrieltetar 0:41cc45429aba 275 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 276 msgTx.id=ASSERVISSEMENT_CONFIG_ACCEL;
gabrieltetar 0:41cc45429aba 277 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 278 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 279 msgTx.len=4;
gabrieltetar 0:41cc45429aba 280 msgTx.data[0]=(unsigned char)(acceleration&0x00FF);
gabrieltetar 0:41cc45429aba 281 msgTx.data[1]=(unsigned char)((acceleration&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 282
gabrieltetar 0:41cc45429aba 283 msgTx.data[2]=(unsigned char)(deceleration&0x00FF);
gabrieltetar 0:41cc45429aba 284 msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 285
gabrieltetar 0:41cc45429aba 286 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 287
gabrieltetar 0:41cc45429aba 288 }
gabrieltetar 0:41cc45429aba 289 void courbeBezier(int nbCourbes, short P1[][2], short C1[][2], short C2[][2], char sens)
gabrieltetar 0:41cc45429aba 290 {
gabrieltetar 0:41cc45429aba 291 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 292 msgTx.id=ASSERVISSEMENT_BEZIER; // tx nouvelle position en (x,y,theta)
gabrieltetar 0:41cc45429aba 293 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 294 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 295
gabrieltetar 0:41cc45429aba 296 msgTx.len=2;
gabrieltetar 0:41cc45429aba 297
gabrieltetar 0:41cc45429aba 298 msgTx.data[0]=(unsigned char)nbCourbes;
gabrieltetar 0:41cc45429aba 299 msgTx.data[1]=sens;
gabrieltetar 0:41cc45429aba 300
gabrieltetar 0:41cc45429aba 301 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 302
gabrieltetar 0:41cc45429aba 303 wait_ms(150);
gabrieltetar 0:41cc45429aba 304
gabrieltetar 0:41cc45429aba 305
gabrieltetar 0:41cc45429aba 306 for (int i = 0; i < nbCourbes; i++)
gabrieltetar 0:41cc45429aba 307 {
gabrieltetar 0:41cc45429aba 308 msgTx.len=7;
gabrieltetar 0:41cc45429aba 309 // x sur 2 octets
gabrieltetar 0:41cc45429aba 310 msgTx.data[0]=(unsigned char)P1[i][0];
gabrieltetar 0:41cc45429aba 311 msgTx.data[1]=(unsigned char)(P1[i][0]>>8);
gabrieltetar 0:41cc45429aba 312 // y sur 2 octets
gabrieltetar 0:41cc45429aba 313 msgTx.data[2]=(unsigned char)P1[i][1];
gabrieltetar 0:41cc45429aba 314 msgTx.data[3]=(unsigned char)(P1[i][1]>>8);
gabrieltetar 0:41cc45429aba 315
gabrieltetar 0:41cc45429aba 316 msgTx.data[4]=(unsigned char)C1[i][0];
gabrieltetar 0:41cc45429aba 317 msgTx.data[5]=(unsigned char)(C1[i][0]>>8);
gabrieltetar 0:41cc45429aba 318
gabrieltetar 0:41cc45429aba 319 msgTx.data[6]=(unsigned char)i;
gabrieltetar 0:41cc45429aba 320
gabrieltetar 0:41cc45429aba 321 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 322
gabrieltetar 0:41cc45429aba 323 wait_us(150);
gabrieltetar 0:41cc45429aba 324
gabrieltetar 0:41cc45429aba 325
gabrieltetar 0:41cc45429aba 326 msgTx.len=7;
gabrieltetar 0:41cc45429aba 327 // y sur 2 octets
gabrieltetar 0:41cc45429aba 328 msgTx.data[0]=(unsigned char)C1[i][1];
gabrieltetar 0:41cc45429aba 329 msgTx.data[1]=(unsigned char)(C1[i][1]>>8);
gabrieltetar 0:41cc45429aba 330 // x sur 2 octets
gabrieltetar 0:41cc45429aba 331 msgTx.data[2]=(unsigned char)C2[i][0];
gabrieltetar 0:41cc45429aba 332 msgTx.data[3]=(unsigned char)(C2[i][0]>>8);
gabrieltetar 0:41cc45429aba 333 // y sur 2 octets
gabrieltetar 0:41cc45429aba 334 msgTx.data[4]=(unsigned char)C2[i][1];
gabrieltetar 0:41cc45429aba 335 msgTx.data[5]=(unsigned char)(C2[i][1]>>8);
gabrieltetar 0:41cc45429aba 336
gabrieltetar 0:41cc45429aba 337 msgTx.data[6]=(unsigned char)(i + 100);//Bidouille pour envoyer les points en deux trames
gabrieltetar 0:41cc45429aba 338
gabrieltetar 0:41cc45429aba 339 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 340
gabrieltetar 0:41cc45429aba 341 wait_us(150);
gabrieltetar 0:41cc45429aba 342 }
gabrieltetar 0:41cc45429aba 343 }
gabrieltetar 0:41cc45429aba 344
gabrieltetar 0:41cc45429aba 345
gabrieltetar 0:41cc45429aba 346
gabrieltetar 0:41cc45429aba 347 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 348 /* FUNCTION NAME: SendSpeedDecel */
gabrieltetar 0:41cc45429aba 349 /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
gabrieltetar 0:41cc45429aba 350 /****************************************************************************************/
gabrieltetar 0:41cc45429aba 351 /*
gabrieltetar 0:41cc45429aba 352 void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration)
gabrieltetar 0:41cc45429aba 353 {
gabrieltetar 0:41cc45429aba 354 CANMessage msgTx=CANMessage();
gabrieltetar 0:41cc45429aba 355 msgTx.id=ASSERVISSEMENT_CONFIG_DECEL;
gabrieltetar 0:41cc45429aba 356 msgTx.format=CANStandard;
gabrieltetar 0:41cc45429aba 357 msgTx.type=CANData;
gabrieltetar 0:41cc45429aba 358 msgTx.len=4;
gabrieltetar 0:41cc45429aba 359 msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
gabrieltetar 0:41cc45429aba 360 msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 361 msgTx.data[2]=(unsigned char)(deceleration&0x00FF);
gabrieltetar 0:41cc45429aba 362 msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8);
gabrieltetar 0:41cc45429aba 363
gabrieltetar 0:41cc45429aba 364 can2.write(msgTx);
gabrieltetar 0:41cc45429aba 365
gabrieltetar 0:41cc45429aba 366 }*/
gabrieltetar 0:41cc45429aba 367
gabrieltetar 0:41cc45429aba 368