CRAC Team / CRAC-Strat_2017_fin_premier_match

code de start qui marche a la fin du premier match, base pour la suite

Fork of CRAC-Strat_2017_homologation_petit_rob by CRAC Team

Asservissement/Asservissement.cpp@4:88431b537477, 2016-04-17 (annotated)

Committer:
antbig
Date:
Sun Apr 17 14:03:03 2016 +0000
Revision:
4:88431b537477
Parent:
1:116040d14164
Child:
5:dcd817534b57
Ajout possibilit? de choisir id strat via CAN

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