Secret_Betago / Mbed 2 deprecated CleaningM-Palm_V2_copy

สัสชิน

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

Fork of clean_V2 by Betago

main.cpp@8:fc70c78a443b, 2016-06-08 (annotated)

Committer:
icyzkungz
Date:
Wed Jun 08 17:19:21 2016 +0000
Revision:
8:fc70c78a443b
Parent:
7:ffd6959444ae 
??????

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