CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Asservissement/Asservissement.cpp@29:41e02746041d, 2018-04-06 (annotated)

Committer:
Sitkah
Date:
Fri Apr 06 13:47:19 2018 +0000
Revision:
29:41e02746041d
Parent:
28:acd18776ed2d
Child:
44:badcbe8766e9
version fusionn?e

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