CRAC Team / Mbed 2 deprecated code_carte_IHM_sans_bug

code de la carte IHM avant les bugs et avant le travail effectué avec Melchior

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Asservissement/Asservissement.cpp@0:41cc45429aba, 2020-01-30 (annotated)

Committer:
gabrieltetar
Date:
Thu Jan 30 16:45:47 2020 +0000
Revision:
0:41cc45429aba
Child:
21:d137ec53c3a9
sqdsq

Who changed what in which revision?

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