File content as of revision 6:adf1f4351f9f:

//*****************************************************/
//  Include  //
#include "mbed.h"
#include "pinconfig.h"
#include "PID.h"
//#include "Motor.h"
#include "eeprom.h"
#include "Receiver.h"
#include "Motion_EEPROM_Address.h"
#include "move.h"
#include "UNTRASONIC.h"
#include "BufferedSerial.h"
#include "rplidar.h"
RPLidar lidar;
//#include "pidcontrol.h"
BufferedSerial se_lidar(PA_11,PA_12);
#define EEPROM_DELAY 2
DigitalOut rs485_dirc1(RS485_DIRC);
//#define DEBUG_UP
//#define DEBUG_LOW
PwmOut VMO(PC_8);
InterruptIn encoderA_d(PB_12);
DigitalIn encoderB_d(PB_13);
InterruptIn encoderA_1(PB_1);
DigitalIn encoderB_1(PB_2);
InterruptIn encoderA_2(PB_14);
DigitalIn encoderB_2(PB_15);
Timer timerStart;
Timer tim;
Timeout time_getsensor;
Timeout time_distance;
Timeout shutdown;
move m1;
//*****************************************************/

// Global  //
//timer
 int timer_now=0,timer_later=0;
 int times=0,timer_buffer=0;
 
 //encoder
 int Encoderpos = 0;
 int real_d=0;
 float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
//pid

double setp1=0,setp2=0;
float outPID =0;
float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ;
PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
//Ticker Recieve;
//-- Communication --
COMMUNICATION  *com1;

//BufferedSerial PC(SERIAL_TX,SERIAL_RX);
Serial PC(SERIAL_TX,SERIAL_RX);
Bear_Receiver com(PA_9,PA_10,115200);
int16_t MY_ID = 0x00;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
float KP_LEFT_BUFF=0,KI_LEFT_BUFF=0,KD_LEFT_BUFF=0,KP_RIGHT_BUFF=0,KI_RIGHT_BUFF =0,KD_RIGHT_BUFF=0;

 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
 void RC();

//rplidar
 //float distances = 0;
 //float angle    = 0;
//ool  startBit = 0;
//char  quality =0 ;


void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);


DigitalOut myled(LED1);


void Rx_interrupt()
{
    //s1.get_motor();รับค่ามอเตอร์
    RC();
    timer_later= timer_now;
  
}
void EncoderA_1()//ซ้าย
{   if(encoderB_1==0)
        { Encoderpos = Encoderpos + 1;}
    else
   { Encoderpos = Encoderpos -1;}
   pulse_1+=1;
   //Encoderpos = Encoderpos + 1;
   //valocity+=1;
   //pc.printf("%d \n",Encoderpos);
   //pc.printf("pulse=%f  \n",pulse);
   //if(pulse==128)
   //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
}
  void EncoderA_2()//ขวา
{ 
    if(encoderB_2==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_2+=1;
    //pc.printf("%d",Encoderpos);
}
void EncoderA_D()
{ 
    if(encoderB_d==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_d+=1;
    if(pulse_d==128)
    {
        Z_d+=1;
        pulse_d=0;
    }
  
}
void getvelo1()//จาก encoder
{
    valocity1=pulse_1*((2*3.14*r)/128);
    PC.printf("valocity=%f  \n",valocity1);
    count=0;
    timerStart.reset();
}
void getvelo2()
{
    valocity2=pulse_2*((2*3.14*r)/128);
    PC.printf("valocity=%f  \n",valocity2);
    count=0;
    timerStart.reset();
}
void get_d()//ระยะทาง
{
    real_d=Z_d*(2*3.14*r);
    //ส่งข้อมูล
    
}
void get_rplidar()
{
     if (IS_OK(lidar.waitPoint())) 
     {
    
    } 
    else
    {
  
       lidar.startScan();
  
    }
    
}
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    
}
void PID_m1()//left
{
    setp1=map(1.0,0.0,1.094,0.0,1.0);
    P1.setSetPoint(setp1);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo1();
           //pc.printf("TIME \n");
           times=0;
           pulse_1=0;
        }
    P1.setProcessValue(valocity1);
    outPID=P1.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_1(setp1,outPID);
}
void PID_m2()//right
{
    setp2=map(1.0,0.0,1.094,0.0,1.0);
    P2.setSetPoint(setp2);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo2();
           //pc.printf("TIME \n");
           times=0;
           pulse_2=0;
        }
    P2.setProcessValue(valocity2);
    outPID=P2.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_2(setp2,outPID);
}


void RC()
{
    myled =1;
    uint8_t data_array[30];
    uint8_t id=0;
    uint8_t ins=0;
    uint8_t status=0xFF;



    status = com.ReceiveCommand(&id,data_array,&ins);
    PC.printf("status = 0x%02x\n\r",status);
    if(status == ANDANTE_ERRBIT_NONE) {
        CmdCheck((int16_t)id,data_array,ins);
        PC.printf("RC******************************");
    }

}
/*******************************************************/
int buf=0;
int main()
{
    PC.baud(115200);
    lidar.begin(se_lidar);
    tim.start();
    //com1 = new COMMUNICATION(PA_9,PA_10,115200);
       encoderA_1.rise(&EncoderA_1);
       timerStart.start();   
       P1.setMode(1);
       P1.setBias(0);
     //  pc.printf("READY \n");
    //pc.attach(&Rx_interrupt, Serial::RxIrq);
    lidar.setAngle(0,360);
    while(1) {

        VMO=1;
        get_rplidar();
    /*    if(tim.read_ms()-buf>=1000){
      for(int x=0;x<=359;x++){
        printf("%d,",lidar.Data[x]);
      }
      buf = tim.read_ms();
    }*/
        RC();
        //wait_ms(1);
    }
}









void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
       PC.printf("cmdcheck\n");
     if(id==MY_ID) {
        switch (ins) {
            case PING: {
                break;
            }
            case WRITE_DATA: {
                switch (command[0]) {
                    case ID: {
                        ///
                        MY_ID = (int16_t)command[1];
                        break;
                    }
                    case SET_VELOCITY_LEFT: {
                        //
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                        VL=Int+(flo/10000);
                        PC.printf("VL=%f /n",VL);
                        break;
                    }
                    case SET_VELOCITY_RIGHT: {
                        //
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                        VR=Int+flo;
                        PC.printf("VL=%f /n",VR);
                        break;
                    }
                    case SET_VELOCITY_MAX_LEFT: {
                        //
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                        VLmax=Int+flo;
                        PC.printf("VL=%f /n",VLmax);
                        break;
                    }
                    case SET_VELOCITY_MAX_RIGHT: {
                        //
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                        VRmax=Int+flo;
                        PC.printf("VRmax = %f",VRmax);
                      //  PC.printf("*****************************");
                        break;
                    }
                    //save to rom
                    case SET_KP_LEFT: {
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                         float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);

                       KP_LEFT=Int+flo;
                       PC.printf("KP_LEFT=%f /n",KP_LEFT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_LEFT_KP,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                    case SET_KI_LEFT: {
                       uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                         float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);

                       KI_LEFT=Int+flo;
                          PC.printf("KI_LEFT=%f /n",KI_LEFT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_LEFT_KI ,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                    case SET_KD_LEFT: {
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                         float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                       KD_LEFT=Int+flo;
                          PC.printf("KD_LEFT=%f /n",KD_LEFT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_LEFT_KD,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                    case  SET_KP_RIGHT: {
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                         float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                       KP_RIGHT=Int+flo;
                          PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_RIGHT_KP,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                    case SET_KI_RIGHT: {
                       uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                         float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                       KI_RIGHT=Int+flo;
                          PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_RIGHT_KI,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                    case SET_KD_RIGHT: {
                        uint8_t int_buffer[2],float_buffer[2];
                        float Int,flo;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        float_buffer[0]=command[3];
                        float_buffer[1]=command[4];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
                       KD_RIGHT=Int+flo;
                          PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
                       int32_t data_buff;
                       data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
                       memory.write(ADDRESS_RIGHT_KD,data_buff);
                       wait_ms(EEPROM_DELAY);
                        break;
                    }
                 }
            } break;
             case READ_DATA: {  PC.printf("READ_DATA\n");
                switch (command[0]) {
                    case GET_LIDAR: {
                       /* int i=0,j=1,k=0;
                         uint8_t intData[2]={0x00,0x01},floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 4;//122
                        package.instructionErrorId = WRITE_DATA;
                        PC.printf("GETDATA\n");
                        while(k<60)
                        { PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                         com.FloatSep( lidar.Data[k],intData,floatData);
                         package.parameter[i]=intData[0];
                         package.parameter[j]=intData[1];
                         i=i+2;
                         j=j+2;
                         k++;
                         
                        }
                     //   PC.printf("SEND\n");
                        //rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         PC.printf("SEND2\n");
                         com1->sendCommunicatePacket(&package);
                         PC.printf("SEND3\n");
                         
                          
                         
                  
                        */
                        com.sendlidar();
                        PC.printf("SEND2\n");
                        break;
                        }
                     case GET_LIDAR2: {
                        int i=0,j=1,k=60;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                        
                        while(k<120)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                 com.FloatSep( lidar.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                            
                        }
                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                     case GET_LIDAR3: {
                        int i=0,j=1,k=120;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<180)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                com.FloatSep( lidar.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                     break;
                        }
                      case GET_LIDAR4: {
                        int i=0,j=1,k=180;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<240)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                com.FloatSep( lidar.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                     break;
                        }
                          case GET_LIDAR5: {
                        int i=0,j=1,k=240;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<300)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                com.FloatSep( lidar.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        
                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                     break;
                        }
                          case GET_LIDAR6: {
                        int i=0,j=1,k=300;
                         uint8_t intData[2],floatData[2];
                         ANDANTE_PROTOCOL_PACKET package;
                        //BUFFER_SIZE=143
                        package.robotId = MY_ID;
                         package.length = 122;
                        package.instructionErrorId = WRITE_DATA;
                          while(k<360)
                        {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
                                com.FloatSep( lidar.Data[k],intData,floatData);
                                package.parameter[i]=intData[0];
                                package.parameter[j]=intData[1];
                                i=i+2;
                                j=j+2;
                                k++;
                        }
                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                     break;
                        }
                        
                    case GET_BATTERY: {
                        
                        break;
                        }
                    case GET_VELOCITY_LEFT: {
                        uint8_t intVelo_L[2],floatVelo_L[2];
                        com.FloatSep(valocity1,intVelo_L,floatVelo_L);
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intVelo_L[0];
                        package.parameter[1]=intVelo_L[1];
                         package.parameter[2]=floatVelo_L[0];
                        package.parameter[3]=floatVelo_L[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        
                        break;
                        }
                    case GET_VELOCITY_RIGHT : {
                        uint8_t intVelo_R[2],floatVelo_R[2];
                        com.FloatSep(valocity2,intVelo_R,floatVelo_R);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intVelo_R[0];
                        package.parameter[1]=intVelo_R[1];
                         package.parameter[2]=floatVelo_R[0];
                        package.parameter[3]=floatVelo_R[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KP_LEFT: {
                        memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF);
                        uint8_t intKPL[2],floatKPL[2];
                        com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKPL[0];
                        package.parameter[1]=intKPL[1];
                         package.parameter[2]=floatKPL[0];
                        package.parameter[3]=floatKPL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KI_LEFT: {
                        memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
                        uint8_t intKIL[2],floatKIL[2];
                        com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKIL[0];
                        package.parameter[1]=intKIL[1];
                         package.parameter[2]=floatKIL[0];
                        package.parameter[3]=floatKIL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KD_LEFT: {
                        memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
                        uint8_t intKDL[2],floatKDL[2];
                        com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDL[0];
                        package.parameter[1]=intKDL[1];
                         package.parameter[2]=floatKDL[0];
                        package.parameter[3]=floatKDL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KP_RIGHT: {
                        memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
                        uint8_t intKDR[2],floatKDR[2];
                        com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDR[0];
                        package.parameter[1]=intKDR[1];
                         package.parameter[2]=floatKDR[0];
                        package.parameter[3]=floatKDR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KI_RIGHT: {
                        memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
                        uint8_t intKIR[2],floatKIR[2];
                        com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKIR[0];
                        package.parameter[1]=intKIR[1];
                         package.parameter[2]=floatKIR[0];
                        package.parameter[3]=floatKIR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KD_RIGHT: {
                        memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
                        uint8_t intKDR[2],floatKDR[2];
                        com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDR[0];
                        package.parameter[1]=intKDR[1];
                         package.parameter[2]=floatKDR[0];
                        package.parameter[3]=floatKDR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
           }
     }break;
                    
    }
  }
}