main.cpp

00001 //*****************************************************/
00002 //  Include  //
00003 #include "mbed.h"
00004 #include "pinconfig.h"
00005 #include "PID.h"
00006 //#include "Motor.h"
00007 #include "eeprom.h"
00008 #include "Receiver.h"
00009 #include "Motion_EEPROM_Address.h"
00010 #include "move.h"
00011 #include "UNTRASONIC.h"
00012 #include "BufferedSerial.h"
00013 #include "rplidar.h"
00014 RPLidar lidar;
00015 //#include "pidcontrol.h"
00016 BufferedSerial se_lidar(PA_11,PA_12);
00017 #define EEPROM_DELAY 2
00018 DigitalOut rs485_dirc1(RS485_DIRC);
00019 //#define DEBUG_UP
00020 //#define DEBUG_LOW
00021 PwmOut VMO(PC_8);
00022 InterruptIn encoderA_d(PB_12);
00023 DigitalIn encoderB_d(PB_13);
00024 InterruptIn encoderA_1(PB_1);
00025 DigitalIn encoderB_1(PB_2);
00026 InterruptIn encoderA_2(PB_14);
00027 DigitalIn encoderB_2(PB_15);
00028 Timer timerStart;
00029 Timer tim;
00030 Timeout time_getsensor;
00031 Timeout time_distance;
00032 Timeout shutdown;
00033 move m1;
00034 //*****************************************************/
00035 
00036 // Global  //
00037 //timer
00038 int timer_now=0,timer_later=0;
00039 int times=0,timer_buffer=0;
00040 
00041 //encoder
00042 int Encoderpos = 0;
00043 int real_d=0;
00044 float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
00045 //pid
00046 
00047 double setp1=0,setp2=0;
00048 float outPID =0;
00049 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 ;
00050 PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
00051 PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
00052 //Ticker Recieve;
00053 //-- Communication --
00054 COMMUNICATION  *com1;
00055 
00056 //BufferedSerial PC(SERIAL_TX,SERIAL_RX);
00057 Serial PC(SERIAL_TX,SERIAL_RX);
00058 Bear_Receiver com(PA_9,PA_10,115200);
00059 int16_t MY_ID = 0x00;
00060 //-- Memorry --
00061 EEPROM memory(PB_4,PA_8,0);
00062 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;
00063 
00064 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
00065 void RC();
00066 
00067 //rplidar
00068 //float distances = 0;
00069 //float angle    = 0;
00070 //ool  startBit = 0;
00071 //char  quality =0 ;
00072 
00073 
00074 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
00075 
00076 
00077 DigitalOut myled(LED1);
00078 
00079 
00080 void Rx_interrupt()
00081 {
00082     //s1.get_motor();รับค่ามอเตอร์
00083     RC();
00084     timer_later= timer_now;
00085 
00086 }
00087 void EncoderA_1()//ซ้าย
00088 {
00089     if(encoderB_1==0) {
00090         Encoderpos = Encoderpos + 1;
00091     } else {
00092         Encoderpos = Encoderpos -1;
00093     }
00094     pulse_1+=1;
00095     //Encoderpos = Encoderpos + 1;
00096     //valocity+=1;
00097     //pc.printf("%d \n",Encoderpos);
00098     //pc.printf("pulse=%f  \n",pulse);
00099     //if(pulse==128)
00100     //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
00101 }
00102 void EncoderA_2()//ขวา
00103 {
00104     if(encoderB_2==0) {
00105         Encoderpos = Encoderpos + 1;
00106     } else {
00107         Encoderpos = Encoderpos -1;
00108     }
00109     pulse_2+=1;
00110     //pc.printf("%d",Encoderpos);
00111 }
00112 void EncoderA_D()
00113 {
00114     if(encoderB_d==0) {
00115         Encoderpos = Encoderpos + 1;
00116     } else {
00117         Encoderpos = Encoderpos -1;
00118     }
00119     pulse_d+=1;
00120     if(pulse_d==128) {
00121         Z_d+=1;
00122         pulse_d=0;
00123     }
00124 
00125 }
00126 void getvelo1()//จาก encoder
00127 {
00128     valocity1=pulse_1*((2*3.14*r)/128);
00129     PC.printf("valocity=%f  \n",valocity1);
00130     count=0;
00131     timerStart.reset();
00132 }
00133 void getvelo2()
00134 {
00135     valocity2=pulse_2*((2*3.14*r)/128);
00136     PC.printf("valocity=%f  \n",valocity2);
00137     count=0;
00138     timerStart.reset();
00139 }
00140 void get_d()//ระยะทาง
00141 {
00142     real_d=Z_d*(2*3.14*r);
00143     //ส่งข้อมูล
00144 
00145 }
00146 void get_rplidar()
00147 {
00148     if (IS_OK(lidar.waitPoint())) {
00149 
00150     } else {
00151 
00152         lidar.startScan();
00153 
00154     }
00155 
00156 }
00157 double map(double x, double in_min, double in_max, double out_min, double out_max)
00158 {
00159     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00160 
00161 }
00162 void PID_m1()//left
00163 {
00164     setp1=map(1.0,0.0,1.094,0.0,1.0);
00165     P1.setSetPoint(setp1);
00166     times=timerStart.read();
00167     if(times==1) { // m/s
00168         getvelo1();
00169         //pc.printf("TIME \n");
00170         times=0;
00171         pulse_1=0;
00172     }
00173     P1.setProcessValue(valocity1);
00174     outPID=P1.compute();
00175     //pc.printf("outPID=%f \n",outPID);
00176     m1.movespeed_1(setp1,outPID);
00177 }
00178 void PID_m2()//right
00179 {
00180     setp2=map(1.0,0.0,1.094,0.0,1.0);
00181     P2.setSetPoint(setp2);
00182     times=timerStart.read();
00183     if(times==1) { // m/s
00184         getvelo2();
00185         //pc.printf("TIME \n");
00186         times=0;
00187         pulse_2=0;
00188     }
00189     P2.setProcessValue(valocity2);
00190     outPID=P2.compute();
00191     //pc.printf("outPID=%f \n",outPID);
00192     m1.movespeed_2(setp2,outPID);
00193 }
00194 
00195 
00196 void RC()
00197 {
00198     myled =1;
00199     uint8_t data_array[30];
00200     uint8_t id=0;
00201     uint8_t ins=0;
00202     uint8_t status=0xFF;
00203 
00204 
00205 
00206     status = com.ReceiveCommand(&id,data_array,&ins);
00207     PC.printf("status = 0x%02x\n\r",status);
00208     if(status == ANDANTE_ERRBIT_NONE) {
00209         CmdCheck((int16_t)id,data_array,ins);
00210         PC.printf("RC******************************");
00211     }
00212 
00213 }
00214 /*******************************************************/
00215 int buf=0;
00216 float lidar_angle_max=360,lidar_angle_min=0;
00217 int main()
00218 {
00219     PC.baud(115200);
00220     lidar.begin(se_lidar);
00221     tim.start();
00222     //com1 = new COMMUNICATION(PA_9,PA_10,115200);
00223     encoderA_1.rise(&EncoderA_1);
00224     timerStart.start();
00225     P1.setMode(1);
00226     P1.setBias(0);
00227     //  pc.printf("READY \n");
00228     //pc.attach(&Rx_interrupt, Serial::RxIrq);
00229     lidar.setAngle(lidar_angle_min,lidar_angle_max);
00230     while(1) {
00231 
00232         VMO=1;
00233         get_rplidar();
00234         /*    if(tim.read_ms()-buf>=1000){
00235           for(int x=0;x<=359;x++){
00236             printf("%d,",lidar.Data[x]);
00237           }
00238           buf = tim.read_ms();
00239         }*/
00240         RC();
00241         //wait_ms(1);
00242     }
00243 }
00244 
00245 
00246 
00247 
00248 
00249 
00250 
00251 
00252 
00253 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
00254 {
00255     PC.printf("cmdcheck\n");
00256     if(id==MY_ID) {
00257         switch (ins) {
00258             case PING: {
00259                 break;
00260             }
00261             case WRITE_DATA: {
00262                 switch (command[0]) {
00263                     case SET_MAX_LIDAR_DEGREE: {
00264                         uint8_t int_buffer[2],float_buffer[2];
00265                         uint8_t int_buffer2[2],float_buffer2[2];
00266                         float Int,flo;
00267                         int_buffer[0]=command[1];
00268                         int_buffer[1]=command[2];
00269                         float_buffer[0]=command[3];
00270                         float_buffer[1]=command[4];
00271                         int_buffer2[0]=command[5];
00272                         int_buffer2[1]=command[6];
00273                         float_buffer2[0]=command[7];
00274                         float_buffer2[1]=command[8];
00275                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00276                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00277                         lidar_angle_max=Int+(flo/10000);
00278                         
00279                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer2);
00280                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer2);
00281                         lidar_angle_max=Int+(flo/10000);
00282                         PC.printf("Min,Max Degree of Lidar : %f %f/n",lidar_angle_min,lidar_angle_max);
00283                         break;
00284                     }
00285                     
00286                     case SET_VELOCITY_LEFT: {
00287                         uint8_t int_buffer[2],float_buffer[2];
00288                         float Int,flo;
00289                         int_buffer[0]=command[1];
00290                         int_buffer[1]=command[2];
00291                         float_buffer[0]=command[3];
00292                         float_buffer[1]=command[4];
00293                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00294                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00295                         VL=Int+(flo/10000);
00296                         PC.printf("VL=%f /n",VL);
00297                         break;
00298                     }
00299                     case SET_VELOCITY_RIGHT: {
00300                         //
00301                         uint8_t int_buffer[2],float_buffer[2];
00302                         float Int,flo;
00303                         int_buffer[0]=command[1];
00304                         int_buffer[1]=command[2];
00305                         float_buffer[0]=command[3];
00306                         float_buffer[1]=command[4];
00307                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00308                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00309                         VR=Int+flo;
00310                         PC.printf("VL=%f /n",VR);
00311                         break;
00312                     }
00313                     case SET_VELOCITY_MAX_LEFT: {
00314                         //
00315                         uint8_t int_buffer[2],float_buffer[2];
00316                         float Int,flo;
00317                         int_buffer[0]=command[1];
00318                         int_buffer[1]=command[2];
00319                         float_buffer[0]=command[3];
00320                         float_buffer[1]=command[4];
00321                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00322                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00323                         VLmax=Int+flo;
00324                         PC.printf("VL=%f /n",VLmax);
00325                         break;
00326                     }
00327                     case SET_VELOCITY_MAX_RIGHT: {
00328                         //
00329                         uint8_t int_buffer[2],float_buffer[2];
00330                         float Int,flo;
00331                         int_buffer[0]=command[1];
00332                         int_buffer[1]=command[2];
00333                         float_buffer[0]=command[3];
00334                         float_buffer[1]=command[4];
00335                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00336                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00337                         VRmax=Int+flo;
00338                         PC.printf("VRmax = %f",VRmax);
00339                         //  PC.printf("*****************************");
00340                         break;
00341                     }
00342                     //save to rom
00343                     case SET_KP_LEFT: {
00344                         uint8_t int_buffer[2],float_buffer[2];
00345                         float Int,flo;
00346                         int_buffer[0]=command[1];
00347                         int_buffer[1]=command[2];
00348                         float_buffer[0]=command[3];
00349                         float_buffer[1]=command[4];
00350                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00351                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00352 
00353                         KP_LEFT=Int+flo;
00354                         PC.printf("KP_LEFT=%f /n",KP_LEFT);
00355                         int32_t data_buff;
00356                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00357                         memory.write(ADDRESS_LEFT_KP,data_buff);
00358                         wait_ms(EEPROM_DELAY);
00359                         break;
00360                     }
00361                     case SET_KI_LEFT: {
00362                         uint8_t int_buffer[2],float_buffer[2];
00363                         float Int,flo;
00364                         int_buffer[0]=command[1];
00365                         int_buffer[1]=command[2];
00366                         float_buffer[0]=command[3];
00367                         float_buffer[1]=command[4];
00368                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00369                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00370 
00371                         KI_LEFT=Int+flo;
00372                         PC.printf("KI_LEFT=%f /n",KI_LEFT);
00373                         int32_t data_buff;
00374                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00375                         memory.write(ADDRESS_LEFT_KI ,data_buff);
00376                         wait_ms(EEPROM_DELAY);
00377                         break;
00378                     }
00379                     case SET_KD_LEFT: {
00380                         uint8_t int_buffer[2],float_buffer[2];
00381                         float Int,flo;
00382                         int_buffer[0]=command[1];
00383                         int_buffer[1]=command[2];
00384                         float_buffer[0]=command[3];
00385                         float_buffer[1]=command[4];
00386                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00387                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00388                         KD_LEFT=Int+flo;
00389                         PC.printf("KD_LEFT=%f /n",KD_LEFT);
00390                         int32_t data_buff;
00391                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00392                         memory.write(ADDRESS_LEFT_KD,data_buff);
00393                         wait_ms(EEPROM_DELAY);
00394                         break;
00395                     }
00396                     case  SET_KP_RIGHT: {
00397                         uint8_t int_buffer[2],float_buffer[2];
00398                         float Int,flo;
00399                         int_buffer[0]=command[1];
00400                         int_buffer[1]=command[2];
00401                         float_buffer[0]=command[3];
00402                         float_buffer[1]=command[4];
00403                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00404                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00405                         KP_RIGHT=Int+flo;
00406                         PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
00407                         int32_t data_buff;
00408                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00409                         memory.write(ADDRESS_RIGHT_KP,data_buff);
00410                         wait_ms(EEPROM_DELAY);
00411                         break;
00412                     }
00413                     case SET_KI_RIGHT: {
00414                         uint8_t int_buffer[2],float_buffer[2];
00415                         float Int,flo;
00416                         int_buffer[0]=command[1];
00417                         int_buffer[1]=command[2];
00418                         float_buffer[0]=command[3];
00419                         float_buffer[1]=command[4];
00420                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00421                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00422                         KI_RIGHT=Int+flo;
00423                         PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
00424                         int32_t data_buff;
00425                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00426                         memory.write(ADDRESS_RIGHT_KI,data_buff);
00427                         wait_ms(EEPROM_DELAY);
00428                         break;
00429                     }
00430                     case SET_KD_RIGHT: {
00431                         uint8_t int_buffer[2],float_buffer[2];
00432                         float Int,flo;
00433                         int_buffer[0]=command[1];
00434                         int_buffer[1]=command[2];
00435                         float_buffer[0]=command[3];
00436                         float_buffer[1]=command[4];
00437                         Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
00438                         flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
00439                         KD_RIGHT=Int+flo;
00440                         PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
00441                         int32_t data_buff;
00442                         data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
00443                         memory.write(ADDRESS_RIGHT_KD,data_buff);
00444                         wait_ms(EEPROM_DELAY);
00445                         break;
00446                     }
00447                 }
00448             }
00449             break;
00450             case READ_DATA: {
00451                 PC.printf("READ_DATA\n");
00452                 switch (command[0]) {
00453                     /*case GET_LIDAR1: {
00454                         uint8_t IntArray[2],FloatArray[2];
00455                         int numberK=0;
00456                         ANDANTE_PROTOCOL_PACKET package;
00457 
00458                         package.robotId = ID;
00459                         package.length = 362;
00460                         package.instructionErrorId = WRITE_DATA;
00461 
00462                         for(int i=1; i<=360; i++) {
00463                             if(i==2) {
00464                                 com.FloatSep(lidar.Data[numberK],IntArray,FloatArray);
00465                                 package.parameter[i-1]=IntArray[0];
00466                                 package.parameter[i]=IntArray[1];
00467                                 numberK++;
00468                             }
00469                         }
00470                         wait_us(RS485_DELAY);
00471                         com1->sendCommunicatePacket(&package);
00472                         break;
00473                     }*/
00474                     case GET_LIDAR2: {
00475                         int i=0,j=1,k=60;
00476                         uint8_t intData[2],floatData[2];
00477                         ANDANTE_PROTOCOL_PACKET package;
00478                         //BUFFER_SIZE=143
00479                         package.robotId = MY_ID;
00480                         package.length = 122;
00481                         package.instructionErrorId = WRITE_DATA;
00482 
00483                         while(k<120) {
00484                             //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
00485                             com.FloatSep( lidar.Data[k],intData,floatData);
00486                             package.parameter[i]=intData[0];
00487                             package.parameter[j]=intData[1];
00488                             i=i+2;
00489                             j=j+2;
00490                             k++;
00491 
00492                         }
00493                         ////rs485_dirc1=1;
00494                         wait_us(RS485_DELAY);
00495                         com1->sendCommunicatePacket(&package);
00496 
00497                         break;
00498                     }
00499                     case GET_BATTERY: {
00500 
00501                         break;
00502                     }
00503                     case GET_VELOCITY_LEFT: {
00504                         uint8_t intVelo_L[2],floatVelo_L[2];
00505                         com.FloatSep(valocity1,intVelo_L,floatVelo_L);
00506                         ANDANTE_PROTOCOL_PACKET package;
00507 
00508                         package.robotId = MY_ID;
00509                         package.length = 6;
00510                         package.instructionErrorId = WRITE_DATA;
00511                         package.parameter[0]=intVelo_L[0];
00512                         package.parameter[1]=intVelo_L[1];
00513                         package.parameter[2]=floatVelo_L[0];
00514                         package.parameter[3]=floatVelo_L[1];
00515 
00516                         ////rs485_dirc1=1;
00517                         wait_us(RS485_DELAY);
00518                         com1->sendCommunicatePacket(&package);
00519 
00520 
00521                         break;
00522                     }
00523                     case GET_VELOCITY_RIGHT : {
00524                         uint8_t intVelo_R[2],floatVelo_R[2];
00525                         com.FloatSep(valocity2,intVelo_R,floatVelo_R);
00526 
00527 
00528                         ANDANTE_PROTOCOL_PACKET package;
00529 
00530                         package.robotId = MY_ID;
00531                         package.length = 6;
00532                         package.instructionErrorId = WRITE_DATA;
00533                         package.parameter[0]=intVelo_R[0];
00534                         package.parameter[1]=intVelo_R[1];
00535                         package.parameter[2]=floatVelo_R[0];
00536                         package.parameter[3]=floatVelo_R[1];
00537 
00538                         ////rs485_dirc1=1;
00539                         wait_us(RS485_DELAY);
00540                         com1->sendCommunicatePacket(&package);
00541 
00542                         break;
00543                     }
00544                     case GET_KP_LEFT: {
00545                         memory.read(ADDRESS_LEFT_KP ,KP_LEFT);
00546                         memory.read(ADDRESS_LEFT_KP ,KI_LEFT);
00547                         memory.read(ADDRESS_LEFT_KP ,KD_LEFT);
00548                         
00549                         com.sendKpKiKdLeft(KP_LEFT,KI_LEFT,KD_LEFT);
00550                         
00551                         /*uint8_t IntKp[2],FloatKp[2];
00552                         uint8_t IntKi[2],FloatKi[2];
00553                         uint8_t IntKd[2],FloatKd[2];
00554 
00555                         com.FloatSep(KP_LEFT_BUFF,IntKp,FloatKp);
00556                         com.FloatSep(KI_LEFT_BUFF,IntKi,FloatKi);
00557                         com.FloatSep(KD_LEFT_BUFF,IntKd,FloatKd);
00558 
00559 
00560                         ANDANTE_PROTOCOL_PACKET package;
00561 
00562                         package.robotId = ID;
00563                         package.length = 14;
00564                         package.instructionErrorId = WRITE_DATA;
00565                         package.parameter[0]=IntKp[0];
00566                         package.parameter[1]=IntKp[1];
00567                         package.parameter[2]=FloatKp[0];
00568                         package.parameter[3]=FloatKp[1];
00569                         package.parameter[4]=IntKi[0];
00570                         package.parameter[5]=IntKi[1];
00571                         package.parameter[6]=FloatKi[0];
00572                         package.parameter[7]=FloatKi[1];
00573                         package.parameter[8]=IntKd[0];
00574                         package.parameter[9]=IntKd[1];
00575                         package.parameter[10]=FloatKd[0];
00576                         package.parameter[11]=FloatKd[1];
00577 
00578                         
00579                         wait_us(RS485_DELAY);
00580                         com1->sendCommunicatePacket(&package);*/
00581                         break;
00582                     }
00583                     case GET_KI_LEFT: {
00584                         memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
00585                         uint8_t intKIL[2],floatKIL[2];
00586                         com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
00587 
00588 
00589                         ANDANTE_PROTOCOL_PACKET package;
00590 
00591                         package.robotId = MY_ID;
00592                         package.length = 6;
00593                         package.instructionErrorId = WRITE_DATA;
00594                         package.parameter[0]=intKIL[0];
00595                         package.parameter[1]=intKIL[1];
00596                         package.parameter[2]=floatKIL[0];
00597                         package.parameter[3]=floatKIL[1];
00598 
00599                         ////rs485_dirc1=1;
00600                         wait_us(RS485_DELAY);
00601                         com1->sendCommunicatePacket(&package);
00602 
00603                         break;
00604                     }
00605                     case GET_KD_LEFT: {
00606                         memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
00607                         uint8_t intKDL[2],floatKDL[2];
00608                         com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
00609 
00610 
00611                         ANDANTE_PROTOCOL_PACKET package;
00612 
00613                         package.robotId = MY_ID;
00614                         package.length = 6;
00615                         package.instructionErrorId = WRITE_DATA;
00616                         package.parameter[0]=intKDL[0];
00617                         package.parameter[1]=intKDL[1];
00618                         package.parameter[2]=floatKDL[0];
00619                         package.parameter[3]=floatKDL[1];
00620 
00621                         ////rs485_dirc1=1;
00622                         wait_us(RS485_DELAY);
00623                         com1->sendCommunicatePacket(&package);
00624 
00625                         break;
00626                     }
00627                     case GET_KP_RIGHT: {
00628                         memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
00629                         uint8_t intKDR[2],floatKDR[2];
00630                         com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
00631 
00632 
00633                         ANDANTE_PROTOCOL_PACKET package;
00634 
00635                         package.robotId = MY_ID;
00636                         package.length = 6;
00637                         package.instructionErrorId = WRITE_DATA;
00638                         package.parameter[0]=intKDR[0];
00639                         package.parameter[1]=intKDR[1];
00640                         package.parameter[2]=floatKDR[0];
00641                         package.parameter[3]=floatKDR[1];
00642 
00643                         ////rs485_dirc1=1;
00644                         wait_us(RS485_DELAY);
00645                         com1->sendCommunicatePacket(&package);
00646 
00647                         break;
00648                     }
00649                     case GET_KI_RIGHT: {
00650                         memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
00651                         uint8_t intKIR[2],floatKIR[2];
00652                         com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
00653 
00654 
00655                         ANDANTE_PROTOCOL_PACKET package;
00656 
00657                         package.robotId = MY_ID;
00658                         package.length = 6;
00659                         package.instructionErrorId = WRITE_DATA;
00660                         package.parameter[0]=intKIR[0];
00661                         package.parameter[1]=intKIR[1];
00662                         package.parameter[2]=floatKIR[0];
00663                         package.parameter[3]=floatKIR[1];
00664 
00665                         ////rs485_dirc1=1;
00666                         wait_us(RS485_DELAY);
00667                         com1->sendCommunicatePacket(&package);
00668 
00669                         break;
00670                     }
00671                     case GET_KD_RIGHT: {
00672                         memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
00673                         uint8_t intKDR[2],floatKDR[2];
00674                         com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
00675 
00676 
00677                         ANDANTE_PROTOCOL_PACKET package;
00678 
00679                         package.robotId = MY_ID;
00680                         package.length = 6;
00681                         package.instructionErrorId = WRITE_DATA;
00682                         package.parameter[0]=intKDR[0];
00683                         package.parameter[1]=intKDR[1];
00684                         package.parameter[2]=floatKDR[0];
00685                         package.parameter[3]=floatKDR[1];
00686 
00687                         ////rs485_dirc1=1;
00688                         wait_us(RS485_DELAY);
00689                         com1->sendCommunicatePacket(&package);
00690 
00691                         break;
00692                     }
00693                     case GET_MAX_LIDAR_DEGREE: {
00694                         com.sendMaxMinLidarDegree(lidar_angle_max,lidar_angle_min);
00695                         /*uint8_t intMax[2],floatMax[2];
00696                         uint8_t intMin[2],floatMin[2];
00697                         com.FloatSep(lidar_angle_max,intMax,floatMax);
00698                         com.FloatSep(lidar_angle_min,intMin,floatMin);
00699                         ANDANTE_PROTOCOL_PACKET package;
00700 
00701                         package.robotId = MY_ID;
00702                         package.length = 10;
00703                         package.instructionErrorId = WRITE_DATA;
00704                         package.parameter[0]=intMax[0];
00705                         package.parameter[1]=intMax[1];
00706                         package.parameter[2]=floatMax[0];
00707                         package.parameter[3]=floatMax[1];
00708                         package.parameter[4]=intMin[0];
00709                         package.parameter[5]=intMin[1];
00710                         package.parameter[6]=floatMin[0];
00711                         package.parameter[7]=floatMin[1];
00712 
00713                         ////rs485_dirc1=1;
00714                         wait_us(RS485_DELAY);
00715                         com1->sendCommunicatePacket(&package);*/
00716                     }
00717                     case GET_LIDAR1: {
00718                         com.sendSensorDataAll1(lidar.Data);
00719                     }
00720                 }
00721             }
00722             break;
00723 
00724         }
00725     }
00726 }