CRAC Team / CRAC-Strat_2017_avant_2ieme_match

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CRAC-Strat_2017_fin_premier_match by CRAC Team

Asservissement/Asservissement.cpp@1:116040d14164, 2016-04-15 (annotated)

Committer:
antbig
Date:
Fri Apr 15 10:49:40 2016 +0000
Revision:
1:116040d14164
Parent:
0:ad97421fb1fb
Child:
4:88431b537477
Premier test fonctionnel avec le petit robot,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 0:ad97421fb1fb 1 #include "Asservissement.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
antbig 0:ad97421fb1fb 8 void SendRawId (unsigned short id)
antbig 0:ad97421fb1fb 9 {
antbig 0:ad97421fb1fb 10 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 11 msgTx.id=id;
antbig 0:ad97421fb1fb 12 msgTx.len=0;
antbig 0:ad97421fb1fb 13 can1.write(msgTx);
antbig 0:ad97421fb1fb 14 wait_us(200);
antbig 0:ad97421fb1fb 15 }
antbig 0:ad97421fb1fb 16
antbig 0:ad97421fb1fb 17 /*********************************************************************************************/
antbig 0:ad97421fb1fb 18 /* FUNCTION NAME: GoToPosition */
antbig 0:ad97421fb1fb 19 /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
antbig 0:ad97421fb1fb 20 /*********************************************************************************************/
antbig 0:ad97421fb1fb 21
antbig 0:ad97421fb1fb 22 void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
antbig 0:ad97421fb1fb 23 {
antbig 0:ad97421fb1fb 24 //id_to_expect=ACK_CONSIGNE;
antbig 0:ad97421fb1fb 25
antbig 0:ad97421fb1fb 26 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 27 msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
antbig 0:ad97421fb1fb 28 msgTx.len=7;
antbig 0:ad97421fb1fb 29 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 30 msgTx.type=CANData;
antbig 0:ad97421fb1fb 31 // x sur 2 octets
antbig 0:ad97421fb1fb 32 msgTx.data[0]=(unsigned char)x;
antbig 0:ad97421fb1fb 33 msgTx.data[1]=(unsigned char)(x>>8);
antbig 0:ad97421fb1fb 34 // y sur 2 octets
antbig 0:ad97421fb1fb 35 msgTx.data[2]=(unsigned char)y;
antbig 0:ad97421fb1fb 36 msgTx.data[3]=(unsigned char)(y>>8);
antbig 0:ad97421fb1fb 37 // theta signé sur 2 octets
antbig 0:ad97421fb1fb 38 msgTx.data[4]=(unsigned char)theta;
antbig 0:ad97421fb1fb 39 msgTx.data[5]=(unsigned char)(theta>>8);
antbig 0:ad97421fb1fb 40 msgTx.data[6]=sens;
antbig 0:ad97421fb1fb 41
antbig 0:ad97421fb1fb 42 can1.write(msgTx);
antbig 0:ad97421fb1fb 43 }
antbig 0:ad97421fb1fb 44
antbig 0:ad97421fb1fb 45 /****************************************************************************************/
antbig 0:ad97421fb1fb 46 /* FUNCTION NAME: Rotate */
antbig 0:ad97421fb1fb 47 /* DESCRIPTION : Transmission CAN correspondant à une rotation */
antbig 0:ad97421fb1fb 48 /****************************************************************************************/
antbig 0:ad97421fb1fb 49
antbig 0:ad97421fb1fb 50 void Rotate (signed short angle)
antbig 0:ad97421fb1fb 51 {
antbig 0:ad97421fb1fb 52 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 53 msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
antbig 1:116040d14164 54 msgTx.len=2;
antbig 0:ad97421fb1fb 55 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 56 msgTx.type=CANData;
antbig 0:ad97421fb1fb 57 // Angle signé sur 2 octets
antbig 0:ad97421fb1fb 58 msgTx.data[0]=(unsigned char)angle;
antbig 0:ad97421fb1fb 59 msgTx.data[1]=(unsigned char)(angle>>8);
antbig 0:ad97421fb1fb 60
antbig 0:ad97421fb1fb 61 can1.write(msgTx);
antbig 1:116040d14164 62 }
antbig 1:116040d14164 63
antbig 1:116040d14164 64
antbig 1:116040d14164 65 /*********************************************************************************************/
antbig 1:116040d14164 66 /* FUNCTION NAME: GoStraight */
antbig 1:116040d14164 67 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
antbig 1:116040d14164 68 /* recalage : 0 => pas de recalage */
antbig 1:116040d14164 69 /* 1 => recalage en X */
antbig 1:116040d14164 70 /* 2 => Recalage en Y */
antbig 1:116040d14164 71 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
antbig 1:116040d14164 72 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
antbig 1:116040d14164 73 /* 0 => non */
antbig 1:116040d14164 74 /* 1 => oui */
antbig 1:116040d14164 75 /*********************************************************************************************/
antbig 1:116040d14164 76 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
antbig 1:116040d14164 77 {
antbig 1:116040d14164 78 CANMessage msgTx=CANMessage();
antbig 1:116040d14164 79 msgTx.id=ASSERVISSEMENT_RECALAGE;
antbig 1:116040d14164 80 msgTx.len=6;
antbig 1:116040d14164 81 msgTx.format=CANStandard;
antbig 1:116040d14164 82 msgTx.type=CANData;
antbig 1:116040d14164 83 // x sur 2 octets
antbig 1:116040d14164 84 msgTx.data[0]=(unsigned char)distance;
antbig 1:116040d14164 85 msgTx.data[1]=(unsigned char)(distance>>8);
antbig 1:116040d14164 86 //Recalage sur 1 octet
antbig 1:116040d14164 87 msgTx.data[2]=recalage;
antbig 1:116040d14164 88 //Valeur du recalage sur 2 octets
antbig 1:116040d14164 89 msgTx.data[3]=(unsigned char)newValue;
antbig 1:116040d14164 90 msgTx.data[4]=(unsigned char)(newValue>>8);
antbig 1:116040d14164 91 //Enchainement sur 1 octet
antbig 1:116040d14164 92 msgTx.data[5]=isEnchainement;
antbig 1:116040d14164 93
antbig 1:116040d14164 94 can1.write(msgTx);
antbig 0:ad97421fb1fb 95 }