CRAC Team / CRAC-Strat_2019

v2019

Dependencies:   CRAC-Strat_2019 SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Dependents:   Codeprincipal_2019 CRAC-Strat_2019

Asservissement/Asservissement.cpp@44:badcbe8766e9, 2019-05-07 (annotated)

Committer:
Artiom
Date:
Tue May 07 17:37:46 2019 +0000
Revision:
44:badcbe8766e9
Parent:
29:41e02746041d
Child:
50:a5361ffeefc8
2019

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