Biblio à utiliser pour communiquer avec la carte moteur
Dependents: Asserv_mot Asserv_mot
Asservissement.cpp@1:162aa6608ffe, 2017-04-19 (annotated)
- Committer:
- Brand101
- Date:
- Wed Apr 19 17:41:35 2017 +0000
- Revision:
- 1:162aa6608ffe
- Parent:
- 0:3a1f19e51eb2
- Child:
- 2:dc3ec8d77b6a
B1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kkoichy | 0:3a1f19e51eb2 | 1 | #include "Asservissement.h" |
Brand101 | 1:162aa6608ffe | 2 | #include "My_ident.h" |
kkoichy | 0:3a1f19e51eb2 | 3 | |
kkoichy | 0:3a1f19e51eb2 | 4 | /*********************************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 5 | /* FUNCTION NAME: SendRawId */ |
kkoichy | 0:3a1f19e51eb2 | 6 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
kkoichy | 0:3a1f19e51eb2 | 7 | /*********************************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 8 | void SendRawId (unsigned short id) |
kkoichy | 0:3a1f19e51eb2 | 9 | { |
kkoichy | 0:3a1f19e51eb2 | 10 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 11 | msgTx.id=id; |
kkoichy | 0:3a1f19e51eb2 | 12 | msgTx.len=0; |
kkoichy | 0:3a1f19e51eb2 | 13 | can1.write(msgTx); |
Brand101 | 1:162aa6608ffe | 14 | //wait_us(200); |
kkoichy | 0:3a1f19e51eb2 | 15 | } |
kkoichy | 0:3a1f19e51eb2 | 16 | |
kkoichy | 0:3a1f19e51eb2 | 17 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 18 | /* FUNCTION NAME: SendAck */ |
kkoichy | 0:3a1f19e51eb2 | 19 | /* DESCRIPTION : Envoyer un acknowledge */ |
kkoichy | 0:3a1f19e51eb2 | 20 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 21 | void SendAck(unsigned short id, unsigned short from) |
kkoichy | 0:3a1f19e51eb2 | 22 | { |
kkoichy | 0:3a1f19e51eb2 | 23 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 24 | msgTx.id=id; |
kkoichy | 0:3a1f19e51eb2 | 25 | msgTx.len=2; |
kkoichy | 0:3a1f19e51eb2 | 26 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 27 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 28 | // from sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 29 | msgTx.data[0]=(unsigned char)from; |
kkoichy | 0:3a1f19e51eb2 | 30 | msgTx.data[1]=(unsigned char)(from>>8); |
kkoichy | 0:3a1f19e51eb2 | 31 | |
kkoichy | 0:3a1f19e51eb2 | 32 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 33 | } |
kkoichy | 0:3a1f19e51eb2 | 34 | |
kkoichy | 0:3a1f19e51eb2 | 35 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 36 | /* FUNCTION NAME: GoToPosition */ |
kkoichy | 0:3a1f19e51eb2 | 37 | /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */ |
kkoichy | 0:3a1f19e51eb2 | 38 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 39 | void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens) |
kkoichy | 0:3a1f19e51eb2 | 40 | { |
kkoichy | 0:3a1f19e51eb2 | 41 | //id_to_expect=ACK_CONSIGNE; |
kkoichy | 0:3a1f19e51eb2 | 42 | |
kkoichy | 0:3a1f19e51eb2 | 43 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 44 | msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta) |
kkoichy | 0:3a1f19e51eb2 | 45 | msgTx.len=7; |
kkoichy | 0:3a1f19e51eb2 | 46 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 47 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 48 | // x sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 49 | msgTx.data[0]=(unsigned char)x; |
kkoichy | 0:3a1f19e51eb2 | 50 | msgTx.data[1]=(unsigned char)(x>>8); |
kkoichy | 0:3a1f19e51eb2 | 51 | // y sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 52 | msgTx.data[2]=(unsigned char)y; |
kkoichy | 0:3a1f19e51eb2 | 53 | msgTx.data[3]=(unsigned char)(y>>8); |
kkoichy | 0:3a1f19e51eb2 | 54 | // theta signé sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 55 | msgTx.data[4]=(unsigned char)theta; |
kkoichy | 0:3a1f19e51eb2 | 56 | msgTx.data[5]=(unsigned char)(theta>>8); |
kkoichy | 0:3a1f19e51eb2 | 57 | msgTx.data[6]=sens; |
kkoichy | 0:3a1f19e51eb2 | 58 | |
kkoichy | 0:3a1f19e51eb2 | 59 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 60 | } |
kkoichy | 0:3a1f19e51eb2 | 61 | |
kkoichy | 0:3a1f19e51eb2 | 62 | /****************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 63 | /* FUNCTION NAME: Rotate */ |
kkoichy | 0:3a1f19e51eb2 | 64 | /* DESCRIPTION : Transmission CAN correspondant à une rotation */ |
kkoichy | 0:3a1f19e51eb2 | 65 | /****************************************************************************************/ |
Brand101 | 1:162aa6608ffe | 66 | void Rotate (signed short angle) |
kkoichy | 0:3a1f19e51eb2 | 67 | { |
kkoichy | 0:3a1f19e51eb2 | 68 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 69 | msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot |
kkoichy | 0:3a1f19e51eb2 | 70 | msgTx.len=2; |
kkoichy | 0:3a1f19e51eb2 | 71 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 72 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 73 | // Angle signé sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 74 | msgTx.data[0]=(unsigned char)angle; |
kkoichy | 0:3a1f19e51eb2 | 75 | msgTx.data[1]=(unsigned char)(angle>>8); |
kkoichy | 0:3a1f19e51eb2 | 76 | |
kkoichy | 0:3a1f19e51eb2 | 77 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 78 | } |
kkoichy | 0:3a1f19e51eb2 | 79 | |
kkoichy | 0:3a1f19e51eb2 | 80 | |
kkoichy | 0:3a1f19e51eb2 | 81 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 82 | /* FUNCTION NAME: GoStraight */ |
kkoichy | 0:3a1f19e51eb2 | 83 | /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */ |
kkoichy | 0:3a1f19e51eb2 | 84 | /* recalage : 0 => pas de recalage */ |
kkoichy | 0:3a1f19e51eb2 | 85 | /* 1 => recalage en X */ |
kkoichy | 0:3a1f19e51eb2 | 86 | /* 2 => Recalage en Y */ |
kkoichy | 0:3a1f19e51eb2 | 87 | /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */ |
kkoichy | 0:3a1f19e51eb2 | 88 | /* isEnchainement : Indique si il faut executer l'instruction en enchainement */ |
kkoichy | 0:3a1f19e51eb2 | 89 | /* 0 => non */ |
kkoichy | 0:3a1f19e51eb2 | 90 | /* 1 => oui */ |
kkoichy | 0:3a1f19e51eb2 | 91 | /* 2 => dernière instruction de l'enchainement */ |
kkoichy | 0:3a1f19e51eb2 | 92 | /*********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 93 | void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement) |
kkoichy | 0:3a1f19e51eb2 | 94 | { |
kkoichy | 0:3a1f19e51eb2 | 95 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 96 | msgTx.id=ASSERVISSEMENT_RECALAGE; |
kkoichy | 0:3a1f19e51eb2 | 97 | msgTx.len=6; |
kkoichy | 0:3a1f19e51eb2 | 98 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 99 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 100 | // x sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 101 | msgTx.data[0]=(unsigned char)distance; |
kkoichy | 0:3a1f19e51eb2 | 102 | msgTx.data[1]=(unsigned char)(distance>>8); |
kkoichy | 0:3a1f19e51eb2 | 103 | //Recalage sur 1 octet |
kkoichy | 0:3a1f19e51eb2 | 104 | msgTx.data[2]=recalage; |
kkoichy | 0:3a1f19e51eb2 | 105 | //Valeur du recalage sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 106 | msgTx.data[3]=(unsigned char)newValue; |
kkoichy | 0:3a1f19e51eb2 | 107 | msgTx.data[4]=(unsigned char)(newValue>>8); |
kkoichy | 0:3a1f19e51eb2 | 108 | //Enchainement sur 1 octet |
kkoichy | 0:3a1f19e51eb2 | 109 | msgTx.data[5]=isEnchainement; |
kkoichy | 0:3a1f19e51eb2 | 110 | |
kkoichy | 0:3a1f19e51eb2 | 111 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 112 | //wait_ms(500); |
kkoichy | 0:3a1f19e51eb2 | 113 | } |
kkoichy | 0:3a1f19e51eb2 | 114 | |
kkoichy | 0:3a1f19e51eb2 | 115 | /********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 116 | /* FUNCTION NAME: BendRadius */ |
kkoichy | 0:3a1f19e51eb2 | 117 | /* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */ |
kkoichy | 0:3a1f19e51eb2 | 118 | /********************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 119 | void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement) |
kkoichy | 0:3a1f19e51eb2 | 120 | { |
kkoichy | 0:3a1f19e51eb2 | 121 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 122 | msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure |
kkoichy | 0:3a1f19e51eb2 | 123 | msgTx.len=6; |
kkoichy | 0:3a1f19e51eb2 | 124 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 125 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 126 | // Rayon sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 127 | msgTx.data[0]=(unsigned char)rayon; |
kkoichy | 0:3a1f19e51eb2 | 128 | msgTx.data[1]=(unsigned char)(rayon>>8); |
kkoichy | 0:3a1f19e51eb2 | 129 | // Angle signé sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 130 | msgTx.data[2]=(unsigned char)angle; |
kkoichy | 0:3a1f19e51eb2 | 131 | msgTx.data[3]=(unsigned char)(angle>>8); |
kkoichy | 0:3a1f19e51eb2 | 132 | // Sens signé sur 1 octet |
kkoichy | 0:3a1f19e51eb2 | 133 | msgTx.data[4]=sens; |
kkoichy | 0:3a1f19e51eb2 | 134 | // Enchainement sur 1 octet |
kkoichy | 0:3a1f19e51eb2 | 135 | msgTx.data[5]=enchainement; |
kkoichy | 0:3a1f19e51eb2 | 136 | |
kkoichy | 0:3a1f19e51eb2 | 137 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 138 | } |
kkoichy | 0:3a1f19e51eb2 | 139 | |
kkoichy | 0:3a1f19e51eb2 | 140 | void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta) |
kkoichy | 0:3a1f19e51eb2 | 141 | { |
kkoichy | 0:3a1f19e51eb2 | 142 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 143 | msgTx.id=canId; |
kkoichy | 0:3a1f19e51eb2 | 144 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 145 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 146 | msgTx.len=6; |
kkoichy | 0:3a1f19e51eb2 | 147 | |
kkoichy | 0:3a1f19e51eb2 | 148 | // x sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 149 | msgTx.data[0]=(unsigned char)x; |
kkoichy | 0:3a1f19e51eb2 | 150 | msgTx.data[1]=(unsigned char)(x>>8); |
kkoichy | 0:3a1f19e51eb2 | 151 | // y sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 152 | msgTx.data[2]=(unsigned char)y; |
kkoichy | 0:3a1f19e51eb2 | 153 | msgTx.data[3]=(unsigned char)(y>>8); |
kkoichy | 0:3a1f19e51eb2 | 154 | // theta signé sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 155 | msgTx.data[4]=(unsigned char)theta; |
kkoichy | 0:3a1f19e51eb2 | 156 | msgTx.data[5]=(unsigned char)(theta>>8); |
kkoichy | 0:3a1f19e51eb2 | 157 | |
kkoichy | 0:3a1f19e51eb2 | 158 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 159 | } |
kkoichy | 0:3a1f19e51eb2 | 160 | |
kkoichy | 0:3a1f19e51eb2 | 161 | /****************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 162 | /* FUNCTION NAME: setAsservissementEtat */ |
kkoichy | 0:3a1f19e51eb2 | 163 | /* DESCRIPTION : Activer ou désactiver l'asservissement */ |
kkoichy | 0:3a1f19e51eb2 | 164 | /****************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 165 | void setAsservissementEtat(unsigned char enable) |
kkoichy | 0:3a1f19e51eb2 | 166 | { |
kkoichy | 0:3a1f19e51eb2 | 167 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 168 | msgTx.id=ASSERVISSEMENT_ENABLE; // Tx rotation autour du centre du robot |
kkoichy | 0:3a1f19e51eb2 | 169 | msgTx.len=1; |
kkoichy | 0:3a1f19e51eb2 | 170 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 171 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 172 | // Angle signé sur 2 octets |
kkoichy | 0:3a1f19e51eb2 | 173 | msgTx.data[0]=(unsigned char)((enable==0)?0:1); |
kkoichy | 0:3a1f19e51eb2 | 174 | |
kkoichy | 0:3a1f19e51eb2 | 175 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 176 | } |
kkoichy | 0:3a1f19e51eb2 | 177 | |
kkoichy | 0:3a1f19e51eb2 | 178 | |
kkoichy | 0:3a1f19e51eb2 | 179 | /****************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 180 | /* FUNCTION NAME: SendSpeed */ |
kkoichy | 0:3a1f19e51eb2 | 181 | /* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ |
kkoichy | 0:3a1f19e51eb2 | 182 | /****************************************************************************************/ |
kkoichy | 0:3a1f19e51eb2 | 183 | void SendSpeed (unsigned short vitesse, unsigned short acceleration) |
kkoichy | 0:3a1f19e51eb2 | 184 | { |
kkoichy | 0:3a1f19e51eb2 | 185 | CANMessage msgTx=CANMessage(); |
kkoichy | 0:3a1f19e51eb2 | 186 | msgTx.id=ASSERVISSEMENT_CONFIG; |
kkoichy | 0:3a1f19e51eb2 | 187 | msgTx.format=CANStandard; |
kkoichy | 0:3a1f19e51eb2 | 188 | msgTx.type=CANData; |
kkoichy | 0:3a1f19e51eb2 | 189 | msgTx.len=4; |
kkoichy | 0:3a1f19e51eb2 | 190 | msgTx.data[0]=(unsigned char)(vitesse&0x00FF); |
kkoichy | 0:3a1f19e51eb2 | 191 | msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); |
kkoichy | 0:3a1f19e51eb2 | 192 | msgTx.data[2]=(unsigned char)(acceleration&0x00FF); |
kkoichy | 0:3a1f19e51eb2 | 193 | msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8); |
kkoichy | 0:3a1f19e51eb2 | 194 | |
kkoichy | 0:3a1f19e51eb2 | 195 | can1.write(msgTx); |
kkoichy | 0:3a1f19e51eb2 | 196 | } |