FRC_2019 | Mbed

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

Committer:: Wael_H
Date:: 2019-05-09
Revision:: 5:34048faec367
Child:: 7:753e901d441b
File content as of revision 5:34048faec367:

#include "robot_general.h"

double KppD = 0.5;     //2     
double KipD = 0.001*TE/0.02;     //0.1   //0.001
double KdpD = 2.5/TE*0.02;     //3     //2.5

double KppG = 0.5;     //2    
double KipG = 0.001*TE/0.02;     //0.1   //0.001
double KdpG = 2.5/TE*0.02;     //3     //2.5 

CAN can(PB_8, PB_9,1000000); 
unsigned char pt_lecture_can = 0, pt_ecriture_can = 0;
int nb_msg_can = 0;
CANMessage msg_can[256];
short pos[3];
int flagFinDpl = 0; 
 
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()
{
    while(nb_msg_can>0)
    {
        //dbug.printf("tram CAN recvived %x\n", msg_can[pt_lecture_can].id);
        if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION) get_pos(msg_can[pt_lecture_can], pos);
        else if(msg_can[pt_lecture_can].id == 0x111)
        {
            flagFinDpl = 1;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++;
        //remonter_tab_can(msg_can, nb_msg);
        //if(msg_can[0].id != 0x26 && msg_can[0].id != 0x762) dbug.printf("CAN_id : %x\n", msg_can[0].id);
        //dbug.printf("CAN_id : %x\n", msg_can[0].id);
    }
} 
 
void get_pos(CANMessage msg_pos, short* position)
{
    memcpy((void *)((void *)&position[0]), &(msg_pos.data[0]), 2);
    //memcpy((void *)((void *)&x[0] + 1), &(msg_can[0].data[1]), 1);
    memcpy((void *)((void *)&position[1]), &(msg_pos.data[2]), 2);
    //memcpy((void *)((void *)&x[1] + 1), &(msg_can[0].data[3]), 1);
    memcpy((void *)((void *)&position[2]), &(msg_pos.data[4]), 2);
    //memcpy((void *)((void *)&x[2] + 1), &(msg_can[0].data[5]), 1);
    
    //pc.printf("odo: x = %d, y = %d, te = %d.\n\r", position[0], position[1], position[2]);
}
 
void can_init(void)
{
    
    can.attach(&isr_can, CAN::RxIrq);
}

int compare_tab(short* t1, short* t2, int size)
{
    int flag = 0;
    for(int i=0; i<size; i++)
    {
        if(t1[i] != t2[i]) flag++;
    }
    return flag;
}

void remonter_tab_can(CANMessage* tab, int taille)
{
    for(int i=0;i<(taille);i++)
    {
        tab[i] = tab[i+1];
    }
}

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 5:34048faec367:

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 5:34048faec367:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
