CRAC Team / CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT

code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Asservissement/Asservissement.cpp@0:ad97421fb1fb, 2016-04-13 (annotated)

Committer:
antbig
Date:
Wed Apr 13 22:04:54 2016 +0000
Revision:
0:ad97421fb1fb
Child:
1:116040d14164
Ajout interruption fin de match

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 0:ad97421fb1fb 54 msgTx.len=1;
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 0:ad97421fb1fb 62 }