CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Asservissement/Asservissement.cpp@67:96f914f92d2d, 2019-05-26 (annotated)

Committer:
Artiom
Date:
Sun May 26 12:49:15 2019 +0000
Revision:
67:96f914f92d2d
Parent:
50:a5361ffeefc8
Child:
73:bf4d6d9db13b
bezier

Who changed what in which revision?

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