CRAC Team / CRAC-Strat_2017_avant_2ieme_match

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CRAC-Strat_2017_fin_premier_match by CRAC Team

Asservissement/Asservissement.cpp@6:eddfa414fd11, 2016-04-25 (annotated)

Committer:
antbig
Date:
Mon Apr 25 10:59:22 2016 +0000
Revision:
6:eddfa414fd11
Parent:
5:dcd817534b57
Child:
9:d0042422d95a
Correction check carte petit robot (carte actionneurs non fonctionnel)

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 5:dcd817534b57 17 /* FUNCTION NAME: SendAck */
antbig 5:dcd817534b57 18 /* DESCRIPTION : Envoyer un acknowledge */
antbig 5:dcd817534b57 19 /*********************************************************************************************/
antbig 5:dcd817534b57 20 void SendAck(unsigned short id, unsigned short from)
antbig 5:dcd817534b57 21 {
antbig 5:dcd817534b57 22 CANMessage msgTx=CANMessage();
antbig 5:dcd817534b57 23 msgTx.id=id;
antbig 5:dcd817534b57 24 msgTx.len=2;
antbig 5:dcd817534b57 25 msgTx.format=CANStandard;
antbig 5:dcd817534b57 26 msgTx.type=CANData;
antbig 5:dcd817534b57 27 // from sur 2 octets
antbig 5:dcd817534b57 28 msgTx.data[0]=(unsigned char)from;
antbig 5:dcd817534b57 29 msgTx.data[1]=(unsigned char)(from>>8);
antbig 5:dcd817534b57 30
antbig 5:dcd817534b57 31 can1.write(msgTx);
antbig 5:dcd817534b57 32 }
antbig 5:dcd817534b57 33
antbig 5:dcd817534b57 34 /*********************************************************************************************/
antbig 0:ad97421fb1fb 35 /* FUNCTION NAME: GoToPosition */
antbig 0:ad97421fb1fb 36 /* DESCRIPTION : Transmission CAN correspondant à un asservissement en position (x,y,theta) */
antbig 0:ad97421fb1fb 37 /*********************************************************************************************/
antbig 0:ad97421fb1fb 38 void GoToPosition (unsigned short x,unsigned short y,signed short theta,signed char sens)
antbig 0:ad97421fb1fb 39 {
antbig 0:ad97421fb1fb 40 //id_to_expect=ACK_CONSIGNE;
antbig 0:ad97421fb1fb 41
antbig 0:ad97421fb1fb 42 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 43 msgTx.id=ASSERVISSEMENT_XYT; // tx nouvelle position en (x,y,theta)
antbig 0:ad97421fb1fb 44 msgTx.len=7;
antbig 0:ad97421fb1fb 45 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 46 msgTx.type=CANData;
antbig 0:ad97421fb1fb 47 // x sur 2 octets
antbig 0:ad97421fb1fb 48 msgTx.data[0]=(unsigned char)x;
antbig 0:ad97421fb1fb 49 msgTx.data[1]=(unsigned char)(x>>8);
antbig 0:ad97421fb1fb 50 // y sur 2 octets
antbig 0:ad97421fb1fb 51 msgTx.data[2]=(unsigned char)y;
antbig 0:ad97421fb1fb 52 msgTx.data[3]=(unsigned char)(y>>8);
antbig 0:ad97421fb1fb 53 // theta signé sur 2 octets
antbig 0:ad97421fb1fb 54 msgTx.data[4]=(unsigned char)theta;
antbig 0:ad97421fb1fb 55 msgTx.data[5]=(unsigned char)(theta>>8);
antbig 0:ad97421fb1fb 56 msgTx.data[6]=sens;
antbig 0:ad97421fb1fb 57
antbig 0:ad97421fb1fb 58 can1.write(msgTx);
antbig 0:ad97421fb1fb 59 }
antbig 0:ad97421fb1fb 60
antbig 0:ad97421fb1fb 61 /****************************************************************************************/
antbig 0:ad97421fb1fb 62 /* FUNCTION NAME: Rotate */
antbig 0:ad97421fb1fb 63 /* DESCRIPTION : Transmission CAN correspondant à une rotation */
antbig 0:ad97421fb1fb 64 /****************************************************************************************/
antbig 0:ad97421fb1fb 65 void Rotate (signed short angle)
antbig 0:ad97421fb1fb 66 {
antbig 0:ad97421fb1fb 67 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 68 msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
antbig 1:116040d14164 69 msgTx.len=2;
antbig 0:ad97421fb1fb 70 msgTx.format=CANStandard;
antbig 0:ad97421fb1fb 71 msgTx.type=CANData;
antbig 0:ad97421fb1fb 72 // Angle signé sur 2 octets
antbig 0:ad97421fb1fb 73 msgTx.data[0]=(unsigned char)angle;
antbig 0:ad97421fb1fb 74 msgTx.data[1]=(unsigned char)(angle>>8);
antbig 0:ad97421fb1fb 75
antbig 0:ad97421fb1fb 76 can1.write(msgTx);
antbig 1:116040d14164 77 }
antbig 1:116040d14164 78
antbig 1:116040d14164 79
antbig 1:116040d14164 80 /*********************************************************************************************/
antbig 1:116040d14164 81 /* FUNCTION NAME: GoStraight */
antbig 1:116040d14164 82 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
antbig 1:116040d14164 83 /* recalage : 0 => pas de recalage */
antbig 1:116040d14164 84 /* 1 => recalage en X */
antbig 1:116040d14164 85 /* 2 => Recalage en Y */
antbig 1:116040d14164 86 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
antbig 1:116040d14164 87 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
antbig 1:116040d14164 88 /* 0 => non */
antbig 1:116040d14164 89 /* 1 => oui */
antbig 6:eddfa414fd11 90 /* 2 => dernière instruction de l'enchainement */
antbig 1:116040d14164 91 /*********************************************************************************************/
antbig 1:116040d14164 92 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
antbig 1:116040d14164 93 {
antbig 1:116040d14164 94 CANMessage msgTx=CANMessage();
antbig 1:116040d14164 95 msgTx.id=ASSERVISSEMENT_RECALAGE;
antbig 1:116040d14164 96 msgTx.len=6;
antbig 1:116040d14164 97 msgTx.format=CANStandard;
antbig 1:116040d14164 98 msgTx.type=CANData;
antbig 1:116040d14164 99 // x sur 2 octets
antbig 1:116040d14164 100 msgTx.data[0]=(unsigned char)distance;
antbig 1:116040d14164 101 msgTx.data[1]=(unsigned char)(distance>>8);
antbig 1:116040d14164 102 //Recalage sur 1 octet
antbig 1:116040d14164 103 msgTx.data[2]=recalage;
antbig 1:116040d14164 104 //Valeur du recalage sur 2 octets
antbig 1:116040d14164 105 msgTx.data[3]=(unsigned char)newValue;
antbig 1:116040d14164 106 msgTx.data[4]=(unsigned char)(newValue>>8);
antbig 1:116040d14164 107 //Enchainement sur 1 octet
antbig 1:116040d14164 108 msgTx.data[5]=isEnchainement;
antbig 1:116040d14164 109
antbig 1:116040d14164 110 can1.write(msgTx);
antbig 6:eddfa414fd11 111 }
antbig 6:eddfa414fd11 112
antbig 6:eddfa414fd11 113 /********************************************************************************************/
antbig 6:eddfa414fd11 114 /* FUNCTION NAME: BendRadius */
antbig 6:eddfa414fd11 115 /* DESCRIPTION : Transmission CAN correspondant à un rayon de courbure */
antbig 6:eddfa414fd11 116 /********************************************************************************************/
antbig 6:eddfa414fd11 117 void BendRadius (unsigned short rayon,signed short angle,signed char sens, unsigned char enchainement)
antbig 6:eddfa414fd11 118 {
antbig 6:eddfa414fd11 119 CANMessage msgTx=CANMessage();
antbig 6:eddfa414fd11 120 msgTx.id=ASSERVISSEMENT_COURBURE; // tx asservissement rayon de courbure
antbig 6:eddfa414fd11 121 msgTx.len=6;
antbig 6:eddfa414fd11 122 msgTx.format=CANStandard;
antbig 6:eddfa414fd11 123 msgTx.type=CANData;
antbig 6:eddfa414fd11 124 // Rayon sur 2 octets
antbig 6:eddfa414fd11 125 msgTx.data[0]=(unsigned char)rayon;
antbig 6:eddfa414fd11 126 msgTx.data[1]=(unsigned char)(rayon>>8);
antbig 6:eddfa414fd11 127 // Angle signé sur 2 octets
antbig 6:eddfa414fd11 128 msgTx.data[2]=(unsigned char)angle;
antbig 6:eddfa414fd11 129 msgTx.data[3]=(unsigned char)(angle>>8);
antbig 6:eddfa414fd11 130 // Sens signé sur 1 octet
antbig 6:eddfa414fd11 131 msgTx.data[4]=sens;
antbig 6:eddfa414fd11 132 // Enchainement sur 1 octet
antbig 6:eddfa414fd11 133 msgTx.data[5]=enchainement;
antbig 6:eddfa414fd11 134
antbig 6:eddfa414fd11 135 can1.write(msgTx);
antbig 6:eddfa414fd11 136 }