FRC_equipe1 / Mbed 2 deprecated FRC_2019

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.

Dependencies:   mbed

CAN_asser.cpp

Committer:
Wael_H
Date:
2019-05-23
Revision:
8:94ecfe411d02
Parent:
7:753e901d441b
Child:
9:2113adf37c66

File content as of revision 8:94ecfe411d02:

#include "mbed.h"
#include "CAN_asser.h"

CANMessage msg_can[256];
unsigned char pt_lecture_can = 0, pt_ecriture_can = 0;
int nb_msg_can = 0;
short pos[3];
extern bool flagFinDpl;
extern CAN can;
Serial dbug(USBTX, USBRX, 115200);

void can_init(void)
{
    can.attach(&isr_can, CAN::RxIrq);
}

void isr_can()
{
    can.read(msg_can[pt_ecriture_can]);
    nb_msg_can++;
    pt_ecriture_can++;
    if(nb_msg_can > 6)
    {
        //dbug.printf("!!!!!! CAN BUFFER OVERFLOW !!!!!!!!\n");
         pt_lecture_can++;
         nb_msg_can--;
    }
}

void trait_can(bool& flagFinDpl)
{
    while(nb_msg_can>0)
    {
        /*if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION)
            get_pos(msg_can[pt_lecture_can], pos);*/
        
        if(msg_can[pt_lecture_can].id == 0x111)
        {
            flagFinDpl = true;
            //dbug.printf("CAN fin dpl\n");
        }
        
        /*else if(msg_can[pt_lecture_can].id == 0x101)
            dbug.printf("CAN revived\n");*/
            
        nb_msg_can--;
        pt_lecture_can++;
    }
}

void get_pos(CANMessage msg_pos, short* position)
{
    memcpy((void *)((void *)&position[0]), &(msg_pos.data[0]), 2);
    memcpy((void *)((void *)&position[1]), &(msg_pos.data[2]), 2);
    memcpy((void *)((void *)&position[2]), &(msg_pos.data[4]), 2);
}

void SendCoefK (unsigned short id, double K)
{
    
    CANMessage msgTx=CANMessage();
    
    msgTx.id=id;
    
    msgTx.len=2;
    
    msgTx.format=CANStandard;
    
    msgTx.type=CANData;
    
    // from sur 2 octets
    for(int i=0;i<8;i++)
    {
        memcpy(&(msgTx.data[i]), (void *)((void *)&K + i), 1);
    }
    
    can.write(msgTx);
}
/*********************************************************************************************************/

/* 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;
    
    can.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);
    
    
    can.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;
    
    
    can.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);
    
    
    can.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;
    
    
    can.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;
    
    
    can.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);
    
    
    can.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);
    
    
    can.write(msgTx);
}


/****************************************************************************************/

/* FUNCTION NAME: SendSpeed */

/* DESCRIPTION : Envoie un asservissement paramètre retournant à une vitesse */
/****************************************************************************************/
void SendSpeed (unsigned short vitesse, unsigned short acceleration)
    {
    
    CANMessage msgTx=CANMessage();
    
    msgTx.id=ASSERVISSEMENT_CONFIG;
    
    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)(acceleration&0x00FF);
    
    
    msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
    
    
    can.write(msgTx);


}

/****************************************************************************************/

/* 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);
    
    can.write(msgTx);


}