CRAC Team / Asservissement

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?

UserRevisionLine numberNew 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 }