code petit robot pour homologation
Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by
Asservissement/Asservissement.cpp
- Committer:
- antbig
- Date:
- 2016-04-13
- Revision:
- 0:ad97421fb1fb
- Child:
- 1:116040d14164
File content as of revision 0:ad97421fb1fb:
#include "Asservissement.h" /*********************************************************************************************************/ /* FUNCTION NAME: SendRawId */ /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ /*********************************************************************************************************/ void SendRawId (unsigned short id) { CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=0; can1.write(msgTx); wait_us(200); } /*********************************************************************************************/ /* FUNCTION NAME: GoToPosition */ /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */ /*********************************************************************************************/ void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens) { //id_to_expect=ACK_CONSIGNE; CANMessage msgTx=CANMessage(); msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta) msgTx.len=7; msgTx.format=CANStandard; msgTx.type=CANData; // x sur 2 octets msgTx.data[0]=(unsigned char)x; msgTx.data[1]=(unsigned char)(x>>8); // y sur 2 octets msgTx.data[2]=(unsigned char)y; msgTx.data[3]=(unsigned char)(y>>8); // theta signé sur 2 octets msgTx.data[4]=(unsigned char)theta; msgTx.data[5]=(unsigned char)(theta>>8); msgTx.data[6]=sens; can1.write(msgTx); } /****************************************************************************************/ /* FUNCTION NAME: Rotate */ /* DESCRIPTION : Transmission CAN correspondant à une rotation */ /****************************************************************************************/ void Rotate (signed short angle) { CANMessage msgTx=CANMessage(); msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot msgTx.len=1; msgTx.format=CANStandard; msgTx.type=CANData; // Angle signé sur 2 octets msgTx.data[0]=(unsigned char)angle; msgTx.data[1]=(unsigned char)(angle>>8); can1.write(msgTx); }