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@47:ced7758fb2ce, 2020-03-12 (annotated)

Committer:
maximilienlv
Date:
Thu Mar 12 06:40:56 2020 +0000
Revision:
47:ced7758fb2ce
Parent:
45:11614fc23e53 
optimisation et suppression de fonctions inutiles;

Who changed what in which revision?

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