CRAC Team / Mbed 2 deprecated ACRAC_carte_esclave_GROS_ROBOT_2019

Carte esclave gros robot

Dependencies:   mbed Herkulex_Library_2019 ident_crac actions_Pr

Asservissement/Asservissement.cpp@45:11614fc23e53, 2019-05-31 (annotated)

Committer:
marwanesaich
Date:
Fri May 31 20:53:19 2019 +0000
Revision:
45:11614fc23e53
Parent:
39:124575240d09
Child:
47:ced7758fb2ce
NON FONCTIONNEL Test goldenium

Who changed what in which revision?

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