File content as of revision 1:162aa6608ffe:

#include "mbed.h"
#include "Nucleo_Encoder_16_bits.h"
#include "MYCRAC_utility.h"
#include "Asservissement.h"
#include "global.h"
#include "MY_Asserv.h"


/****************************************************************************************/
/* FUNCTION NAME: Send_assev_info_comfig                                                */
/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
/****************************************************************************************/
void Send_assev_info_comfig(void)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=ASSERVISSEMENT_INFO_CONSIGNE;   // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    msgTx.len=6;
    msgTx.data[0]=(unsigned char)(((short)Kpp1)&0xFF);
    msgTx.data[1]=(unsigned char)(((short)Kpp1 >> 8)&0xFF);
    msgTx.data[2]=(unsigned char)(((short)Kip1)&0xFF);
    msgTx.data[3]=(unsigned char)(((short)Kip1 >> 8)&0xFF);
    msgTx.data[4]=(unsigned char)(((short)Kdp1)&0xFF);
    msgTx.data[5]=(unsigned char)(((short)Kdp1 >> 8)&0xFF);

    can1.write(msgTx);
}

/****************************************************************************************/
/* FUNCTION NAME: Send_odometrie                                                        */
/* DESCRIPTION  : Envoie  de l'odometrie                                                */
/****************************************************************************************/
void Send_odometrie(void)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=ODOMETRIE_SMALL_POSITION;  // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    msgTx.len=6;
    msgTx.data[0]=((short)Odo_x) & 0xFF;
    msgTx.data[1]=((short)Odo_x >> 8) & 0xFF;
    msgTx.data[2]=((short)Odo_y) & 0xFF;
    msgTx.data[3]=((short)Odo_y >> 8) & 0xFF;
    msgTx.data[4]=((short)Odo_theta % 3600) & 0xFF;
    msgTx.data[5]=(((short)Odo_theta % 3600) >> 8) & 0xFF;

    can1.write(msgTx);
}

/****************************************************************************************/
/* FUNCTION NAME: Debug_Asserv                                                          */
/* DESCRIPTION  : Envoie  vall encod + consigne_pos                                     */
/* Ps           : les encodeurs sont sur 32 bits, là on ne prend que les 16 premiérs    */
/****************************************************************************************/
void Debug_Asserv(void)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=DEBUG_ASSERV;  // si proposition accepter remplacer par "ACKNOWLEDGE_MOTEUR"
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    msgTx.len=6;
    msgTx.data[0]=(short)encoder1.GetCounter(); 
    msgTx.data[1]=(short)encoder1.GetCounter()>>8;
    msgTx.data[2]=(short)encoder2.GetCounter();
    msgTx.data[3]=(short)encoder2.GetCounter()>>8;
    msgTx.data[4]=(short)(consigne_pos);
    msgTx.data[5]=(char)consigne_pos >> 8;

    can1.write(msgTx);
}

/****************************************************************************************/
/* FUNCTION NAME: motor_of                                                              */
/* DESCRIPTION  : prevenir que les moteur sont à l'arret                                */
/****************************************************************************************/
void motor_of(void)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=INSTRUCTION_END_MOTEUR;
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    msgTx.len=2;
    msgTx.data[0]= 0;
    msgTx.data[1]= 0;
    
    can1.write(msgTx);
}