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@4:de5a65c17664, 2016-06-05 (annotated)

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