![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Asservissement/Asservissement.cpp@0:41cc45429aba, 2020-01-30 (annotated)
- Committer:
- gabrieltetar
- Date:
- Thu Jan 30 16:45:47 2020 +0000
- Revision:
- 0:41cc45429aba
- Child:
- 21:d137ec53c3a9
sqdsq
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gabrieltetar | 0:41cc45429aba | 1 | |
gabrieltetar | 0:41cc45429aba | 2 | #include "Asservissement.h" |
gabrieltetar | 0:41cc45429aba | 3 | |
gabrieltetar | 0:41cc45429aba | 4 | /*********************************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 5 | /* FUNCTION NAME: SendRawId */ |
gabrieltetar | 0:41cc45429aba | 6 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
gabrieltetar | 0:41cc45429aba | 7 | /*********************************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 8 | void SendRawId (unsigned short id) |
gabrieltetar | 0:41cc45429aba | 9 | { |
gabrieltetar | 0:41cc45429aba | 10 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 11 | msgTx.id=id; |
gabrieltetar | 0:41cc45429aba | 12 | msgTx.len=0; |
gabrieltetar | 0:41cc45429aba | 13 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 14 | wait_us(200); |
gabrieltetar | 0:41cc45429aba | 15 | } |
gabrieltetar | 0:41cc45429aba | 16 | |
gabrieltetar | 0:41cc45429aba | 17 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 18 | /* FUNCTION NAME: SendAck */ |
gabrieltetar | 0:41cc45429aba | 19 | /* DESCRIPTION : Envoyer un acknowledge */ |
gabrieltetar | 0:41cc45429aba | 20 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 21 | void SendAck(unsigned short id, unsigned short from) |
gabrieltetar | 0:41cc45429aba | 22 | { |
gabrieltetar | 0:41cc45429aba | 23 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 24 | msgTx.id=id; |
gabrieltetar | 0:41cc45429aba | 25 | msgTx.len=2; |
gabrieltetar | 0:41cc45429aba | 26 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 27 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 28 | // from sur 2 octets |
gabrieltetar | 0:41cc45429aba | 29 | msgTx.data[0]=(unsigned char)from; |
gabrieltetar | 0:41cc45429aba | 30 | msgTx.data[1]=(unsigned char)(from>>8); |
gabrieltetar | 0:41cc45429aba | 31 | |
gabrieltetar | 0:41cc45429aba | 32 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 33 | } |
gabrieltetar | 0:41cc45429aba | 34 | |
gabrieltetar | 0:41cc45429aba | 35 | |
gabrieltetar | 0:41cc45429aba | 36 | void Send2Short(unsigned short id, unsigned short d1, unsigned short d2) |
gabrieltetar | 0:41cc45429aba | 37 | { |
gabrieltetar | 0:41cc45429aba | 38 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 39 | msgTx.id=id; |
gabrieltetar | 0:41cc45429aba | 40 | msgTx.len=4; |
gabrieltetar | 0:41cc45429aba | 41 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 42 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 43 | // from sur 2 octets |
gabrieltetar | 0:41cc45429aba | 44 | msgTx.data[0]=(unsigned char)d1; |
gabrieltetar | 0:41cc45429aba | 45 | msgTx.data[1]=(unsigned char)(d1>>8); |
gabrieltetar | 0:41cc45429aba | 46 | msgTx.data[2]=(unsigned char)d2; |
gabrieltetar | 0:41cc45429aba | 47 | msgTx.data[3]=(unsigned char)(d2>>8); |
gabrieltetar | 0:41cc45429aba | 48 | |
gabrieltetar | 0:41cc45429aba | 49 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 50 | } |
gabrieltetar | 0:41cc45429aba | 51 | |
gabrieltetar | 0:41cc45429aba | 52 | void SendMsgCan(unsigned short id, unsigned char* data, int len) |
gabrieltetar | 0:41cc45429aba | 53 | { |
gabrieltetar | 0:41cc45429aba | 54 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 55 | msgTx.id=id; |
gabrieltetar | 0:41cc45429aba | 56 | msgTx.len=len; |
gabrieltetar | 0:41cc45429aba | 57 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 58 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 59 | // from sur 2 octets |
gabrieltetar | 0:41cc45429aba | 60 | for(int i = 0; i<len; i++) |
gabrieltetar | 0:41cc45429aba | 61 | { |
gabrieltetar | 0:41cc45429aba | 62 | msgTx.data[i]=data[i]; |
gabrieltetar | 0:41cc45429aba | 63 | } |
gabrieltetar | 0:41cc45429aba | 64 | |
gabrieltetar | 0:41cc45429aba | 65 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 66 | } |
gabrieltetar | 0:41cc45429aba | 67 | void SendCharCan(unsigned short id, unsigned char data) |
gabrieltetar | 0:41cc45429aba | 68 | { |
gabrieltetar | 0:41cc45429aba | 69 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 70 | msgTx.id=id; |
gabrieltetar | 0:41cc45429aba | 71 | msgTx.len=1; |
gabrieltetar | 0:41cc45429aba | 72 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 73 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 74 | msgTx.data[0]=data; |
gabrieltetar | 0:41cc45429aba | 75 | |
gabrieltetar | 0:41cc45429aba | 76 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 77 | } |
gabrieltetar | 0:41cc45429aba | 78 | |
gabrieltetar | 0:41cc45429aba | 79 | |
gabrieltetar | 0:41cc45429aba | 80 | |
gabrieltetar | 0:41cc45429aba | 81 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 82 | /* FUNCTION NAME: GoToPosition */ |
gabrieltetar | 0:41cc45429aba | 83 | /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */ |
gabrieltetar | 0:41cc45429aba | 84 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 85 | void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens) |
gabrieltetar | 0:41cc45429aba | 86 | { |
gabrieltetar | 0:41cc45429aba | 87 | //id_to_expect=ACK_CONSIGNE; |
gabrieltetar | 0:41cc45429aba | 88 | |
gabrieltetar | 0:41cc45429aba | 89 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 90 | msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta) |
gabrieltetar | 0:41cc45429aba | 91 | msgTx.len=7; |
gabrieltetar | 0:41cc45429aba | 92 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 93 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 94 | // x sur 2 octets |
gabrieltetar | 0:41cc45429aba | 95 | msgTx.data[0]=(unsigned char)x; |
gabrieltetar | 0:41cc45429aba | 96 | msgTx.data[1]=(unsigned char)(x>>8); |
gabrieltetar | 0:41cc45429aba | 97 | // y sur 2 octets |
gabrieltetar | 0:41cc45429aba | 98 | msgTx.data[2]=(unsigned char)y; |
gabrieltetar | 0:41cc45429aba | 99 | msgTx.data[3]=(unsigned char)(y>>8); |
gabrieltetar | 0:41cc45429aba | 100 | // theta signé sur 2 octets |
gabrieltetar | 0:41cc45429aba | 101 | msgTx.data[4]=(unsigned char)theta; |
gabrieltetar | 0:41cc45429aba | 102 | msgTx.data[5]=(unsigned char)(theta>>8); |
gabrieltetar | 0:41cc45429aba | 103 | msgTx.data[6]=sens; |
gabrieltetar | 0:41cc45429aba | 104 | |
gabrieltetar | 0:41cc45429aba | 105 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 106 | } |
gabrieltetar | 0:41cc45429aba | 107 | |
gabrieltetar | 0:41cc45429aba | 108 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 109 | /* FUNCTION NAME: Rotate */ |
gabrieltetar | 0:41cc45429aba | 110 | /* DESCRIPTION : Transmission CAN correspondant à une rotation */ |
gabrieltetar | 0:41cc45429aba | 111 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 112 | void Rotate (signed short angle) |
gabrieltetar | 0:41cc45429aba | 113 | { |
gabrieltetar | 0:41cc45429aba | 114 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 115 | msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot |
gabrieltetar | 0:41cc45429aba | 116 | msgTx.len=2; |
gabrieltetar | 0:41cc45429aba | 117 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 118 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 119 | // Angle signé sur 2 octets |
gabrieltetar | 0:41cc45429aba | 120 | msgTx.data[0]=(unsigned char)angle; |
gabrieltetar | 0:41cc45429aba | 121 | msgTx.data[1]=(unsigned char)(angle>>8); |
gabrieltetar | 0:41cc45429aba | 122 | |
gabrieltetar | 0:41cc45429aba | 123 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 124 | } |
gabrieltetar | 0:41cc45429aba | 125 | |
gabrieltetar | 0:41cc45429aba | 126 | |
gabrieltetar | 0:41cc45429aba | 127 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 128 | /* FUNCTION NAME: GoStraight */ |
gabrieltetar | 0:41cc45429aba | 129 | /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */ |
gabrieltetar | 0:41cc45429aba | 130 | /* recalage : 0 => pas de recalage */ |
gabrieltetar | 0:41cc45429aba | 131 | /* 1 => recalage en X */ |
gabrieltetar | 0:41cc45429aba | 132 | /* 2 => Recalage en Y */ |
gabrieltetar | 0:41cc45429aba | 133 | /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */ |
gabrieltetar | 0:41cc45429aba | 134 | /* isEnchainement : Indique si il faut executer l'instruction en enchainement */ |
gabrieltetar | 0:41cc45429aba | 135 | /* 0 => non */ |
gabrieltetar | 0:41cc45429aba | 136 | /* 1 => oui */ |
gabrieltetar | 0:41cc45429aba | 137 | /* 2 => dernière instruction de l'enchainement */ |
gabrieltetar | 0:41cc45429aba | 138 | /*********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 139 | void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement) |
gabrieltetar | 0:41cc45429aba | 140 | { |
gabrieltetar | 0:41cc45429aba | 141 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 142 | msgTx.id=ASSERVISSEMENT_RECALAGE; |
gabrieltetar | 0:41cc45429aba | 143 | msgTx.len=6; |
gabrieltetar | 0:41cc45429aba | 144 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 145 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 146 | // x sur 2 octets |
gabrieltetar | 0:41cc45429aba | 147 | msgTx.data[0]=(unsigned char)distance; |
gabrieltetar | 0:41cc45429aba | 148 | msgTx.data[1]=(unsigned char)(distance>>8); |
gabrieltetar | 0:41cc45429aba | 149 | //Recalage sur 1 octet |
gabrieltetar | 0:41cc45429aba | 150 | msgTx.data[2]=recalage; |
gabrieltetar | 0:41cc45429aba | 151 | //Valeur du recalage sur 2 octets |
gabrieltetar | 0:41cc45429aba | 152 | msgTx.data[3]=(unsigned char)newValue; |
gabrieltetar | 0:41cc45429aba | 153 | msgTx.data[4]=(unsigned char)(newValue>>8); |
gabrieltetar | 0:41cc45429aba | 154 | //Enchainement sur 1 octet |
gabrieltetar | 0:41cc45429aba | 155 | msgTx.data[5]=isEnchainement; |
gabrieltetar | 0:41cc45429aba | 156 | |
gabrieltetar | 0:41cc45429aba | 157 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 158 | //wait_ms(500); |
gabrieltetar | 0:41cc45429aba | 159 | } |
gabrieltetar | 0:41cc45429aba | 160 | |
gabrieltetar | 0:41cc45429aba | 161 | /********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 162 | /* FUNCTION NAME: BendRadius */ |
gabrieltetar | 0:41cc45429aba | 163 | /* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */ |
gabrieltetar | 0:41cc45429aba | 164 | /********************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 165 | void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement) |
gabrieltetar | 0:41cc45429aba | 166 | { |
gabrieltetar | 0:41cc45429aba | 167 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 168 | msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure |
gabrieltetar | 0:41cc45429aba | 169 | msgTx.len=6; |
gabrieltetar | 0:41cc45429aba | 170 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 171 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 172 | // Rayon sur 2 octets |
gabrieltetar | 0:41cc45429aba | 173 | msgTx.data[0]=(unsigned char)rayon; |
gabrieltetar | 0:41cc45429aba | 174 | msgTx.data[1]=(unsigned char)(rayon>>8); |
gabrieltetar | 0:41cc45429aba | 175 | // Angle signé sur 2 octets |
gabrieltetar | 0:41cc45429aba | 176 | msgTx.data[2]=(unsigned char)angle; |
gabrieltetar | 0:41cc45429aba | 177 | msgTx.data[3]=(unsigned char)(angle>>8); |
gabrieltetar | 0:41cc45429aba | 178 | // Sens signé sur 1 octet |
gabrieltetar | 0:41cc45429aba | 179 | msgTx.data[4]=sens; |
gabrieltetar | 0:41cc45429aba | 180 | // Enchainement sur 1 octet |
gabrieltetar | 0:41cc45429aba | 181 | msgTx.data[5]=enchainement; |
gabrieltetar | 0:41cc45429aba | 182 | |
gabrieltetar | 0:41cc45429aba | 183 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 184 | } |
gabrieltetar | 0:41cc45429aba | 185 | |
gabrieltetar | 0:41cc45429aba | 186 | void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta) |
gabrieltetar | 0:41cc45429aba | 187 | { |
gabrieltetar | 0:41cc45429aba | 188 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 189 | msgTx.id=canId; |
gabrieltetar | 0:41cc45429aba | 190 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 191 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 192 | msgTx.len=6; |
gabrieltetar | 0:41cc45429aba | 193 | |
gabrieltetar | 0:41cc45429aba | 194 | // x sur 2 octets |
gabrieltetar | 0:41cc45429aba | 195 | msgTx.data[0]=(unsigned char)x; |
gabrieltetar | 0:41cc45429aba | 196 | msgTx.data[1]=(unsigned char)(x>>8); |
gabrieltetar | 0:41cc45429aba | 197 | // y sur 2 octets |
gabrieltetar | 0:41cc45429aba | 198 | msgTx.data[2]=(unsigned char)y; |
gabrieltetar | 0:41cc45429aba | 199 | msgTx.data[3]=(unsigned char)(y>>8); |
gabrieltetar | 0:41cc45429aba | 200 | // theta signé sur 2 octets |
gabrieltetar | 0:41cc45429aba | 201 | msgTx.data[4]=(unsigned char)theta; |
gabrieltetar | 0:41cc45429aba | 202 | msgTx.data[5]=(unsigned char)(theta>>8); |
gabrieltetar | 0:41cc45429aba | 203 | |
gabrieltetar | 0:41cc45429aba | 204 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 205 | } |
gabrieltetar | 0:41cc45429aba | 206 | |
gabrieltetar | 0:41cc45429aba | 207 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 208 | /* FUNCTION NAME: setAsservissementEtat */ |
gabrieltetar | 0:41cc45429aba | 209 | /* DESCRIPTION : Activer ou désactiver l'asservissement */ |
gabrieltetar | 0:41cc45429aba | 210 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 211 | void setAsservissementEtat(unsigned char enable) |
gabrieltetar | 0:41cc45429aba | 212 | { |
gabrieltetar | 0:41cc45429aba | 213 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 214 | msgTx.id=ASSERVISSEMENT_ENABLE; // Tx rotation autour du centre du robot |
gabrieltetar | 0:41cc45429aba | 215 | msgTx.len=1; |
gabrieltetar | 0:41cc45429aba | 216 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 217 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 218 | // Angle signé sur 2 octets |
gabrieltetar | 0:41cc45429aba | 219 | msgTx.data[0]=(unsigned char)((enable==0)?0:1); |
gabrieltetar | 0:41cc45429aba | 220 | |
gabrieltetar | 0:41cc45429aba | 221 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 222 | } |
gabrieltetar | 0:41cc45429aba | 223 | |
gabrieltetar | 0:41cc45429aba | 224 | |
gabrieltetar | 0:41cc45429aba | 225 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 226 | /* FUNCTION NAME: SendSpeed accel decel */ |
gabrieltetar | 0:41cc45429aba | 227 | /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ |
gabrieltetar | 0:41cc45429aba | 228 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 229 | void SendSpeedAccelDecel (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration) |
gabrieltetar | 0:41cc45429aba | 230 | { |
gabrieltetar | 0:41cc45429aba | 231 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 232 | msgTx.id=ASSERVISSEMENT_CONFIG; |
gabrieltetar | 0:41cc45429aba | 233 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 234 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 235 | msgTx.len=8; |
gabrieltetar | 0:41cc45429aba | 236 | msgTx.data[0]=(unsigned char)(vitesse&0x00FF); |
gabrieltetar | 0:41cc45429aba | 237 | msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 238 | |
gabrieltetar | 0:41cc45429aba | 239 | msgTx.data[2]=(unsigned char)(acceleration&0x00FF); |
gabrieltetar | 0:41cc45429aba | 240 | msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 241 | |
gabrieltetar | 0:41cc45429aba | 242 | msgTx.data[4]=(unsigned char)(deceleration&0x00FF); |
gabrieltetar | 0:41cc45429aba | 243 | msgTx.data[5]=(unsigned char)((deceleration&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 244 | |
gabrieltetar | 0:41cc45429aba | 245 | msgTx.data[6]=(unsigned char)(acceleration&0x00FF);//cloto |
gabrieltetar | 0:41cc45429aba | 246 | msgTx.data[7]=(unsigned char)((acceleration&0xFF00)>>8);//cloto |
gabrieltetar | 0:41cc45429aba | 247 | |
gabrieltetar | 0:41cc45429aba | 248 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 249 | |
gabrieltetar | 0:41cc45429aba | 250 | } |
gabrieltetar | 0:41cc45429aba | 251 | |
gabrieltetar | 0:41cc45429aba | 252 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 253 | /* FUNCTION NAME: SendSpeed */ |
gabrieltetar | 0:41cc45429aba | 254 | /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ |
gabrieltetar | 0:41cc45429aba | 255 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 256 | void SendSpeed (unsigned short vitesse) |
gabrieltetar | 0:41cc45429aba | 257 | { |
gabrieltetar | 0:41cc45429aba | 258 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 259 | msgTx.id=ASSERVISSEMENT_CONFIG_VIT; |
gabrieltetar | 0:41cc45429aba | 260 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 261 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 262 | msgTx.len=2; |
gabrieltetar | 0:41cc45429aba | 263 | msgTx.data[0]=(unsigned char)(vitesse&0x00FF); |
gabrieltetar | 0:41cc45429aba | 264 | msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 265 | |
gabrieltetar | 0:41cc45429aba | 266 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 267 | |
gabrieltetar | 0:41cc45429aba | 268 | } |
gabrieltetar | 0:41cc45429aba | 269 | |
gabrieltetar | 0:41cc45429aba | 270 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 271 | /* FUNCTION NAME: SendSpeed */ |
gabrieltetar | 0:41cc45429aba | 272 | /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ |
gabrieltetar | 0:41cc45429aba | 273 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 274 | void SendAccel(unsigned short acceleration,unsigned short deceleration) |
gabrieltetar | 0:41cc45429aba | 275 | { |
gabrieltetar | 0:41cc45429aba | 276 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 277 | msgTx.id=ASSERVISSEMENT_CONFIG_ACCEL; |
gabrieltetar | 0:41cc45429aba | 278 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 279 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 280 | msgTx.len=4; |
gabrieltetar | 0:41cc45429aba | 281 | msgTx.data[0]=(unsigned char)(acceleration&0x00FF); |
gabrieltetar | 0:41cc45429aba | 282 | msgTx.data[1]=(unsigned char)((acceleration&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 283 | |
gabrieltetar | 0:41cc45429aba | 284 | msgTx.data[2]=(unsigned char)(deceleration&0x00FF); |
gabrieltetar | 0:41cc45429aba | 285 | msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 286 | |
gabrieltetar | 0:41cc45429aba | 287 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 288 | |
gabrieltetar | 0:41cc45429aba | 289 | } |
gabrieltetar | 0:41cc45429aba | 290 | void courbeBezier(int nbCourbes, short P1[][2], short C1[][2], short C2[][2], char sens) |
gabrieltetar | 0:41cc45429aba | 291 | { |
gabrieltetar | 0:41cc45429aba | 292 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 293 | msgTx.id=ASSERVISSEMENT_BEZIER; // tx nouvelle position en (x,y,theta) |
gabrieltetar | 0:41cc45429aba | 294 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 295 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 296 | |
gabrieltetar | 0:41cc45429aba | 297 | msgTx.len=2; |
gabrieltetar | 0:41cc45429aba | 298 | |
gabrieltetar | 0:41cc45429aba | 299 | msgTx.data[0]=(unsigned char)nbCourbes; |
gabrieltetar | 0:41cc45429aba | 300 | msgTx.data[1]=sens; |
gabrieltetar | 0:41cc45429aba | 301 | |
gabrieltetar | 0:41cc45429aba | 302 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 303 | |
gabrieltetar | 0:41cc45429aba | 304 | wait_ms(150); |
gabrieltetar | 0:41cc45429aba | 305 | |
gabrieltetar | 0:41cc45429aba | 306 | |
gabrieltetar | 0:41cc45429aba | 307 | for (int i = 0; i < nbCourbes; i++) |
gabrieltetar | 0:41cc45429aba | 308 | { |
gabrieltetar | 0:41cc45429aba | 309 | msgTx.len=7; |
gabrieltetar | 0:41cc45429aba | 310 | // x sur 2 octets |
gabrieltetar | 0:41cc45429aba | 311 | msgTx.data[0]=(unsigned char)P1[i][0]; |
gabrieltetar | 0:41cc45429aba | 312 | msgTx.data[1]=(unsigned char)(P1[i][0]>>8); |
gabrieltetar | 0:41cc45429aba | 313 | // y sur 2 octets |
gabrieltetar | 0:41cc45429aba | 314 | msgTx.data[2]=(unsigned char)P1[i][1]; |
gabrieltetar | 0:41cc45429aba | 315 | msgTx.data[3]=(unsigned char)(P1[i][1]>>8); |
gabrieltetar | 0:41cc45429aba | 316 | |
gabrieltetar | 0:41cc45429aba | 317 | msgTx.data[4]=(unsigned char)C1[i][0]; |
gabrieltetar | 0:41cc45429aba | 318 | msgTx.data[5]=(unsigned char)(C1[i][0]>>8); |
gabrieltetar | 0:41cc45429aba | 319 | |
gabrieltetar | 0:41cc45429aba | 320 | msgTx.data[6]=(unsigned char)i; |
gabrieltetar | 0:41cc45429aba | 321 | |
gabrieltetar | 0:41cc45429aba | 322 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 323 | |
gabrieltetar | 0:41cc45429aba | 324 | wait_us(150); |
gabrieltetar | 0:41cc45429aba | 325 | |
gabrieltetar | 0:41cc45429aba | 326 | |
gabrieltetar | 0:41cc45429aba | 327 | msgTx.len=7; |
gabrieltetar | 0:41cc45429aba | 328 | // y sur 2 octets |
gabrieltetar | 0:41cc45429aba | 329 | msgTx.data[0]=(unsigned char)C1[i][1]; |
gabrieltetar | 0:41cc45429aba | 330 | msgTx.data[1]=(unsigned char)(C1[i][1]>>8); |
gabrieltetar | 0:41cc45429aba | 331 | // x sur 2 octets |
gabrieltetar | 0:41cc45429aba | 332 | msgTx.data[2]=(unsigned char)C2[i][0]; |
gabrieltetar | 0:41cc45429aba | 333 | msgTx.data[3]=(unsigned char)(C2[i][0]>>8); |
gabrieltetar | 0:41cc45429aba | 334 | // y sur 2 octets |
gabrieltetar | 0:41cc45429aba | 335 | msgTx.data[4]=(unsigned char)C2[i][1]; |
gabrieltetar | 0:41cc45429aba | 336 | msgTx.data[5]=(unsigned char)(C2[i][1]>>8); |
gabrieltetar | 0:41cc45429aba | 337 | |
gabrieltetar | 0:41cc45429aba | 338 | msgTx.data[6]=(unsigned char)(i + 100);//Bidouille pour envoyer les points en deux trames |
gabrieltetar | 0:41cc45429aba | 339 | |
gabrieltetar | 0:41cc45429aba | 340 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 341 | |
gabrieltetar | 0:41cc45429aba | 342 | wait_us(150); |
gabrieltetar | 0:41cc45429aba | 343 | } |
gabrieltetar | 0:41cc45429aba | 344 | } |
gabrieltetar | 0:41cc45429aba | 345 | |
gabrieltetar | 0:41cc45429aba | 346 | |
gabrieltetar | 0:41cc45429aba | 347 | |
gabrieltetar | 0:41cc45429aba | 348 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 349 | /* FUNCTION NAME: SendSpeedDecel */ |
gabrieltetar | 0:41cc45429aba | 350 | /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ |
gabrieltetar | 0:41cc45429aba | 351 | /****************************************************************************************/ |
gabrieltetar | 0:41cc45429aba | 352 | /* |
gabrieltetar | 0:41cc45429aba | 353 | void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration) |
gabrieltetar | 0:41cc45429aba | 354 | { |
gabrieltetar | 0:41cc45429aba | 355 | CANMessage msgTx=CANMessage(); |
gabrieltetar | 0:41cc45429aba | 356 | msgTx.id=ASSERVISSEMENT_CONFIG_DECEL; |
gabrieltetar | 0:41cc45429aba | 357 | msgTx.format=CANStandard; |
gabrieltetar | 0:41cc45429aba | 358 | msgTx.type=CANData; |
gabrieltetar | 0:41cc45429aba | 359 | msgTx.len=4; |
gabrieltetar | 0:41cc45429aba | 360 | msgTx.data[0]=(unsigned char)(vitesse&0x00FF); |
gabrieltetar | 0:41cc45429aba | 361 | msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 362 | msgTx.data[2]=(unsigned char)(deceleration&0x00FF); |
gabrieltetar | 0:41cc45429aba | 363 | msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8); |
gabrieltetar | 0:41cc45429aba | 364 | |
gabrieltetar | 0:41cc45429aba | 365 | can2.write(msgTx); |
gabrieltetar | 0:41cc45429aba | 366 | |
gabrieltetar | 0:41cc45429aba | 367 | }*/ |
gabrieltetar | 0:41cc45429aba | 368 | |
gabrieltetar | 0:41cc45429aba | 369 |