CRAC Team / Mbed 2 deprecated carte_esclave_PETIT_ROBOT_2019

carte esclave Petit Robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Asservissement/Asservissement.cpp@46:85208ab1fa12, 2019-05-31 (annotated)

Committer:
Artiom
Date:
Fri May 31 21:03:45 2019 +0000
Revision:
46:85208ab1fa12
Parent:
39:124575240d09 
fork PR

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