FRC_2019 | Mbed

FRC_equipe1 » Code » FRC_2019
FRC_equipe1 / Mbed 2 deprecated FRC_2019
Dependencies: mbed
CAN_asser.cpp

Committer:: theolp
Date:: 2019-05-27
Revision:: 11:e8c4a1c6553d
Parent:: 9:2113adf37c66
File content as of revision 11:e8c4a1c6553d:

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

CAN can(PB_8, PB_9,1000000);
//Serial dbug(USBTX, USBRX, 115200);    //PC
Serial dbug(PA_0, PA_1, 9600);         //Bluetooth

//Propre fonction valeur absolue
int val_abs(int a)
{
    if(a < 0)
        return -1*a;
    return a;
}
//Propre fonction qui saute des lignes pour dbug bluetooth
void sauter_lignes(int nb_lignes)
{
    for(unsigned i=0; i<nb_lignes; ++i)
        dbug.printf("\n\r");  
}

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 majFlagDpl(bool& flagFinDpl)
{
    while(nb_msg_can>0)
    {
        if(msg_can[pt_lecture_can].id == 0x111)
            flagFinDpl = true;
            
        nb_msg_can--;
        pt_lecture_can++;
    }
}

void majPos(int* pos)
{
    while(nb_msg_can>0)
    {
        if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION)
            get_pos(msg_can[pt_lecture_can], (short*)pos);
            
        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);


}
Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	25 Mar 2019
Imports:	4
Forks:	0
Commits:	42
Dependents:	0
Dependencies:	1
Followers:	8
Important changes to repositories hosted on mbed.com

CAN_asser.cpp

File content as of revision 11:e8c4a1c6553d:

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

CAN_asser.cpp

File content as of revision 11:e8c4a1c6553d:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
