Betago / Mbed 2 deprecated clean_V1

v1

Dependencies:   BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed

Fork of CleaningMachine_Betago by palm and chin

main.cpp@6:adf1f4351f9f, 2016-06-07 (annotated)

Committer:
palmdotax
Date:
Tue Jun 07 03:28:39 2016 +0000
Revision:
6:adf1f4351f9f
Parent:
5:fe76f3dae81e 
v1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
palmdotax 1:45f1573d65a1 1 //*****************************************************/
palmdotax 1:45f1573d65a1 2 // Include //
palmdotax 1:45f1573d65a1 3 #include "mbed.h"
icyzkungz 0:84f05cd2f197 4 #include "pinconfig.h"
palmdotax 1:45f1573d65a1 5 #include "PID.h"
palmdotax 1:45f1573d65a1 6 //#include "Motor.h"
palmdotax 1:45f1573d65a1 7 #include "eeprom.h"
palmdotax 1:45f1573d65a1 8 #include "Receiver.h"
palmdotax 1:45f1573d65a1 9 #include "Motion_EEPROM_Address.h"
palmdotax 1:45f1573d65a1 10 #include "move.h"
palmdotax 1:45f1573d65a1 11 #include "UNTRASONIC.h"
palmdotax 2:f873deba2305 12 #include "BufferedSerial.h"
palmdotax 2:f873deba2305 13 #include "rplidar.h"
palmdotax 2:f873deba2305 14 RPLidar lidar;
palmdotax 1:45f1573d65a1 15 //#include "pidcontrol.h"
palmdotax 5:fe76f3dae81e 16 BufferedSerial se_lidar(PA_11,PA_12);
palmdotax 1:45f1573d65a1 17 #define EEPROM_DELAY 2
palmdotax 1:45f1573d65a1 18 DigitalOut rs485_dirc1(RS485_DIRC);
palmdotax 1:45f1573d65a1 19 //#define DEBUG_UP
palmdotax 1:45f1573d65a1 20 //#define DEBUG_LOW
palmdotax 5:fe76f3dae81e 21 PwmOut VMO(PC_8);
palmdotax 1:45f1573d65a1 22 InterruptIn encoderA_d(PB_12);
palmdotax 1:45f1573d65a1 23 DigitalIn encoderB_d(PB_13);
palmdotax 1:45f1573d65a1 24 InterruptIn encoderA_1(PB_1);
palmdotax 1:45f1573d65a1 25 DigitalIn encoderB_1(PB_2);
palmdotax 1:45f1573d65a1 26 InterruptIn encoderA_2(PB_14);
palmdotax 1:45f1573d65a1 27 DigitalIn encoderB_2(PB_15);
palmdotax 1:45f1573d65a1 28 Timer timerStart;
palmdotax 5:fe76f3dae81e 29 Timer tim;
palmdotax 1:45f1573d65a1 30 Timeout time_getsensor;
palmdotax 1:45f1573d65a1 31 Timeout time_distance;
palmdotax 1:45f1573d65a1 32 Timeout shutdown;
palmdotax 1:45f1573d65a1 33 move m1;
palmdotax 1:45f1573d65a1 34 //*****************************************************/
palmdotax 2:f873deba2305 35
palmdotax 1:45f1573d65a1 36 // Global //
palmdotax 1:45f1573d65a1 37 //timer
palmdotax 1:45f1573d65a1 38 int timer_now=0,timer_later=0;
palmdotax 1:45f1573d65a1 39 int times=0,timer_buffer=0;
palmdotax 1:45f1573d65a1 40
palmdotax 1:45f1573d65a1 41 //encoder
palmdotax 1:45f1573d65a1 42 int Encoderpos = 0;
palmdotax 1:45f1573d65a1 43 int real_d=0;
palmdotax 1:45f1573d65a1 44 float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
palmdotax 1:45f1573d65a1 45 //pid
palmdotax 1:45f1573d65a1 46
palmdotax 1:45f1573d65a1 47 double setp1=0,setp2=0;
palmdotax 1:45f1573d65a1 48 float outPID =0;
palmdotax 2:f873deba2305 49 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 ;
palmdotax 1:45f1573d65a1 50 PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
palmdotax 1:45f1573d65a1 51 PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
palmdotax 1:45f1573d65a1 52 //Ticker Recieve;
palmdotax 1:45f1573d65a1 53 //-- Communication --
palmdotax 5:fe76f3dae81e 54 COMMUNICATION *com1;
palmdotax 5:fe76f3dae81e 55
palmdotax 4:de5a65c17664 56 //BufferedSerial PC(SERIAL_TX,SERIAL_RX);
palmdotax 4:de5a65c17664 57 Serial PC(SERIAL_TX,SERIAL_RX);
palmdotax 4:de5a65c17664 58 Bear_Receiver com(PA_9,PA_10,115200);
palmdotax 1:45f1573d65a1 59 int16_t MY_ID = 0x00;
palmdotax 1:45f1573d65a1 60 //-- Memorry --
palmdotax 1:45f1573d65a1 61 EEPROM memory(PB_4,PA_8,0);
palmdotax 2:f873deba2305 62 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;
palmdotax 1:45f1573d65a1 63
palmdotax 2:f873deba2305 64 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
palmdotax 2:f873deba2305 65 void RC();
palmdotax 1:45f1573d65a1 66
palmdotax 2:f873deba2305 67 //rplidar
palmdotax 4:de5a65c17664 68 //float distances = 0;
palmdotax 4:de5a65c17664 69 //float angle = 0;
palmdotax 4:de5a65c17664 70 //ool startBit = 0;
palmdotax 4:de5a65c17664 71 //char quality =0 ;
palmdotax 1:45f1573d65a1 72
palmdotax 1:45f1573d65a1 73
palmdotax 1:45f1573d65a1 74 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
palmdotax 1:45f1573d65a1 75
palmdotax 1:45f1573d65a1 76
palmdotax 1:45f1573d65a1 77 DigitalOut myled(LED1);
palmdotax 1:45f1573d65a1 78
icyzkungz 0:84f05cd2f197 79
palmdotax 1:45f1573d65a1 80 void Rx_interrupt()
palmdotax 1:45f1573d65a1 81 {
palmdotax 1:45f1573d65a1 82 //s1.get_motor();รับค่ามอเตอร์
palmdotax 1:45f1573d65a1 83 RC();
palmdotax 1:45f1573d65a1 84 timer_later= timer_now;
palmdotax 1:45f1573d65a1 85
palmdotax 1:45f1573d65a1 86 }
palmdotax 1:45f1573d65a1 87 void EncoderA_1()//ซ้าย
palmdotax 1:45f1573d65a1 88 { if(encoderB_1==0)
palmdotax 1:45f1573d65a1 89 { Encoderpos = Encoderpos + 1;}
palmdotax 1:45f1573d65a1 90 else
palmdotax 1:45f1573d65a1 91 { Encoderpos = Encoderpos -1;}
palmdotax 1:45f1573d65a1 92 pulse_1+=1;
palmdotax 1:45f1573d65a1 93 //Encoderpos = Encoderpos + 1;
palmdotax 1:45f1573d65a1 94 //valocity+=1;
palmdotax 1:45f1573d65a1 95 //pc.printf("%d \n",Encoderpos);
palmdotax 1:45f1573d65a1 96 //pc.printf("pulse=%f \n",pulse);
palmdotax 1:45f1573d65a1 97 //if(pulse==128)
palmdotax 1:45f1573d65a1 98 //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
palmdotax 1:45f1573d65a1 99 }
palmdotax 1:45f1573d65a1 100 void EncoderA_2()//ขวา
palmdotax 1:45f1573d65a1 101 {
palmdotax 1:45f1573d65a1 102 if(encoderB_2==0)
palmdotax 1:45f1573d65a1 103 { Encoderpos = Encoderpos + 1;}
palmdotax 1:45f1573d65a1 104 else
palmdotax 1:45f1573d65a1 105 { Encoderpos = Encoderpos -1;}
palmdotax 1:45f1573d65a1 106 pulse_2+=1;
palmdotax 1:45f1573d65a1 107 //pc.printf("%d",Encoderpos);
palmdotax 1:45f1573d65a1 108 }
palmdotax 1:45f1573d65a1 109 void EncoderA_D()
palmdotax 1:45f1573d65a1 110 {
palmdotax 1:45f1573d65a1 111 if(encoderB_d==0)
palmdotax 1:45f1573d65a1 112 { Encoderpos = Encoderpos + 1;}
palmdotax 1:45f1573d65a1 113 else
palmdotax 1:45f1573d65a1 114 { Encoderpos = Encoderpos -1;}
palmdotax 1:45f1573d65a1 115 pulse_d+=1;
palmdotax 1:45f1573d65a1 116 if(pulse_d==128)
palmdotax 1:45f1573d65a1 117 {
palmdotax 1:45f1573d65a1 118 Z_d+=1;
palmdotax 1:45f1573d65a1 119 pulse_d=0;
palmdotax 1:45f1573d65a1 120 }
palmdotax 2:f873deba2305 121
palmdotax 1:45f1573d65a1 122 }
palmdotax 1:45f1573d65a1 123 void getvelo1()//จาก encoder
palmdotax 1:45f1573d65a1 124 {
palmdotax 1:45f1573d65a1 125 valocity1=pulse_1*((2*3.14*r)/128);
palmdotax 2:f873deba2305 126 PC.printf("valocity=%f \n",valocity1);
palmdotax 1:45f1573d65a1 127 count=0;
palmdotax 1:45f1573d65a1 128 timerStart.reset();
palmdotax 1:45f1573d65a1 129 }
palmdotax 1:45f1573d65a1 130 void getvelo2()
palmdotax 1:45f1573d65a1 131 {
palmdotax 1:45f1573d65a1 132 valocity2=pulse_2*((2*3.14*r)/128);
palmdotax 2:f873deba2305 133 PC.printf("valocity=%f \n",valocity2);
palmdotax 1:45f1573d65a1 134 count=0;
palmdotax 1:45f1573d65a1 135 timerStart.reset();
palmdotax 1:45f1573d65a1 136 }
palmdotax 1:45f1573d65a1 137 void get_d()//ระยะทาง
palmdotax 1:45f1573d65a1 138 {
palmdotax 1:45f1573d65a1 139 real_d=Z_d*(2*3.14*r);
palmdotax 1:45f1573d65a1 140 //ส่งข้อมูล
palmdotax 1:45f1573d65a1 141
palmdotax 1:45f1573d65a1 142 }
palmdotax 2:f873deba2305 143 void get_rplidar()
palmdotax 2:f873deba2305 144 {
palmdotax 4:de5a65c17664 145 if (IS_OK(lidar.waitPoint()))
palmdotax 4:de5a65c17664 146 {
palmdotax 4:de5a65c17664 147
palmdotax 4:de5a65c17664 148 }
palmdotax 4:de5a65c17664 149 else
palmdotax 4:de5a65c17664 150 {
palmdotax 4:de5a65c17664 151
palmdotax 2:f873deba2305 152 lidar.startScan();
palmdotax 4:de5a65c17664 153
palmdotax 2:f873deba2305 154 }
palmdotax 2:f873deba2305 155
palmdotax 2:f873deba2305 156 }
palmdotax 1:45f1573d65a1 157 double map(double x, double in_min, double in_max, double out_min, double out_max)
palmdotax 1:45f1573d65a1 158 {
palmdotax 1:45f1573d65a1 159 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
palmdotax 1:45f1573d65a1 160
palmdotax 1:45f1573d65a1 161 }
palmdotax 1:45f1573d65a1 162 void PID_m1()//left
palmdotax 1:45f1573d65a1 163 {
palmdotax 1:45f1573d65a1 164 setp1=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 1:45f1573d65a1 165 P1.setSetPoint(setp1);
palmdotax 1:45f1573d65a1 166 times=timerStart.read();
palmdotax 1:45f1573d65a1 167 if(times==1)// m/s
palmdotax 1:45f1573d65a1 168 {
palmdotax 1:45f1573d65a1 169 getvelo1();
palmdotax 1:45f1573d65a1 170 //pc.printf("TIME \n");
palmdotax 1:45f1573d65a1 171 times=0;
palmdotax 1:45f1573d65a1 172 pulse_1=0;
palmdotax 1:45f1573d65a1 173 }
palmdotax 1:45f1573d65a1 174 P1.setProcessValue(valocity1);
palmdotax 1:45f1573d65a1 175 outPID=P1.compute();
palmdotax 1:45f1573d65a1 176 //pc.printf("outPID=%f \n",outPID);
palmdotax 1:45f1573d65a1 177 m1.movespeed_1(setp1,outPID);
palmdotax 1:45f1573d65a1 178 }
palmdotax 1:45f1573d65a1 179 void PID_m2()//right
palmdotax 1:45f1573d65a1 180 {
palmdotax 1:45f1573d65a1 181 setp2=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 1:45f1573d65a1 182 P2.setSetPoint(setp2);
palmdotax 1:45f1573d65a1 183 times=timerStart.read();
palmdotax 1:45f1573d65a1 184 if(times==1)// m/s
palmdotax 1:45f1573d65a1 185 {
palmdotax 1:45f1573d65a1 186 getvelo2();
palmdotax 1:45f1573d65a1 187 //pc.printf("TIME \n");
palmdotax 1:45f1573d65a1 188 times=0;
palmdotax 1:45f1573d65a1 189 pulse_2=0;
palmdotax 1:45f1573d65a1 190 }
palmdotax 1:45f1573d65a1 191 P2.setProcessValue(valocity2);
palmdotax 1:45f1573d65a1 192 outPID=P2.compute();
palmdotax 1:45f1573d65a1 193 //pc.printf("outPID=%f \n",outPID);
palmdotax 2:f873deba2305 194 m1.movespeed_2(setp2,outPID);
palmdotax 1:45f1573d65a1 195 }
palmdotax 1:45f1573d65a1 196
palmdotax 1:45f1573d65a1 197
palmdotax 2:f873deba2305 198 void RC()
palmdotax 1:45f1573d65a1 199 {
palmdotax 1:45f1573d65a1 200 myled =1;
palmdotax 1:45f1573d65a1 201 uint8_t data_array[30];
palmdotax 1:45f1573d65a1 202 uint8_t id=0;
palmdotax 1:45f1573d65a1 203 uint8_t ins=0;
palmdotax 1:45f1573d65a1 204 uint8_t status=0xFF;
palmdotax 1:45f1573d65a1 205
palmdotax 1:45f1573d65a1 206
palmdotax 1:45f1573d65a1 207
palmdotax 1:45f1573d65a1 208 status = com.ReceiveCommand(&id,data_array,&ins);
palmdotax 1:45f1573d65a1 209 PC.printf("status = 0x%02x\n\r",status);
palmdotax 1:45f1573d65a1 210 if(status == ANDANTE_ERRBIT_NONE) {
palmdotax 1:45f1573d65a1 211 CmdCheck((int16_t)id,data_array,ins);
palmdotax 5:fe76f3dae81e 212 PC.printf("RC******************************");
palmdotax 1:45f1573d65a1 213 }
palmdotax 1:45f1573d65a1 214
palmdotax 1:45f1573d65a1 215 }
palmdotax 1:45f1573d65a1 216 /*******************************************************/
palmdotax 5:fe76f3dae81e 217 int buf=0;
icyzkungz 0:84f05cd2f197 218 int main()
icyzkungz 0:84f05cd2f197 219 {
palmdotax 1:45f1573d65a1 220 PC.baud(115200);
palmdotax 5:fe76f3dae81e 221 lidar.begin(se_lidar);
palmdotax 5:fe76f3dae81e 222 tim.start();
palmdotax 5:fe76f3dae81e 223 //com1 = new COMMUNICATION(PA_9,PA_10,115200);
palmdotax 2:f873deba2305 224 encoderA_1.rise(&EncoderA_1);
palmdotax 2:f873deba2305 225 timerStart.start();
palmdotax 2:f873deba2305 226 P1.setMode(1);
palmdotax 2:f873deba2305 227 P1.setBias(0);
palmdotax 2:f873deba2305 228 // pc.printf("READY \n");
palmdotax 2:f873deba2305 229 //pc.attach(&Rx_interrupt, Serial::RxIrq);
palmdotax 5:fe76f3dae81e 230 lidar.setAngle(0,360);
palmdotax 1:45f1573d65a1 231 while(1) {
palmdotax 1:45f1573d65a1 232
palmdotax 5:fe76f3dae81e 233 VMO=1;
palmdotax 5:fe76f3dae81e 234 get_rplidar();
palmdotax 5:fe76f3dae81e 235 /* if(tim.read_ms()-buf>=1000){
palmdotax 5:fe76f3dae81e 236 for(int x=0;x<=359;x++){
palmdotax 5:fe76f3dae81e 237 printf("%d,",lidar.Data[x]);
palmdotax 5:fe76f3dae81e 238 }
palmdotax 5:fe76f3dae81e 239 buf = tim.read_ms();
palmdotax 5:fe76f3dae81e 240 }*/
palmdotax 2:f873deba2305 241 RC();
palmdotax 1:45f1573d65a1 242 //wait_ms(1);
icyzkungz 0:84f05cd2f197 243 }
icyzkungz 0:84f05cd2f197 244 }
icyzkungz 0:84f05cd2f197 245
palmdotax 1:45f1573d65a1 246
palmdotax 1:45f1573d65a1 247
icyzkungz 0:84f05cd2f197 248
palmdotax 1:45f1573d65a1 249
icyzkungz 0:84f05cd2f197 250
icyzkungz 0:84f05cd2f197 251
icyzkungz 0:84f05cd2f197 252
icyzkungz 0:84f05cd2f197 253
palmdotax 1:45f1573d65a1 254 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
palmdotax 1:45f1573d65a1 255 {
palmdotax 1:45f1573d65a1 256 PC.printf("cmdcheck\n");
palmdotax 1:45f1573d65a1 257 if(id==MY_ID) {
palmdotax 1:45f1573d65a1 258 switch (ins) {
palmdotax 1:45f1573d65a1 259 case PING: {
palmdotax 1:45f1573d65a1 260 break;
palmdotax 1:45f1573d65a1 261 }
palmdotax 1:45f1573d65a1 262 case WRITE_DATA: {
palmdotax 1:45f1573d65a1 263 switch (command[0]) {
palmdotax 1:45f1573d65a1 264 case ID: {
palmdotax 1:45f1573d65a1 265 ///
palmdotax 1:45f1573d65a1 266 MY_ID = (int16_t)command[1];
palmdotax 1:45f1573d65a1 267 break;
palmdotax 1:45f1573d65a1 268 }
palmdotax 1:45f1573d65a1 269 case SET_VELOCITY_LEFT: {
palmdotax 1:45f1573d65a1 270 //
palmdotax 4:de5a65c17664 271 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 272 float Int,flo;
palmdotax 1:45f1573d65a1 273 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 274 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 275 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 276 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 277 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 278 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 279 VL=Int+(flo/10000);
palmdotax 1:45f1573d65a1 280 PC.printf("VL=%f /n",VL);
palmdotax 1:45f1573d65a1 281 break;
palmdotax 1:45f1573d65a1 282 }
palmdotax 1:45f1573d65a1 283 case SET_VELOCITY_RIGHT: {
palmdotax 1:45f1573d65a1 284 //
palmdotax 4:de5a65c17664 285 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 286 float Int,flo;
palmdotax 1:45f1573d65a1 287 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 288 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 289 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 290 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 291 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 292 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 293 VR=Int+flo;
palmdotax 4:de5a65c17664 294 PC.printf("VL=%f /n",VR);
palmdotax 1:45f1573d65a1 295 break;
palmdotax 1:45f1573d65a1 296 }
palmdotax 1:45f1573d65a1 297 case SET_VELOCITY_MAX_LEFT: {
palmdotax 1:45f1573d65a1 298 //
palmdotax 4:de5a65c17664 299 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 300 float Int,flo;
palmdotax 1:45f1573d65a1 301 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 302 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 303 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 304 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 305 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 306 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 307 VLmax=Int+flo;
palmdotax 4:de5a65c17664 308 PC.printf("VL=%f /n",VLmax);
palmdotax 1:45f1573d65a1 309 break;
palmdotax 1:45f1573d65a1 310 }
palmdotax 1:45f1573d65a1 311 case SET_VELOCITY_MAX_RIGHT: {
palmdotax 1:45f1573d65a1 312 //
palmdotax 4:de5a65c17664 313 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 314 float Int,flo;
palmdotax 1:45f1573d65a1 315 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 316 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 317 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 318 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 319 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 320 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 321 VRmax=Int+flo;
palmdotax 1:45f1573d65a1 322 PC.printf("VRmax = %f",VRmax);
palmdotax 4:de5a65c17664 323 // PC.printf("*****************************");
palmdotax 1:45f1573d65a1 324 break;
palmdotax 1:45f1573d65a1 325 }
palmdotax 2:f873deba2305 326 //save to rom
palmdotax 1:45f1573d65a1 327 case SET_KP_LEFT: {
palmdotax 4:de5a65c17664 328 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 329 float Int,flo;
palmdotax 1:45f1573d65a1 330 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 331 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 332 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 333 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 334 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 335 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 336
palmdotax 4:de5a65c17664 337 KP_LEFT=Int+flo;
palmdotax 4:de5a65c17664 338 PC.printf("KP_LEFT=%f /n",KP_LEFT);
palmdotax 2:f873deba2305 339 int32_t data_buff;
palmdotax 2:f873deba2305 340 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 341 memory.write(ADDRESS_LEFT_KP,data_buff);
palmdotax 2:f873deba2305 342 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 343 break;
palmdotax 1:45f1573d65a1 344 }
palmdotax 1:45f1573d65a1 345 case SET_KI_LEFT: {
palmdotax 4:de5a65c17664 346 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 347 float Int,flo;
palmdotax 1:45f1573d65a1 348 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 349 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 350 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 351 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 352 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 353 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 354
palmdotax 4:de5a65c17664 355 KI_LEFT=Int+flo;
palmdotax 4:de5a65c17664 356 PC.printf("KI_LEFT=%f /n",KI_LEFT);
palmdotax 2:f873deba2305 357 int32_t data_buff;
palmdotax 2:f873deba2305 358 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 359 memory.write(ADDRESS_LEFT_KI ,data_buff);
palmdotax 2:f873deba2305 360 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 361 break;
palmdotax 1:45f1573d65a1 362 }
palmdotax 1:45f1573d65a1 363 case SET_KD_LEFT: {
palmdotax 4:de5a65c17664 364 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 365 float Int,flo;
palmdotax 1:45f1573d65a1 366 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 367 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 368 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 369 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 370 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 371 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 5:fe76f3dae81e 372 KD_LEFT=Int+flo;
palmdotax 4:de5a65c17664 373 PC.printf("KD_LEFT=%f /n",KD_LEFT);
palmdotax 2:f873deba2305 374 int32_t data_buff;
palmdotax 2:f873deba2305 375 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 376 memory.write(ADDRESS_LEFT_KD,data_buff);
palmdotax 2:f873deba2305 377 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 378 break;
palmdotax 1:45f1573d65a1 379 }
palmdotax 1:45f1573d65a1 380 case SET_KP_RIGHT: {
palmdotax 4:de5a65c17664 381 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 382 float Int,flo;
palmdotax 1:45f1573d65a1 383 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 384 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 385 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 386 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 387 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 388 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 5:fe76f3dae81e 389 KP_RIGHT=Int+flo;
palmdotax 4:de5a65c17664 390 PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
palmdotax 2:f873deba2305 391 int32_t data_buff;
palmdotax 2:f873deba2305 392 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 393 memory.write(ADDRESS_RIGHT_KP,data_buff);
palmdotax 2:f873deba2305 394 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 395 break;
palmdotax 1:45f1573d65a1 396 }
palmdotax 1:45f1573d65a1 397 case SET_KI_RIGHT: {
palmdotax 4:de5a65c17664 398 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 399 float Int,flo;
palmdotax 1:45f1573d65a1 400 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 401 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 402 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 403 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 404 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 405 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 5:fe76f3dae81e 406 KI_RIGHT=Int+flo;
palmdotax 4:de5a65c17664 407 PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
palmdotax 2:f873deba2305 408 int32_t data_buff;
palmdotax 2:f873deba2305 409 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 410 memory.write(ADDRESS_RIGHT_KI,data_buff);
palmdotax 2:f873deba2305 411 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 412 break;
palmdotax 1:45f1573d65a1 413 }
palmdotax 1:45f1573d65a1 414 case SET_KD_RIGHT: {
palmdotax 4:de5a65c17664 415 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 416 float Int,flo;
palmdotax 1:45f1573d65a1 417 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 418 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 419 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 420 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 421 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 422 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 5:fe76f3dae81e 423 KD_RIGHT=Int+flo;
palmdotax 4:de5a65c17664 424 PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
palmdotax 2:f873deba2305 425 int32_t data_buff;
palmdotax 2:f873deba2305 426 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 427 memory.write(ADDRESS_RIGHT_KD,data_buff);
palmdotax 2:f873deba2305 428 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 429 break;
palmdotax 1:45f1573d65a1 430 }
palmdotax 1:45f1573d65a1 431 }
palmdotax 1:45f1573d65a1 432 } break;
palmdotax 5:fe76f3dae81e 433 case READ_DATA: { PC.printf("READ_DATA\n");
palmdotax 1:45f1573d65a1 434 switch (command[0]) {
palmdotax 1:45f1573d65a1 435 case GET_LIDAR: {
palmdotax 5:fe76f3dae81e 436 /* int i=0,j=1,k=0;
palmdotax 5:fe76f3dae81e 437 uint8_t intData[2]={0x00,0x01},floatData[2];
palmdotax 4:de5a65c17664 438 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 439 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 440 package.robotId = MY_ID;
palmdotax 5:fe76f3dae81e 441 package.length = 4;//122
palmdotax 4:de5a65c17664 442 package.instructionErrorId = WRITE_DATA;
palmdotax 5:fe76f3dae81e 443 PC.printf("GETDATA\n");
palmdotax 4:de5a65c17664 444 while(k<60)
palmdotax 5:fe76f3dae81e 445 { PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 446 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 447 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 448 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 449 i=i+2;
palmdotax 4:de5a65c17664 450 j=j+2;
palmdotax 4:de5a65c17664 451 k++;
palmdotax 4:de5a65c17664 452
palmdotax 4:de5a65c17664 453 }
palmdotax 5:fe76f3dae81e 454 // PC.printf("SEND\n");
palmdotax 5:fe76f3dae81e 455 //rs485_dirc1=1;
palmdotax 4:de5a65c17664 456 wait_us(RS485_DELAY);
palmdotax 5:fe76f3dae81e 457 PC.printf("SEND2\n");
palmdotax 4:de5a65c17664 458 com1->sendCommunicatePacket(&package);
palmdotax 5:fe76f3dae81e 459 PC.printf("SEND3\n");
palmdotax 4:de5a65c17664 460
palmdotax 4:de5a65c17664 461
palmdotax 4:de5a65c17664 462
palmdotax 4:de5a65c17664 463
palmdotax 5:fe76f3dae81e 464 */
palmdotax 5:fe76f3dae81e 465 com.sendlidar();
palmdotax 5:fe76f3dae81e 466 PC.printf("SEND2\n");
palmdotax 4:de5a65c17664 467 break;
palmdotax 4:de5a65c17664 468 }
palmdotax 4:de5a65c17664 469 case GET_LIDAR2: {
palmdotax 4:de5a65c17664 470 int i=0,j=1,k=60;
palmdotax 4:de5a65c17664 471 uint8_t intData[2],floatData[2];
palmdotax 4:de5a65c17664 472 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 473 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 474 package.robotId = MY_ID;
palmdotax 4:de5a65c17664 475 package.length = 122;
palmdotax 4:de5a65c17664 476 package.instructionErrorId = WRITE_DATA;
palmdotax 4:de5a65c17664 477
palmdotax 4:de5a65c17664 478 while(k<120)
palmdotax 5:fe76f3dae81e 479 {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 480 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 481 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 482 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 483 i=i+2;
palmdotax 4:de5a65c17664 484 j=j+2;
palmdotax 4:de5a65c17664 485 k++;
palmdotax 4:de5a65c17664 486
palmdotax 4:de5a65c17664 487 }
palmdotax 4:de5a65c17664 488 rs485_dirc1=1;
palmdotax 4:de5a65c17664 489 wait_us(RS485_DELAY);
palmdotax 4:de5a65c17664 490 com1->sendCommunicatePacket(&package);
palmdotax 3:edaab92dbd2f 491
palmdotax 1:45f1573d65a1 492 break;
palmdotax 1:45f1573d65a1 493 }
palmdotax 4:de5a65c17664 494 case GET_LIDAR3: {
palmdotax 4:de5a65c17664 495 int i=0,j=1,k=120;
palmdotax 4:de5a65c17664 496 uint8_t intData[2],floatData[2];
palmdotax 4:de5a65c17664 497 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 498 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 499 package.robotId = MY_ID;
palmdotax 4:de5a65c17664 500 package.length = 122;
palmdotax 4:de5a65c17664 501 package.instructionErrorId = WRITE_DATA;
palmdotax 4:de5a65c17664 502 while(k<180)
palmdotax 5:fe76f3dae81e 503 {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 504 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 505 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 506 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 507 i=i+2;
palmdotax 4:de5a65c17664 508 j=j+2;
palmdotax 4:de5a65c17664 509 k++;
palmdotax 4:de5a65c17664 510 }
palmdotax 4:de5a65c17664 511 rs485_dirc1=1;
palmdotax 4:de5a65c17664 512 wait_us(RS485_DELAY);
palmdotax 4:de5a65c17664 513 com1->sendCommunicatePacket(&package);
palmdotax 4:de5a65c17664 514
palmdotax 4:de5a65c17664 515 break;
palmdotax 4:de5a65c17664 516 }
palmdotax 4:de5a65c17664 517 case GET_LIDAR4: {
palmdotax 4:de5a65c17664 518 int i=0,j=1,k=180;
palmdotax 4:de5a65c17664 519 uint8_t intData[2],floatData[2];
palmdotax 4:de5a65c17664 520 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 521 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 522 package.robotId = MY_ID;
palmdotax 4:de5a65c17664 523 package.length = 122;
palmdotax 4:de5a65c17664 524 package.instructionErrorId = WRITE_DATA;
palmdotax 4:de5a65c17664 525 while(k<240)
palmdotax 5:fe76f3dae81e 526 {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 527 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 528 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 529 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 530 i=i+2;
palmdotax 4:de5a65c17664 531 j=j+2;
palmdotax 4:de5a65c17664 532 k++;
palmdotax 4:de5a65c17664 533 }
palmdotax 4:de5a65c17664 534 rs485_dirc1=1;
palmdotax 4:de5a65c17664 535 wait_us(RS485_DELAY);
palmdotax 4:de5a65c17664 536 com1->sendCommunicatePacket(&package);
palmdotax 4:de5a65c17664 537
palmdotax 4:de5a65c17664 538 break;
palmdotax 4:de5a65c17664 539 }
palmdotax 4:de5a65c17664 540 case GET_LIDAR5: {
palmdotax 4:de5a65c17664 541 int i=0,j=1,k=240;
palmdotax 4:de5a65c17664 542 uint8_t intData[2],floatData[2];
palmdotax 4:de5a65c17664 543 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 544 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 545 package.robotId = MY_ID;
palmdotax 4:de5a65c17664 546 package.length = 122;
palmdotax 4:de5a65c17664 547 package.instructionErrorId = WRITE_DATA;
palmdotax 4:de5a65c17664 548 while(k<300)
palmdotax 5:fe76f3dae81e 549 {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 550 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 551 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 552 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 553 i=i+2;
palmdotax 4:de5a65c17664 554 j=j+2;
palmdotax 4:de5a65c17664 555 k++;
palmdotax 4:de5a65c17664 556 }
palmdotax 5:fe76f3dae81e 557
palmdotax 4:de5a65c17664 558 rs485_dirc1=1;
palmdotax 4:de5a65c17664 559 wait_us(RS485_DELAY);
palmdotax 4:de5a65c17664 560 com1->sendCommunicatePacket(&package);
palmdotax 4:de5a65c17664 561
palmdotax 4:de5a65c17664 562 break;
palmdotax 4:de5a65c17664 563 }
palmdotax 4:de5a65c17664 564 case GET_LIDAR6: {
palmdotax 4:de5a65c17664 565 int i=0,j=1,k=300;
palmdotax 4:de5a65c17664 566 uint8_t intData[2],floatData[2];
palmdotax 4:de5a65c17664 567 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 568 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 569 package.robotId = MY_ID;
palmdotax 4:de5a65c17664 570 package.length = 122;
palmdotax 4:de5a65c17664 571 package.instructionErrorId = WRITE_DATA;
palmdotax 4:de5a65c17664 572 while(k<360)
palmdotax 5:fe76f3dae81e 573 {//PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
palmdotax 4:de5a65c17664 574 com.FloatSep( lidar.Data[k],intData,floatData);
palmdotax 4:de5a65c17664 575 package.parameter[i]=intData[0];
palmdotax 4:de5a65c17664 576 package.parameter[j]=intData[1];
palmdotax 4:de5a65c17664 577 i=i+2;
palmdotax 4:de5a65c17664 578 j=j+2;
palmdotax 4:de5a65c17664 579 k++;
palmdotax 4:de5a65c17664 580 }
palmdotax 4:de5a65c17664 581 rs485_dirc1=1;
palmdotax 4:de5a65c17664 582 wait_us(RS485_DELAY);
palmdotax 4:de5a65c17664 583 com1->sendCommunicatePacket(&package);
palmdotax 4:de5a65c17664 584
palmdotax 4:de5a65c17664 585 break;
palmdotax 4:de5a65c17664 586 }
palmdotax 4:de5a65c17664 587
palmdotax 1:45f1573d65a1 588 case GET_BATTERY: {
palmdotax 1:45f1573d65a1 589
palmdotax 1:45f1573d65a1 590 break;
palmdotax 1:45f1573d65a1 591 }
palmdotax 1:45f1573d65a1 592 case GET_VELOCITY_LEFT: {
palmdotax 1:45f1573d65a1 593 uint8_t intVelo_L[2],floatVelo_L[2];
palmdotax 1:45f1573d65a1 594 com.FloatSep(valocity1,intVelo_L,floatVelo_L);
palmdotax 1:45f1573d65a1 595 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 596
palmdotax 1:45f1573d65a1 597 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 598 package.length = 6;
palmdotax 1:45f1573d65a1 599 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 600 package.parameter[0]=intVelo_L[0];
palmdotax 1:45f1573d65a1 601 package.parameter[1]=intVelo_L[1];
palmdotax 1:45f1573d65a1 602 package.parameter[2]=floatVelo_L[0];
palmdotax 1:45f1573d65a1 603 package.parameter[3]=floatVelo_L[1];
palmdotax 1:45f1573d65a1 604
palmdotax 1:45f1573d65a1 605 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 606 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 607 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 608
palmdotax 1:45f1573d65a1 609
palmdotax 1:45f1573d65a1 610 break;
palmdotax 1:45f1573d65a1 611 }
palmdotax 1:45f1573d65a1 612 case GET_VELOCITY_RIGHT : {
palmdotax 1:45f1573d65a1 613 uint8_t intVelo_R[2],floatVelo_R[2];
palmdotax 1:45f1573d65a1 614 com.FloatSep(valocity2,intVelo_R,floatVelo_R);
palmdotax 1:45f1573d65a1 615
palmdotax 1:45f1573d65a1 616
palmdotax 1:45f1573d65a1 617 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 618
palmdotax 1:45f1573d65a1 619 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 620 package.length = 6;
palmdotax 1:45f1573d65a1 621 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 622 package.parameter[0]=intVelo_R[0];
palmdotax 1:45f1573d65a1 623 package.parameter[1]=intVelo_R[1];
palmdotax 1:45f1573d65a1 624 package.parameter[2]=floatVelo_R[0];
palmdotax 1:45f1573d65a1 625 package.parameter[3]=floatVelo_R[1];
palmdotax 1:45f1573d65a1 626
palmdotax 1:45f1573d65a1 627 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 628 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 629 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 630
palmdotax 1:45f1573d65a1 631 break;
palmdotax 1:45f1573d65a1 632 }
palmdotax 1:45f1573d65a1 633 case GET_KP_LEFT: {
palmdotax 2:f873deba2305 634 memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF);
palmdotax 1:45f1573d65a1 635 uint8_t intKPL[2],floatKPL[2];
palmdotax 2:f873deba2305 636 com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL);
palmdotax 1:45f1573d65a1 637
palmdotax 1:45f1573d65a1 638
palmdotax 1:45f1573d65a1 639 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 640
palmdotax 1:45f1573d65a1 641 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 642 package.length = 6;
palmdotax 1:45f1573d65a1 643 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 644 package.parameter[0]=intKPL[0];
palmdotax 1:45f1573d65a1 645 package.parameter[1]=intKPL[1];
palmdotax 1:45f1573d65a1 646 package.parameter[2]=floatKPL[0];
palmdotax 1:45f1573d65a1 647 package.parameter[3]=floatKPL[1];
palmdotax 1:45f1573d65a1 648
palmdotax 1:45f1573d65a1 649 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 650 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 651 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 652
palmdotax 1:45f1573d65a1 653 break;
palmdotax 1:45f1573d65a1 654 }
palmdotax 1:45f1573d65a1 655 case GET_KI_LEFT: {
palmdotax 2:f873deba2305 656 memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
palmdotax 1:45f1573d65a1 657 uint8_t intKIL[2],floatKIL[2];
palmdotax 2:f873deba2305 658 com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
palmdotax 1:45f1573d65a1 659
palmdotax 1:45f1573d65a1 660
palmdotax 1:45f1573d65a1 661 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 662
palmdotax 1:45f1573d65a1 663 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 664 package.length = 6;
palmdotax 1:45f1573d65a1 665 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 666 package.parameter[0]=intKIL[0];
palmdotax 1:45f1573d65a1 667 package.parameter[1]=intKIL[1];
palmdotax 1:45f1573d65a1 668 package.parameter[2]=floatKIL[0];
palmdotax 1:45f1573d65a1 669 package.parameter[3]=floatKIL[1];
palmdotax 1:45f1573d65a1 670
palmdotax 1:45f1573d65a1 671 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 672 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 673 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 674
palmdotax 1:45f1573d65a1 675 break;
palmdotax 1:45f1573d65a1 676 }
palmdotax 1:45f1573d65a1 677 case GET_KD_LEFT: {
palmdotax 2:f873deba2305 678 memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
palmdotax 1:45f1573d65a1 679 uint8_t intKDL[2],floatKDL[2];
palmdotax 2:f873deba2305 680 com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
palmdotax 1:45f1573d65a1 681
palmdotax 1:45f1573d65a1 682
palmdotax 1:45f1573d65a1 683 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 684
palmdotax 1:45f1573d65a1 685 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 686 package.length = 6;
palmdotax 1:45f1573d65a1 687 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 688 package.parameter[0]=intKDL[0];
palmdotax 1:45f1573d65a1 689 package.parameter[1]=intKDL[1];
palmdotax 1:45f1573d65a1 690 package.parameter[2]=floatKDL[0];
palmdotax 1:45f1573d65a1 691 package.parameter[3]=floatKDL[1];
palmdotax 1:45f1573d65a1 692
palmdotax 1:45f1573d65a1 693 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 694 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 695 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 696
palmdotax 1:45f1573d65a1 697 break;
palmdotax 1:45f1573d65a1 698 }
palmdotax 1:45f1573d65a1 699 case GET_KP_RIGHT: {
palmdotax 2:f873deba2305 700 memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 701 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 702 com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
palmdotax 1:45f1573d65a1 703
palmdotax 1:45f1573d65a1 704
palmdotax 1:45f1573d65a1 705 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 706
palmdotax 1:45f1573d65a1 707 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 708 package.length = 6;
palmdotax 1:45f1573d65a1 709 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 710 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 711 package.parameter[1]=intKDR[1];
palmdotax 1:45f1573d65a1 712 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 713 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 714
palmdotax 1:45f1573d65a1 715 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 716 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 717 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 718
palmdotax 1:45f1573d65a1 719 break;
palmdotax 1:45f1573d65a1 720 }
palmdotax 1:45f1573d65a1 721 case GET_KI_RIGHT: {
palmdotax 2:f873deba2305 722 memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 723 uint8_t intKIR[2],floatKIR[2];
palmdotax 2:f873deba2305 724 com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
palmdotax 1:45f1573d65a1 725
palmdotax 1:45f1573d65a1 726
palmdotax 1:45f1573d65a1 727 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 728
palmdotax 1:45f1573d65a1 729 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 730 package.length = 6;
palmdotax 1:45f1573d65a1 731 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 732 package.parameter[0]=intKIR[0];
palmdotax 1:45f1573d65a1 733 package.parameter[1]=intKIR[1];
palmdotax 1:45f1573d65a1 734 package.parameter[2]=floatKIR[0];
palmdotax 1:45f1573d65a1 735 package.parameter[3]=floatKIR[1];
palmdotax 1:45f1573d65a1 736
palmdotax 1:45f1573d65a1 737 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 738 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 739 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 740
palmdotax 1:45f1573d65a1 741 break;
palmdotax 1:45f1573d65a1 742 }
palmdotax 1:45f1573d65a1 743 case GET_KD_RIGHT: {
palmdotax 2:f873deba2305 744 memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 745 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 746 com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
palmdotax 1:45f1573d65a1 747
palmdotax 1:45f1573d65a1 748
palmdotax 1:45f1573d65a1 749 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 750
palmdotax 1:45f1573d65a1 751 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 752 package.length = 6;
palmdotax 1:45f1573d65a1 753 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 754 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 755 package.parameter[1]=intKDR[1];
palmdotax 1:45f1573d65a1 756 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 757 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 758
palmdotax 1:45f1573d65a1 759 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 760 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 761 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 762
palmdotax 1:45f1573d65a1 763 break;
palmdotax 1:45f1573d65a1 764 }
palmdotax 1:45f1573d65a1 765 }
palmdotax 1:45f1573d65a1 766 }break;
palmdotax 1:45f1573d65a1 767
palmdotax 1:45f1573d65a1 768 }
palmdotax 1:45f1573d65a1 769 }
palmdotax 1:45f1573d65a1 770 }