![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Asservissement/Asservissement.cpp
- Revision:
- 0:41cc45429aba
- Child:
- 21:d137ec53c3a9
diff -r 000000000000 -r 41cc45429aba Asservissement/Asservissement.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Asservissement/Asservissement.cpp Thu Jan 30 16:45:47 2020 +0000 @@ -0,0 +1,369 @@ + +#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; + can2.write(msgTx); + wait_us(200); +} + +/*********************************************************************************************/ +/* FUNCTION NAME: SendAck */ +/* DESCRIPTION : Envoyer un acknowledge */ +/*********************************************************************************************/ +void SendAck(unsigned short id, unsigned short from) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=2; + msgTx.format=CANStandard; + msgTx.type=CANData; + // from sur 2 octets + msgTx.data[0]=(unsigned char)from; + msgTx.data[1]=(unsigned char)(from>>8); + + can2.write(msgTx); +} + + +void Send2Short(unsigned short id, unsigned short d1, unsigned short d2) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=4; + msgTx.format=CANStandard; + msgTx.type=CANData; + // from sur 2 octets + msgTx.data[0]=(unsigned char)d1; + msgTx.data[1]=(unsigned char)(d1>>8); + msgTx.data[2]=(unsigned char)d2; + msgTx.data[3]=(unsigned char)(d2>>8); + + can2.write(msgTx); +} + +void SendMsgCan(unsigned short id, unsigned char* data, int len) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=len; + msgTx.format=CANStandard; + msgTx.type=CANData; + // from sur 2 octets + for(int i = 0; i<len; i++) + { + msgTx.data[i]=data[i]; + } + + can2.write(msgTx); +} +void SendCharCan(unsigned short id, unsigned char data) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=1; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.data[0]=data; + + can2.write(msgTx); +} + + + +/*********************************************************************************************/ +/* 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; + + can2.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=2; + msgTx.format=CANStandard; + msgTx.type=CANData; + // Angle signé sur 2 octets + msgTx.data[0]=(unsigned char)angle; + msgTx.data[1]=(unsigned char)(angle>>8); + + can2.write(msgTx); +} + + +/*********************************************************************************************/ +/* FUNCTION NAME: GoStraight */ +/* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */ +/* recalage : 0 => pas de recalage */ +/* 1 => recalage en X */ +/* 2 => Recalage en Y */ +/* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */ +/* isEnchainement : Indique si il faut executer l'instruction en enchainement */ +/* 0 => non */ +/* 1 => oui */ +/* 2 => dernière instruction de l'enchainement */ +/*********************************************************************************************/ +void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_RECALAGE; + msgTx.len=6; + msgTx.format=CANStandard; + msgTx.type=CANData; + // x sur 2 octets + msgTx.data[0]=(unsigned char)distance; + msgTx.data[1]=(unsigned char)(distance>>8); + //Recalage sur 1 octet + msgTx.data[2]=recalage; + //Valeur du recalage sur 2 octets + msgTx.data[3]=(unsigned char)newValue; + msgTx.data[4]=(unsigned char)(newValue>>8); + //Enchainement sur 1 octet + msgTx.data[5]=isEnchainement; + + can2.write(msgTx); + //wait_ms(500); +} + +/********************************************************************************************/ +/* FUNCTION NAME: BendRadius */ +/* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */ +/********************************************************************************************/ +void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure + msgTx.len=6; + msgTx.format=CANStandard; + msgTx.type=CANData; + // Rayon sur 2 octets + msgTx.data[0]=(unsigned char)rayon; + msgTx.data[1]=(unsigned char)(rayon>>8); + // Angle signé sur 2 octets + msgTx.data[2]=(unsigned char)angle; + msgTx.data[3]=(unsigned char)(angle>>8); + // Sens signé sur 1 octet + msgTx.data[4]=sens; + // Enchainement sur 1 octet + msgTx.data[5]=enchainement; + + can2.write(msgTx); +} + +void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=canId; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=6; + + // 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); + + can2.write(msgTx); +} + +/****************************************************************************************/ +/* FUNCTION NAME: setAsservissementEtat */ +/* DESCRIPTION : Activer ou désactiver l'asservissement */ +/****************************************************************************************/ +void setAsservissementEtat(unsigned char enable) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_ENABLE; // 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)((enable==0)?0:1); + + can2.write(msgTx); +} + + +/****************************************************************************************/ +/* FUNCTION NAME: SendSpeed accel decel */ +/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ +/****************************************************************************************/ +void SendSpeedAccelDecel (unsigned short vitesse, unsigned short acceleration,unsigned short deceleration) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_CONFIG; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=8; + msgTx.data[0]=(unsigned char)(vitesse&0x00FF); + msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); + + msgTx.data[2]=(unsigned char)(acceleration&0x00FF); + msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8); + + msgTx.data[4]=(unsigned char)(deceleration&0x00FF); + msgTx.data[5]=(unsigned char)((deceleration&0xFF00)>>8); + + msgTx.data[6]=(unsigned char)(acceleration&0x00FF);//cloto + msgTx.data[7]=(unsigned char)((acceleration&0xFF00)>>8);//cloto + + can2.write(msgTx); + +} + +/****************************************************************************************/ +/* FUNCTION NAME: SendSpeed */ +/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ +/****************************************************************************************/ +void SendSpeed (unsigned short vitesse) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_CONFIG_VIT; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=2; + msgTx.data[0]=(unsigned char)(vitesse&0x00FF); + msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); + + can2.write(msgTx); + +} + +/****************************************************************************************/ +/* FUNCTION NAME: SendSpeed */ +/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ +/****************************************************************************************/ +void SendAccel(unsigned short acceleration,unsigned short deceleration) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_CONFIG_ACCEL; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=4; + msgTx.data[0]=(unsigned char)(acceleration&0x00FF); + msgTx.data[1]=(unsigned char)((acceleration&0xFF00)>>8); + + msgTx.data[2]=(unsigned char)(deceleration&0x00FF); + msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8); + + can2.write(msgTx); + +} +void courbeBezier(int nbCourbes, short P1[][2], short C1[][2], short C2[][2], char sens) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_BEZIER; // tx nouvelle position en (x,y,theta) + msgTx.format=CANStandard; + msgTx.type=CANData; + + msgTx.len=2; + + msgTx.data[0]=(unsigned char)nbCourbes; + msgTx.data[1]=sens; + + can2.write(msgTx); + + wait_ms(150); + + + for (int i = 0; i < nbCourbes; i++) + { + msgTx.len=7; + // x sur 2 octets + msgTx.data[0]=(unsigned char)P1[i][0]; + msgTx.data[1]=(unsigned char)(P1[i][0]>>8); + // y sur 2 octets + msgTx.data[2]=(unsigned char)P1[i][1]; + msgTx.data[3]=(unsigned char)(P1[i][1]>>8); + + msgTx.data[4]=(unsigned char)C1[i][0]; + msgTx.data[5]=(unsigned char)(C1[i][0]>>8); + + msgTx.data[6]=(unsigned char)i; + + can2.write(msgTx); + + wait_us(150); + + + msgTx.len=7; + // y sur 2 octets + msgTx.data[0]=(unsigned char)C1[i][1]; + msgTx.data[1]=(unsigned char)(C1[i][1]>>8); + // x sur 2 octets + msgTx.data[2]=(unsigned char)C2[i][0]; + msgTx.data[3]=(unsigned char)(C2[i][0]>>8); + // y sur 2 octets + msgTx.data[4]=(unsigned char)C2[i][1]; + msgTx.data[5]=(unsigned char)(C2[i][1]>>8); + + msgTx.data[6]=(unsigned char)(i + 100);//Bidouille pour envoyer les points en deux trames + + can2.write(msgTx); + + wait_us(150); + } +} + + + +/****************************************************************************************/ +/* FUNCTION NAME: SendSpeedDecel */ +/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */ +/****************************************************************************************/ + /* +void SendSpeedDecel (unsigned short vitesse, unsigned short deceleration) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=ASSERVISSEMENT_CONFIG_DECEL; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=4; + msgTx.data[0]=(unsigned char)(vitesse&0x00FF); + msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8); + msgTx.data[2]=(unsigned char)(deceleration&0x00FF); + msgTx.data[3]=(unsigned char)((deceleration&0xFF00)>>8); + + can2.write(msgTx); + +}*/ + + \ No newline at end of file