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

Committer:
antbig
Date:
2016-04-28
Revision:
9:d0042422d95a
Parent:
6:eddfa414fd11
Child:
11:ed13a480ddca

File content as of revision 9:d0042422d95a:

#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: 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);

    can1.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;

    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=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);

    can1.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;

    can1.write(msgTx);
}

/********************************************************************************************/
/* 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;

    can1.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);

    can1.write(msgTx);
}