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@21:edf6d03fdb20, 2019-05-22 (annotated)

Committer:
kyxstark
Date:
Wed May 22 11:19:27 2019 +0000
Revision:
21:edf6d03fdb20
Child:
25:9627700d989f
merge PR/GR;

Who changed what in which revision?

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