Betago
v1
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of
CleaningMachine_Betago
by 
palm and chin
Diff: main.cpp
- Revision:
- 1:45f1573d65a1
- Parent:
- 0:84f05cd2f197
- Child:
- 2:f873deba2305
--- a/main.cpp Mon Feb 15 17:48:23 2016 +0000
+++ b/main.cpp Mon Mar 21 20:21:12 2016 +0000
@@ -1,63 +1,750 @@
-#include "Debug.h"
-#include "UI.h"
+//*****************************************************/
+// Include //
+#include "mbed.h"
#include "pinconfig.h"
-#include "BEAR_Protocol.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 "pidcontrol.h"
+
+#define EEPROM_DELAY 2
+DigitalOut rs485_dirc1(RS485_DIRC);
+//#define DEBUG_UP
+//#define DEBUG_LOW
+
+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);
+DigitalOut rs485_dirc1(RS485_DIRC);
+Timer timerStart;
+Timeout time_getsensor;
+Timeout time_distance;
+Timeout shutdown;
+move m1;
+//*****************************************************/
+//--PID parameter--
+//-Upper-//
+float U_Kc=0.2;
+float U_Ki_gain=0.0;
+float U_Kd_gain=0.0;
+float U_Ti=0.0;
+float U_Td=0.0;
+float U_Ki=U_Kc*U_Ti;
+float U_Kd=U_Kc*U_Td;
+//-lower-//
+float L_Kc=0.2;
+float L_Ki_gain=0.0;
+float L_Kd_gain=0.0;
+float L_Ti=0.0;
+float L_Td=0.0;
+float L_Ki=L_Kc*L_Ti;
+float L_Kd=L_Kc*L_Td;
+//*****************************************************/
+// 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,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ;
+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;
+Serial PC(SERIAL_TX,SERIAL_RX);
+Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200);
+int16_t MY_ID = 0x00;
+//-- Memorry --
+EEPROM memory(PB_4,PA_8,0);
+
+//-- encoder --
+
+//-- Motor --
+//int dir;
+//Motor Upper(PWM_LU,A_LU,B_LU);
+//Motor Lower(PWM_LL,A_LL,B_LL);
+//-- PID --
+//int Upper_SetPoint=20;
+//int Lower_SetPoint=8;
+//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
+//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
+
+//PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
+//PID Low_PID(L_Kc, L_Ti, L_Td);
+
+//*****************************************************/
+//void Start_Up();
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+
+//Timer counterUP;
+//Timer counterLOW;
+
+DigitalOut myled(LED1);
+
-DigitalIn button(USER_BUTTON);
-Serial pc(SERIAL_TX,SERIAL_RX);
+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;
+ }
+ // pc.printf("%d",Encoderpos);
+}
+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);
+ //ส่งข้อมูล
+
+}
+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(1,setp2,outPID);
+}
+/*
+void Read_Encoder(PinName Encoder)
+{
+ ENC.format(8,0);
+ ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
+
+ if(Encoder == EncoderA) {
+ EncA = 0;
+ } else {
+ EncB = 0;
+ }
+ ENC.write(0x41);
+ ENC.write(0x09);
+ data = ENC.write(0x00);
+ if(Encoder == EncoderA) {
+ EncA = 1;
+ } else {
+ EncB = 1;
+ }
-void DebugMode();
+}
+//****************************************************
+int Get_EnValue(int Val)
+{
+ int i = 0;
+ static unsigned char codes[] = {
+ 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
+ 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
+ 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
+ 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
+ 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
+ 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
+ 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
+ 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
+ };
+ if ( MY_ID == 0x01 ) { //when it was left side
+ while(Val != codes[i]) {
+ i++;
+ }
+ }
+ if ( MY_ID == 0x02 ) { //when it was right side
+ while(Val != codes[127-i]) {
+ i++;
+ }
+ }
+ return i;
+
+}
+//****************************************************
+void SET_UpperPID()
+{
+ Upper.period(0.001);
+
+ memory.read(ADDRESS_UP_MARGIN,UpMargin);
+ Up_PID.setMargin(UpMargin);
+
+ memory.read(ADDRESS_UPPER_KP,U_Kc);
+ Up_PID.setKp(U_Kc);
+ memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
+ Up_PID.setKi(U_Ki_gain);
+ memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
+ Up_PID.setKd(U_Kd_gain);
+
+ //Up_PID.setMode(0);
+ //Up_PID.setInputLimits(18,62);
+ //Up_PID.setOutputLimits(0,1);
+}
+//******************************************************/
+/*
+void SET_LowerPID()
+{
+ Lower.period(0.001);
+
+ memory.read(ADDRESS_LOW_MARGIN,LowMargin);
+ Low_PID.setMargin(LowMargin);
+
+ memory.read(ADDRESS_LOWER_KP,L_Kc);
+ Low_PID.setKp(L_Kc);
+ memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
+ Low_PID.setKi(L_Ki_gain);
+
+ memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
+ Low_PID.setKd(L_Kd_gain);
+ //Low_PID.setMode(0);
+ //Low_PID.setInputLimits(0,127); // set range
+ //Low_PID.setOutputLimits(0,1);
+}
+//******************************************************/
+/*
+void Move_Upper()
+{
+ Read_Encoder(EncoderA);
+ Upper_Position = (float)Get_EnValue(data);
+#ifdef DEBUG_UP
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Upper_SetPoint);
+ printf("Upper_Position = %f\n\r",Upper_Position);
+#endif
+ Up_PID.setCurrent(Upper_Position);
+ float speed =Up_PID.compute();
+#ifdef DEBUG_UP
+ printf("E_n= %f\n\r",Up_PID.getErrorNow());
+ printf("Kp = %f\n\r",Up_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Upper.speed(speed);
+}
+//******************************************************/
+/*
+void Move_Lower()
+{
+ Read_Encoder(EncoderB);
+ Lower_Position = (float)Get_EnValue(data);
+ Low_PID.setCurrent(Lower_Position);
+#ifdef DEBUG_LOW
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Lower_SetPoint);
+ printf("Upper_Position = %f\n\r",Lower_Position);
+#endif
+
+ float speed =Low_PID.compute();
+#ifdef DEBUG_LOW
+ printf("E_n= %f\n\r",Low_PID.getErrorNow());
+ printf("Kp = %f\n\r",Low_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Lower.speed(speed);
+}
+//******************************************************/
+
+
+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("s******************************");
+ }
+
+}
+/*******************************************************/
int main()
{
- if(!button) {
- while(!button);
- DebugMode(); //-->Debug.h
+ PC.baud(115200);
+ printf("******************");
+ /*
+ while(1)
+ {
+ Read_Encoder(EncoderA);
+ Upper_Position = Get_EnValue(data);
+ Read_Encoder(EncoderB);
+ Lower_Position = Get_EnValue(data);
+ PC.printf("Upper Position : %f\n",Upper_Position);
+ PC.printf("Lower_Position : %f\n",Lower_Position);
+ wait(0.5);
}
-
- UI ui(SW_WALK1,SW_WALK2,SW_WALK3,SW_SWEEP,SW_WATER,EMERGENCY);
-
- pc.printf("System Start\n");
+ */
+
+
+ //Recieve.attach(&Rc,0.025);
+ //Start_Up();
+
+ //SET_UpperPID();
+ // SET_LowerPID();
+
+ // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
+ /*
while(1)
{
- while(!ui.getEmergencyStatus()) {
- ui.RunSystem();
+
+ Upper.speed(-1);
+ wait_ms(400);
+ Upper.speed(1);
+ wait_ms(400);
+ Upper.break();
+
+ Lower.speed(-1.0);
+ wait_ms(401);
+ Lower.speed(1.0);
+ wait_ms(401);
+ Lower.break();
}
+ */
+
+ // counterUP.start();
+// counterLOW.start();
+
+ while(1) {
+
+ /*
+ //printf("timer = %d\n\r",counter.read_ms());
+ if(counterUP.read_ms() > 1400) {
+
+ if(Upper_SetPoint < 53) {
+ Upper_SetPoint++;
+ } else
+ Upper_SetPoint =18;
+
+ counterUP.reset();
+
+ }
+
+ if(counterLOW.read_ms() > 700) {
+
+ if(Lower_SetPoint < 100) {
+ Lower_SetPoint++;
+ } else
+ Lower_SetPoint =8;
+
+ counterLOW.reset();
+
+ }
+ */
+ // myled =1;
+ //wait_ms(10);
+///////////////////////////////////////////////////// start
+ //Set Set_point
+ // Up_PID.setGoal(Upper_SetPoint);
+ // Low_PID.setGoal(Lower_SetPoint);
+
+ //Control Motor
+ // Move_Upper();
+ // Move_Lower();
+///////////////////////////////////////////////////// stop =306us
+ //uint8_t aaa[1]={10};
+ //com.sendBodyWidth(0x01,aaa);
+ Rc();
+ //wait_ms(1);
}
}
-void DebugMode()
-{
- float temp;
- int option;
- bool first_time = true;
- Debug debug(SERIAL_TX,SERIAL_RX);
- do {
- debug.PrintListMode();
- option = debug.ScanInputData(1);
+
+
- if(option == 1) {
- do {
- temp = debug.Mode1();
- debug.PrintAll(temp);
- } while(temp!=9999);
- }
+
- else if(option == 2) {
- do {
- if(first_time==false) {
- temp = debug.Mode2();
- debug.PrintAll(temp);
- } else {
- temp = debug.Mode2();
- if(temp!=0) first_time = false;
- }
- if(temp==9999) first_time = true;
- } while(temp!=9999);
- }
- debug.setChange();
- } while(option!=9999);
-}
\ No newline at end of file
+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 Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VL=Int/1000;
+ PC.printf("VL=%f /n",VL);
+ break;
+ }
+ case SET_VELOCITY_RIGHT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VR=Int/1000;
+ break;
+ }
+ case SET_VELOCITY_MAX_LEFT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VLmax=Int/1000;
+ break;
+ }
+ case SET_VELOCITY_MAX_RIGHT: {
+ //
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ VRmax=Int/1000;
+ PC.printf("VRmax = %f",VRmax);
+ PC.printf("*****************************");
+ break;
+ }
+ case SET_KP_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KP_LEFT=Int/1000;
+ break;
+ }
+ case SET_KI_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KI_LEFT=Int/1000;
+ break;
+ }
+ case SET_KD_LEFT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KD_LEFT=Int/1000;
+ break;
+ }
+ case SET_KP_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KP_RIGHT=Int/1000;
+ break;
+ }
+ case SET_KI_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KI_RIGHT=Int/1000;
+ break;
+ }
+ case SET_KD_RIGHT: {
+ uint8_t int_buffer[2];
+ float Int;
+ int_buffer[0]=command[1];
+ int_buffer[1]=command[2];
+ Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
+ KD_RIGHT=Int/1000;
+ break;
+ }
+ }
+ } break;
+ case READ_DATA: {
+ switch (command[0]) {
+ case GET_LIDAR: {
+
+ 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: {
+ uint8_t intKPL[2],floatKPL[2];
+ com.FloatSep(KP_LEFT,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: {
+ uint8_t intKIL[2],floatKIL[2];
+ com.FloatSep(KI_LEFT,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: {
+ uint8_t intKDL[2],floatKDL[2];
+ com.FloatSep(KD_LEFT,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: {
+ uint8_t intKDR[2],floatKDR[2];
+ com.FloatSep(KP_RIGHT,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: {
+ uint8_t intKIR[2],floatKIR[2];
+ com.FloatSep(KI_RIGHT,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: {
+ uint8_t intKDR[2],floatKDR[2];
+ com.FloatSep(KD_RIGHT,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;
+
+ }
+ }
+}