Asservissement.cpp

00001 #include "Asservissement.h"
00002 
00003 /*********************************************************************************************************/
00004 /* FUNCTION NAME: SendRawId                                                                              */
00005 /* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
00006 /*********************************************************************************************************/
00007 void SendRawId (unsigned short id)
00008 {
00009     CANMessage msgTx=CANMessage();
00010     msgTx.id=id;
00011     msgTx.len=0;
00012     can1.write(msgTx);
00013     wait_us(200);
00014 }
00015 
00016 /*********************************************************************************************/
00017 /* FUNCTION NAME: SendAck                                                                    */
00018 /* DESCRIPTION  : Envoyer un acknowledge                                                     */
00019 /*********************************************************************************************/
00020 void SendAck(unsigned short id, unsigned short from)
00021 {
00022     CANMessage msgTx=CANMessage();
00023     msgTx.id=id;
00024     msgTx.len=2;
00025     msgTx.format=CANStandard;
00026     msgTx.type=CANData;
00027     // from sur 2 octets
00028     msgTx.data[0]=(unsigned char)from;
00029     msgTx.data[1]=(unsigned char)(from>>8);
00030 
00031     can1.write(msgTx);
00032 }
00033 
00034 /*********************************************************************************************/
00035 /* FUNCTION NAME: GoToPosition                                                               */
00036 /* DESCRIPTION  : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
00037 /*********************************************************************************************/
00038 void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
00039 {
00040     //id_to_expect=ACK_CONSIGNE;
00041 
00042     CANMessage msgTx=CANMessage();
00043     msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
00044     msgTx.len=7;
00045     msgTx.format=CANStandard;
00046     msgTx.type=CANData;
00047     // x sur 2 octets
00048     msgTx.data[0]=(unsigned char)x;
00049     msgTx.data[1]=(unsigned char)(x>>8);
00050     // y sur 2 octets
00051     msgTx.data[2]=(unsigned char)y;
00052     msgTx.data[3]=(unsigned char)(y>>8);
00053     // theta signé sur 2 octets
00054     msgTx.data[4]=(unsigned char)theta;
00055     msgTx.data[5]=(unsigned char)(theta>>8);
00056     msgTx.data[6]=sens;
00057 
00058     can1.write(msgTx);
00059 }
00060 
00061 /****************************************************************************************/
00062 /* FUNCTION NAME: Rotate                                                                */
00063 /* DESCRIPTION  : Transmission CAN correspondant à une rotation                         */
00064 /****************************************************************************************/
00065 void Rotate (signed short angle)
00066 {
00067     CANMessage msgTx=CANMessage();
00068     msgTx.id=ASSERVISSEMENT_ROTATION;  // Tx rotation autour du centre du robot
00069     msgTx.len=2;
00070     msgTx.format=CANStandard;
00071     msgTx.type=CANData;
00072     //  Angle signé sur 2 octets
00073     msgTx.data[0]=(unsigned char)angle;
00074     msgTx.data[1]=(unsigned char)(angle>>8);
00075 
00076     can1.write(msgTx);
00077 }
00078 
00079 
00080 /*********************************************************************************************/
00081 /* FUNCTION NAME: GoStraight                                                                 */
00082 /* DESCRIPTION  : Transmission CAN correspondant à une ligne droite, avec ou sans recalage   */
00083 /*  recalage : 0 => pas de recalage                                                          */
00084 /*             1 => recalage en X                                                            */
00085 /*             2 => Recalage en Y                                                            */
00086 /*  newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo            */
00087 /*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
00088 /*                   0 => non                                                                */
00089 /*                   1 => oui                                                                */
00090 /*                   2 => dernière instruction de l'enchainement                             */
00091 /*********************************************************************************************/
00092 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
00093 {
00094     CANMessage msgTx=CANMessage();
00095     msgTx.id=ASSERVISSEMENT_RECALAGE;
00096     msgTx.len=6;
00097     msgTx.format=CANStandard;
00098     msgTx.type=CANData;
00099     // x sur 2 octets
00100     msgTx.data[0]=(unsigned char)distance;
00101     msgTx.data[1]=(unsigned char)(distance>>8);
00102     //Recalage sur 1 octet
00103     msgTx.data[2]=recalage;
00104     //Valeur du recalage sur 2 octets
00105     msgTx.data[3]=(unsigned char)newValue;
00106     msgTx.data[4]=(unsigned char)(newValue>>8);
00107     //Enchainement sur 1 octet
00108     msgTx.data[5]=isEnchainement;
00109 
00110     can1.write(msgTx);
00111     //wait_ms(500);
00112 }
00113 
00114 /********************************************************************************************/
00115 /* FUNCTION NAME: BendRadius                                                                */
00116 /* DESCRIPTION  : Transmission CAN correspondant à un rayon de courbure                     */
00117 /********************************************************************************************/
00118 void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
00119 {
00120     CANMessage msgTx=CANMessage();
00121     msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
00122     msgTx.len=6;
00123     msgTx.format=CANStandard;
00124     msgTx.type=CANData;
00125     // Rayon sur 2 octets
00126     msgTx.data[0]=(unsigned char)rayon;
00127     msgTx.data[1]=(unsigned char)(rayon>>8);
00128     // Angle signé sur 2 octets
00129     msgTx.data[2]=(unsigned char)angle;
00130     msgTx.data[3]=(unsigned char)(angle>>8);
00131     // Sens signé sur 1 octet
00132     msgTx.data[4]=sens;
00133     // Enchainement sur 1 octet
00134     msgTx.data[5]=enchainement;
00135 
00136     can1.write(msgTx);
00137 }
00138 
00139 void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta)
00140 {
00141     CANMessage msgTx=CANMessage();
00142     msgTx.id=canId;
00143     msgTx.format=CANStandard;
00144     msgTx.type=CANData;
00145     msgTx.len=6;
00146 
00147         // x sur 2 octets
00148     msgTx.data[0]=(unsigned char)x;
00149     msgTx.data[1]=(unsigned char)(x>>8);
00150     // y sur 2 octets
00151     msgTx.data[2]=(unsigned char)y;
00152     msgTx.data[3]=(unsigned char)(y>>8);
00153     // theta signé sur 2 octets
00154     msgTx.data[4]=(unsigned char)theta;
00155     msgTx.data[5]=(unsigned char)(theta>>8);
00156 
00157     can1.write(msgTx);
00158 }
00159 
00160 /****************************************************************************************/
00161 /* FUNCTION NAME: setAsservissementEtat                                                 */
00162 /* DESCRIPTION  : Activer ou désactiver l'asservissement                                */
00163 /****************************************************************************************/
00164 void setAsservissementEtat(unsigned char enable)
00165 {
00166     CANMessage msgTx=CANMessage();
00167     msgTx.id=ASSERVISSEMENT_ENABLE;  // Tx rotation autour du centre du robot
00168     msgTx.len=1;
00169     msgTx.format=CANStandard;
00170     msgTx.type=CANData;
00171     //  Angle signé sur 2 octets
00172     msgTx.data[0]=(unsigned char)((enable==0)?0:1);
00173 
00174     can1.write(msgTx);
00175 }
00176 
00177 
00178 /****************************************************************************************/
00179 /* FUNCTION NAME: SendSpeed                                                             */
00180 /* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
00181 /****************************************************************************************/
00182 void SendSpeed (unsigned short vitesse, unsigned short acceleration)
00183 {
00184     CANMessage msgTx=CANMessage();
00185     msgTx.id=ASSERVISSEMENT_CONFIG;
00186     msgTx.format=CANStandard;
00187     msgTx.type=CANData;
00188     msgTx.len=4;
00189     msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
00190     msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
00191     msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
00192     msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
00193 
00194     can1.write(msgTx);
00195         
00196 }