Arsapol Manamunchaiyaporn / Mbed 2 deprecated CleaningM-Palm

before test

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

Fork of clean_V1 by Betago

main.cpp@7:0dac9d4ff04f, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 16:17:34 2016 +0000
Revision:
7:0dac9d4ff04f
Parent:
5:fe76f3dae81e 
before test

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
icyzkungz 7:0dac9d4ff04f 38 int timer_now=0,timer_later=0;
icyzkungz 7:0dac9d4ff04f 39 int times=0,timer_buffer=0;
icyzkungz 7:0dac9d4ff04f 40
icyzkungz 7:0dac9d4ff04f 41 //encoder
icyzkungz 7:0dac9d4ff04f 42 int Encoderpos = 0;
icyzkungz 7:0dac9d4ff04f 43 int real_d=0;
icyzkungz 7:0dac9d4ff04f 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
icyzkungz 7:0dac9d4ff04f 64 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
icyzkungz 7:0dac9d4ff04f 65 void RC();
palmdotax 1:45f1573d65a1 66
palmdotax 2:f873deba2305 67 //rplidar
icyzkungz 7:0dac9d4ff04f 68 //float distances = 0;
icyzkungz 7:0dac9d4ff04f 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;
icyzkungz 7:0dac9d4ff04f 85
palmdotax 1:45f1573d65a1 86 }
palmdotax 1:45f1573d65a1 87 void EncoderA_1()//ซ้าย
icyzkungz 7:0dac9d4ff04f 88 {
icyzkungz 7:0dac9d4ff04f 89 if(encoderB_1==0) {
icyzkungz 7:0dac9d4ff04f 90 Encoderpos = Encoderpos + 1;
icyzkungz 7:0dac9d4ff04f 91 } else {
icyzkungz 7:0dac9d4ff04f 92 Encoderpos = Encoderpos -1;
icyzkungz 7:0dac9d4ff04f 93 }
icyzkungz 7:0dac9d4ff04f 94 pulse_1+=1;
icyzkungz 7:0dac9d4ff04f 95 //Encoderpos = Encoderpos + 1;
icyzkungz 7:0dac9d4ff04f 96 //valocity+=1;
icyzkungz 7:0dac9d4ff04f 97 //pc.printf("%d \n",Encoderpos);
icyzkungz 7:0dac9d4ff04f 98 //pc.printf("pulse=%f \n",pulse);
icyzkungz 7:0dac9d4ff04f 99 //if(pulse==128)
icyzkungz 7:0dac9d4ff04f 100 //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
palmdotax 1:45f1573d65a1 101 }
icyzkungz 7:0dac9d4ff04f 102 void EncoderA_2()//ขวา
icyzkungz 7:0dac9d4ff04f 103 {
icyzkungz 7:0dac9d4ff04f 104 if(encoderB_2==0) {
icyzkungz 7:0dac9d4ff04f 105 Encoderpos = Encoderpos + 1;
icyzkungz 7:0dac9d4ff04f 106 } else {
icyzkungz 7:0dac9d4ff04f 107 Encoderpos = Encoderpos -1;
icyzkungz 7:0dac9d4ff04f 108 }
palmdotax 1:45f1573d65a1 109 pulse_2+=1;
palmdotax 1:45f1573d65a1 110 //pc.printf("%d",Encoderpos);
palmdotax 1:45f1573d65a1 111 }
palmdotax 1:45f1573d65a1 112 void EncoderA_D()
icyzkungz 7:0dac9d4ff04f 113 {
icyzkungz 7:0dac9d4ff04f 114 if(encoderB_d==0) {
icyzkungz 7:0dac9d4ff04f 115 Encoderpos = Encoderpos + 1;
icyzkungz 7:0dac9d4ff04f 116 } else {
icyzkungz 7:0dac9d4ff04f 117 Encoderpos = Encoderpos -1;
icyzkungz 7:0dac9d4ff04f 118 }
palmdotax 1:45f1573d65a1 119 pulse_d+=1;
icyzkungz 7:0dac9d4ff04f 120 if(pulse_d==128) {
palmdotax 1:45f1573d65a1 121 Z_d+=1;
palmdotax 1:45f1573d65a1 122 pulse_d=0;
palmdotax 1:45f1573d65a1 123 }
icyzkungz 7:0dac9d4ff04f 124
palmdotax 1:45f1573d65a1 125 }
palmdotax 1:45f1573d65a1 126 void getvelo1()//จาก encoder
palmdotax 1:45f1573d65a1 127 {
palmdotax 1:45f1573d65a1 128 valocity1=pulse_1*((2*3.14*r)/128);
palmdotax 2:f873deba2305 129 PC.printf("valocity=%f \n",valocity1);
palmdotax 1:45f1573d65a1 130 count=0;
palmdotax 1:45f1573d65a1 131 timerStart.reset();
palmdotax 1:45f1573d65a1 132 }
palmdotax 1:45f1573d65a1 133 void getvelo2()
palmdotax 1:45f1573d65a1 134 {
palmdotax 1:45f1573d65a1 135 valocity2=pulse_2*((2*3.14*r)/128);
palmdotax 2:f873deba2305 136 PC.printf("valocity=%f \n",valocity2);
palmdotax 1:45f1573d65a1 137 count=0;
palmdotax 1:45f1573d65a1 138 timerStart.reset();
palmdotax 1:45f1573d65a1 139 }
palmdotax 1:45f1573d65a1 140 void get_d()//ระยะทาง
palmdotax 1:45f1573d65a1 141 {
palmdotax 1:45f1573d65a1 142 real_d=Z_d*(2*3.14*r);
palmdotax 1:45f1573d65a1 143 //ส่งข้อมูล
icyzkungz 7:0dac9d4ff04f 144
palmdotax 1:45f1573d65a1 145 }
palmdotax 2:f873deba2305 146 void get_rplidar()
palmdotax 2:f873deba2305 147 {
icyzkungz 7:0dac9d4ff04f 148 if (IS_OK(lidar.waitPoint())) {
icyzkungz 7:0dac9d4ff04f 149
icyzkungz 7:0dac9d4ff04f 150 } else {
icyzkungz 7:0dac9d4ff04f 151
icyzkungz 7:0dac9d4ff04f 152 lidar.startScan();
icyzkungz 7:0dac9d4ff04f 153
palmdotax 2:f873deba2305 154 }
icyzkungz 7:0dac9d4ff04f 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;
icyzkungz 7:0dac9d4ff04f 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);
icyzkungz 7:0dac9d4ff04f 166 times=timerStart.read();
icyzkungz 7:0dac9d4ff04f 167 if(times==1) { // m/s
icyzkungz 7:0dac9d4ff04f 168 getvelo1();
icyzkungz 7:0dac9d4ff04f 169 //pc.printf("TIME \n");
icyzkungz 7:0dac9d4ff04f 170 times=0;
icyzkungz 7:0dac9d4ff04f 171 pulse_1=0;
icyzkungz 7:0dac9d4ff04f 172 }
palmdotax 1:45f1573d65a1 173 P1.setProcessValue(valocity1);
palmdotax 1:45f1573d65a1 174 outPID=P1.compute();
icyzkungz 7:0dac9d4ff04f 175 //pc.printf("outPID=%f \n",outPID);
icyzkungz 7:0dac9d4ff04f 176 m1.movespeed_1(setp1,outPID);
palmdotax 1:45f1573d65a1 177 }
palmdotax 1:45f1573d65a1 178 void PID_m2()//right
palmdotax 1:45f1573d65a1 179 {
palmdotax 1:45f1573d65a1 180 setp2=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 1:45f1573d65a1 181 P2.setSetPoint(setp2);
icyzkungz 7:0dac9d4ff04f 182 times=timerStart.read();
icyzkungz 7:0dac9d4ff04f 183 if(times==1) { // m/s
icyzkungz 7:0dac9d4ff04f 184 getvelo2();
icyzkungz 7:0dac9d4ff04f 185 //pc.printf("TIME \n");
icyzkungz 7:0dac9d4ff04f 186 times=0;
icyzkungz 7:0dac9d4ff04f 187 pulse_2=0;
icyzkungz 7:0dac9d4ff04f 188 }
palmdotax 1:45f1573d65a1 189 P2.setProcessValue(valocity2);
palmdotax 1:45f1573d65a1 190 outPID=P2.compute();
icyzkungz 7:0dac9d4ff04f 191 //pc.printf("outPID=%f \n",outPID);
icyzkungz 7:0dac9d4ff04f 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 5:fe76f3dae81e 210 PC.printf("RC******************************");
palmdotax 1:45f1573d65a1 211 }
palmdotax 1:45f1573d65a1 212
palmdotax 1:45f1573d65a1 213 }
palmdotax 1:45f1573d65a1 214 /*******************************************************/
palmdotax 5:fe76f3dae81e 215 int buf=0;
icyzkungz 7:0dac9d4ff04f 216 float lidar_angle_max=360,lidar_angle_min=0;
icyzkungz 0:84f05cd2f197 217 int main()
icyzkungz 0:84f05cd2f197 218 {
palmdotax 1:45f1573d65a1 219 PC.baud(115200);
palmdotax 5:fe76f3dae81e 220 lidar.begin(se_lidar);
palmdotax 5:fe76f3dae81e 221 tim.start();
palmdotax 5:fe76f3dae81e 222 //com1 = new COMMUNICATION(PA_9,PA_10,115200);
icyzkungz 7:0dac9d4ff04f 223 encoderA_1.rise(&EncoderA_1);
icyzkungz 7:0dac9d4ff04f 224 timerStart.start();
icyzkungz 7:0dac9d4ff04f 225 P1.setMode(1);
icyzkungz 7:0dac9d4ff04f 226 P1.setBias(0);
icyzkungz 7:0dac9d4ff04f 227 // pc.printf("READY \n");
palmdotax 2:f873deba2305 228 //pc.attach(&Rx_interrupt, Serial::RxIrq);
icyzkungz 7:0dac9d4ff04f 229 lidar.setAngle(lidar_angle_min,lidar_angle_max);
palmdotax 1:45f1573d65a1 230 while(1) {
palmdotax 1:45f1573d65a1 231
palmdotax 5:fe76f3dae81e 232 VMO=1;
palmdotax 5:fe76f3dae81e 233 get_rplidar();
icyzkungz 7:0dac9d4ff04f 234 /* if(tim.read_ms()-buf>=1000){
icyzkungz 7:0dac9d4ff04f 235 for(int x=0;x<=359;x++){
icyzkungz 7:0dac9d4ff04f 236 printf("%d,",lidar.Data[x]);
icyzkungz 7:0dac9d4ff04f 237 }
icyzkungz 7:0dac9d4ff04f 238 buf = tim.read_ms();
icyzkungz 7:0dac9d4ff04f 239 }*/
palmdotax 2:f873deba2305 240 RC();
palmdotax 1:45f1573d65a1 241 //wait_ms(1);
icyzkungz 0:84f05cd2f197 242 }
icyzkungz 0:84f05cd2f197 243 }
icyzkungz 0:84f05cd2f197 244
palmdotax 1:45f1573d65a1 245
palmdotax 1:45f1573d65a1 246
icyzkungz 0:84f05cd2f197 247
palmdotax 1:45f1573d65a1 248
icyzkungz 0:84f05cd2f197 249
icyzkungz 0:84f05cd2f197 250
icyzkungz 0:84f05cd2f197 251
icyzkungz 0:84f05cd2f197 252
palmdotax 1:45f1573d65a1 253 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
palmdotax 1:45f1573d65a1 254 {
icyzkungz 7:0dac9d4ff04f 255 PC.printf("cmdcheck\n");
icyzkungz 7:0dac9d4ff04f 256 if(id==MY_ID) {
palmdotax 1:45f1573d65a1 257 switch (ins) {
palmdotax 1:45f1573d65a1 258 case PING: {
palmdotax 1:45f1573d65a1 259 break;
palmdotax 1:45f1573d65a1 260 }
palmdotax 1:45f1573d65a1 261 case WRITE_DATA: {
palmdotax 1:45f1573d65a1 262 switch (command[0]) {
icyzkungz 7:0dac9d4ff04f 263 case SET_MAX_LIDAR_DEGREE: {
icyzkungz 7:0dac9d4ff04f 264 uint8_t int_buffer[2],float_buffer[2];
icyzkungz 7:0dac9d4ff04f 265 uint8_t int_buffer2[2],float_buffer2[2];
icyzkungz 7:0dac9d4ff04f 266 float Int,flo;
icyzkungz 7:0dac9d4ff04f 267 int_buffer[0]=command[1];
icyzkungz 7:0dac9d4ff04f 268 int_buffer[1]=command[2];
icyzkungz 7:0dac9d4ff04f 269 float_buffer[0]=command[3];
icyzkungz 7:0dac9d4ff04f 270 float_buffer[1]=command[4];
icyzkungz 7:0dac9d4ff04f 271 int_buffer2[0]=command[5];
icyzkungz 7:0dac9d4ff04f 272 int_buffer2[1]=command[6];
icyzkungz 7:0dac9d4ff04f 273 float_buffer2[0]=command[7];
icyzkungz 7:0dac9d4ff04f 274 float_buffer2[1]=command[8];
icyzkungz 7:0dac9d4ff04f 275 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
icyzkungz 7:0dac9d4ff04f 276 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
icyzkungz 7:0dac9d4ff04f 277 lidar_angle_max=Int+(flo/10000);
icyzkungz 7:0dac9d4ff04f 278
icyzkungz 7:0dac9d4ff04f 279 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer2);
icyzkungz 7:0dac9d4ff04f 280 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer2);
icyzkungz 7:0dac9d4ff04f 281 lidar_angle_max=Int+(flo/10000);
icyzkungz 7:0dac9d4ff04f 282 PC.printf("Min,Max Degree of Lidar : %f %f/n",lidar_angle_min,lidar_angle_max);
palmdotax 1:45f1573d65a1 283 break;
palmdotax 1:45f1573d65a1 284 }
icyzkungz 7:0dac9d4ff04f 285
palmdotax 1:45f1573d65a1 286 case SET_VELOCITY_LEFT: {
palmdotax 4:de5a65c17664 287 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 288 float Int,flo;
palmdotax 1:45f1573d65a1 289 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 290 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 291 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 292 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 293 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 294 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 295 VL=Int+(flo/10000);
palmdotax 1:45f1573d65a1 296 PC.printf("VL=%f /n",VL);
palmdotax 1:45f1573d65a1 297 break;
palmdotax 1:45f1573d65a1 298 }
palmdotax 1:45f1573d65a1 299 case SET_VELOCITY_RIGHT: {
palmdotax 1:45f1573d65a1 300 //
palmdotax 4:de5a65c17664 301 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 302 float Int,flo;
palmdotax 1:45f1573d65a1 303 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 304 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 305 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 306 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 307 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 308 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 309 VR=Int+flo;
palmdotax 4:de5a65c17664 310 PC.printf("VL=%f /n",VR);
palmdotax 1:45f1573d65a1 311 break;
palmdotax 1:45f1573d65a1 312 }
palmdotax 1:45f1573d65a1 313 case SET_VELOCITY_MAX_LEFT: {
palmdotax 1:45f1573d65a1 314 //
palmdotax 4:de5a65c17664 315 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 316 float Int,flo;
palmdotax 1:45f1573d65a1 317 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 318 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 319 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 320 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 321 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 322 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 323 VLmax=Int+flo;
palmdotax 4:de5a65c17664 324 PC.printf("VL=%f /n",VLmax);
palmdotax 1:45f1573d65a1 325 break;
palmdotax 1:45f1573d65a1 326 }
palmdotax 1:45f1573d65a1 327 case SET_VELOCITY_MAX_RIGHT: {
palmdotax 1:45f1573d65a1 328 //
palmdotax 4:de5a65c17664 329 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 330 float Int,flo;
palmdotax 1:45f1573d65a1 331 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 332 int_buffer[1]=command[2];
palmdotax 4:de5a65c17664 333 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 334 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 335 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
icyzkungz 7:0dac9d4ff04f 336 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 337 VRmax=Int+flo;
palmdotax 1:45f1573d65a1 338 PC.printf("VRmax = %f",VRmax);
icyzkungz 7:0dac9d4ff04f 339 // PC.printf("*****************************");
palmdotax 1:45f1573d65a1 340 break;
palmdotax 1:45f1573d65a1 341 }
palmdotax 2:f873deba2305 342 //save to rom
palmdotax 1:45f1573d65a1 343 case SET_KP_LEFT: {
palmdotax 4:de5a65c17664 344 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 345 float Int,flo;
palmdotax 1:45f1573d65a1 346 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 347 int_buffer[1]=command[2];
icyzkungz 7:0dac9d4ff04f 348 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 349 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 350 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 351 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 352
icyzkungz 7:0dac9d4ff04f 353 KP_LEFT=Int+flo;
icyzkungz 7:0dac9d4ff04f 354 PC.printf("KP_LEFT=%f /n",KP_LEFT);
icyzkungz 7:0dac9d4ff04f 355 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 356 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 357 memory.write(ADDRESS_LEFT_KP,data_buff);
icyzkungz 7:0dac9d4ff04f 358 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 359 break;
palmdotax 1:45f1573d65a1 360 }
palmdotax 1:45f1573d65a1 361 case SET_KI_LEFT: {
icyzkungz 7:0dac9d4ff04f 362 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 363 float Int,flo;
palmdotax 1:45f1573d65a1 364 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 365 int_buffer[1]=command[2];
icyzkungz 7:0dac9d4ff04f 366 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 367 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 368 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 4:de5a65c17664 369 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
palmdotax 4:de5a65c17664 370
icyzkungz 7:0dac9d4ff04f 371 KI_LEFT=Int+flo;
icyzkungz 7:0dac9d4ff04f 372 PC.printf("KI_LEFT=%f /n",KI_LEFT);
icyzkungz 7:0dac9d4ff04f 373 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 374 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 375 memory.write(ADDRESS_LEFT_KI ,data_buff);
icyzkungz 7:0dac9d4ff04f 376 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 377 break;
palmdotax 1:45f1573d65a1 378 }
palmdotax 1:45f1573d65a1 379 case SET_KD_LEFT: {
palmdotax 4:de5a65c17664 380 uint8_t int_buffer[2],float_buffer[2];
palmdotax 4:de5a65c17664 381 float Int,flo;
palmdotax 1:45f1573d65a1 382 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 383 int_buffer[1]=command[2];
icyzkungz 7:0dac9d4ff04f 384 float_buffer[0]=command[3];
palmdotax 4:de5a65c17664 385 float_buffer[1]=command[4];
palmdotax 1:45f1573d65a1 386 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
icyzkungz 7:0dac9d4ff04f 387 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
icyzkungz 7:0dac9d4ff04f 388 KD_LEFT=Int+flo;
icyzkungz 7:0dac9d4ff04f 389 PC.printf("KD_LEFT=%f /n",KD_LEFT);
icyzkungz 7:0dac9d4ff04f 390 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 391 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 392 memory.write(ADDRESS_LEFT_KD,data_buff);
icyzkungz 7:0dac9d4ff04f 393 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 394 break;
palmdotax 1:45f1573d65a1 395 }
palmdotax 1:45f1573d65a1 396 case SET_KP_RIGHT: {
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 7:0dac9d4ff04f 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 7:0dac9d4ff04f 404 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
icyzkungz 7:0dac9d4ff04f 405 KP_RIGHT=Int+flo;
icyzkungz 7:0dac9d4ff04f 406 PC.printf("KP_RIGHT=%f /n",KP_RIGHT);
icyzkungz 7:0dac9d4ff04f 407 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 408 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 409 memory.write(ADDRESS_RIGHT_KP,data_buff);
icyzkungz 7:0dac9d4ff04f 410 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 411 break;
palmdotax 1:45f1573d65a1 412 }
palmdotax 1:45f1573d65a1 413 case SET_KI_RIGHT: {
icyzkungz 7:0dac9d4ff04f 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 7:0dac9d4ff04f 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 7:0dac9d4ff04f 421 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
icyzkungz 7:0dac9d4ff04f 422 KI_RIGHT=Int+flo;
icyzkungz 7:0dac9d4ff04f 423 PC.printf("KI_RIGHT=%f /n",KI_RIGHT);
icyzkungz 7:0dac9d4ff04f 424 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 425 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 426 memory.write(ADDRESS_RIGHT_KI,data_buff);
icyzkungz 7:0dac9d4ff04f 427 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 428 break;
palmdotax 1:45f1573d65a1 429 }
palmdotax 1:45f1573d65a1 430 case SET_KD_RIGHT: {
palmdotax 4:de5a65c17664 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];
palmdotax 4:de5a65c17664 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 7:0dac9d4ff04f 438 flo=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer);
icyzkungz 7:0dac9d4ff04f 439 KD_RIGHT=Int+flo;
icyzkungz 7:0dac9d4ff04f 440 PC.printf("KD_RIGHT=%f /n",KD_RIGHT);
icyzkungz 7:0dac9d4ff04f 441 int32_t data_buff;
icyzkungz 7:0dac9d4ff04f 442 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
icyzkungz 7:0dac9d4ff04f 443 memory.write(ADDRESS_RIGHT_KD,data_buff);
icyzkungz 7:0dac9d4ff04f 444 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 445 break;
palmdotax 1:45f1573d65a1 446 }
icyzkungz 7:0dac9d4ff04f 447 }
icyzkungz 7:0dac9d4ff04f 448 }
icyzkungz 7:0dac9d4ff04f 449 break;
icyzkungz 7:0dac9d4ff04f 450 case READ_DATA: {
icyzkungz 7:0dac9d4ff04f 451 PC.printf("READ_DATA\n");
palmdotax 1:45f1573d65a1 452 switch (command[0]) {
icyzkungz 7:0dac9d4ff04f 453 /*case GET_LIDAR1: {
icyzkungz 7:0dac9d4ff04f 454 uint8_t IntArray[2],FloatArray[2];
icyzkungz 7:0dac9d4ff04f 455 int numberK=0;
icyzkungz 7:0dac9d4ff04f 456 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 7:0dac9d4ff04f 457
icyzkungz 7:0dac9d4ff04f 458 package.robotId = ID;
icyzkungz 7:0dac9d4ff04f 459 package.length = 362;
palmdotax 4:de5a65c17664 460 package.instructionErrorId = WRITE_DATA;
icyzkungz 7:0dac9d4ff04f 461
icyzkungz 7:0dac9d4ff04f 462 for(int i=1; i<=360; i++) {
icyzkungz 7:0dac9d4ff04f 463 if(i==2) {
icyzkungz 7:0dac9d4ff04f 464 com.FloatSep(lidar.Data[numberK],IntArray,FloatArray);
icyzkungz 7:0dac9d4ff04f 465 package.parameter[i-1]=IntArray[0];
icyzkungz 7:0dac9d4ff04f 466 package.parameter[i]=IntArray[1];
icyzkungz 7:0dac9d4ff04f 467 numberK++;
icyzkungz 7:0dac9d4ff04f 468 }
palmdotax 4:de5a65c17664 469 }
icyzkungz 7:0dac9d4ff04f 470 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 471 com1->sendCommunicatePacket(&package);
palmdotax 4:de5a65c17664 472 break;
icyzkungz 7:0dac9d4ff04f 473 }*/
icyzkungz 7:0dac9d4ff04f 474 case GET_LIDAR2: {
palmdotax 4:de5a65c17664 475 int i=0,j=1,k=60;
icyzkungz 7:0dac9d4ff04f 476 uint8_t intData[2],floatData[2];
icyzkungz 7:0dac9d4ff04f 477 ANDANTE_PROTOCOL_PACKET package;
palmdotax 4:de5a65c17664 478 //BUFFER_SIZE=143
palmdotax 4:de5a65c17664 479 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 480 package.length = 122;
palmdotax 4:de5a65c17664 481 package.instructionErrorId = WRITE_DATA;
icyzkungz 7:0dac9d4ff04f 482
icyzkungz 7:0dac9d4ff04f 483 while(k<120) {
icyzkungz 7:0dac9d4ff04f 484 //PC.printf("i= %d,j= %d,k = %d\n",i,j,k);
icyzkungz 7:0dac9d4ff04f 485 com.FloatSep( lidar.Data[k],intData,floatData);
icyzkungz 7:0dac9d4ff04f 486 package.parameter[i]=intData[0];
icyzkungz 7:0dac9d4ff04f 487 package.parameter[j]=intData[1];
icyzkungz 7:0dac9d4ff04f 488 i=i+2;
icyzkungz 7:0dac9d4ff04f 489 j=j+2;
icyzkungz 7:0dac9d4ff04f 490 k++;
icyzkungz 7:0dac9d4ff04f 491
palmdotax 4:de5a65c17664 492 }
icyzkungz 7:0dac9d4ff04f 493 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 494 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 495 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 496
icyzkungz 7:0dac9d4ff04f 497 break;
icyzkungz 7:0dac9d4ff04f 498 }
palmdotax 1:45f1573d65a1 499 case GET_BATTERY: {
icyzkungz 7:0dac9d4ff04f 500
palmdotax 1:45f1573d65a1 501 break;
icyzkungz 7:0dac9d4ff04f 502 }
palmdotax 1:45f1573d65a1 503 case GET_VELOCITY_LEFT: {
palmdotax 1:45f1573d65a1 504 uint8_t intVelo_L[2],floatVelo_L[2];
palmdotax 1:45f1573d65a1 505 com.FloatSep(valocity1,intVelo_L,floatVelo_L);
icyzkungz 7:0dac9d4ff04f 506 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 507
palmdotax 1:45f1573d65a1 508 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 509 package.length = 6;
palmdotax 1:45f1573d65a1 510 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 511 package.parameter[0]=intVelo_L[0];
palmdotax 1:45f1573d65a1 512 package.parameter[1]=intVelo_L[1];
icyzkungz 7:0dac9d4ff04f 513 package.parameter[2]=floatVelo_L[0];
palmdotax 1:45f1573d65a1 514 package.parameter[3]=floatVelo_L[1];
palmdotax 1:45f1573d65a1 515
icyzkungz 7:0dac9d4ff04f 516 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 517 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 518 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 519
icyzkungz 7:0dac9d4ff04f 520
palmdotax 1:45f1573d65a1 521 break;
icyzkungz 7:0dac9d4ff04f 522 }
palmdotax 1:45f1573d65a1 523 case GET_VELOCITY_RIGHT : {
palmdotax 1:45f1573d65a1 524 uint8_t intVelo_R[2],floatVelo_R[2];
palmdotax 1:45f1573d65a1 525 com.FloatSep(valocity2,intVelo_R,floatVelo_R);
icyzkungz 7:0dac9d4ff04f 526
icyzkungz 7:0dac9d4ff04f 527
icyzkungz 7:0dac9d4ff04f 528 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 529
palmdotax 1:45f1573d65a1 530 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 531 package.length = 6;
palmdotax 1:45f1573d65a1 532 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 533 package.parameter[0]=intVelo_R[0];
palmdotax 1:45f1573d65a1 534 package.parameter[1]=intVelo_R[1];
icyzkungz 7:0dac9d4ff04f 535 package.parameter[2]=floatVelo_R[0];
palmdotax 1:45f1573d65a1 536 package.parameter[3]=floatVelo_R[1];
palmdotax 1:45f1573d65a1 537
icyzkungz 7:0dac9d4ff04f 538 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 539 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 540 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 541
palmdotax 1:45f1573d65a1 542 break;
icyzkungz 7:0dac9d4ff04f 543 }
palmdotax 1:45f1573d65a1 544 case GET_KP_LEFT: {
icyzkungz 7:0dac9d4ff04f 545 memory.read(ADDRESS_LEFT_KP ,KP_LEFT);
icyzkungz 7:0dac9d4ff04f 546 memory.read(ADDRESS_LEFT_KP ,KI_LEFT);
icyzkungz 7:0dac9d4ff04f 547 memory.read(ADDRESS_LEFT_KP ,KD_LEFT);
icyzkungz 7:0dac9d4ff04f 548
icyzkungz 7:0dac9d4ff04f 549 com.sendKpKiKdLeft(KP_LEFT,KI_LEFT,KD_LEFT);
palmdotax 1:45f1573d65a1 550
icyzkungz 7:0dac9d4ff04f 551 /*uint8_t IntKp[2],FloatKp[2];
icyzkungz 7:0dac9d4ff04f 552 uint8_t IntKi[2],FloatKi[2];
icyzkungz 7:0dac9d4ff04f 553 uint8_t IntKd[2],FloatKd[2];
icyzkungz 7:0dac9d4ff04f 554
icyzkungz 7:0dac9d4ff04f 555 com.FloatSep(KP_LEFT_BUFF,IntKp,FloatKp);
icyzkungz 7:0dac9d4ff04f 556 com.FloatSep(KI_LEFT_BUFF,IntKi,FloatKi);
icyzkungz 7:0dac9d4ff04f 557 com.FloatSep(KD_LEFT_BUFF,IntKd,FloatKd);
icyzkungz 7:0dac9d4ff04f 558
palmdotax 1:45f1573d65a1 559
icyzkungz 7:0dac9d4ff04f 560 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 7:0dac9d4ff04f 561
icyzkungz 7:0dac9d4ff04f 562 package.robotId = ID;
icyzkungz 7:0dac9d4ff04f 563 package.length = 14;
palmdotax 1:45f1573d65a1 564 package.instructionErrorId = WRITE_DATA;
icyzkungz 7:0dac9d4ff04f 565 package.parameter[0]=IntKp[0];
icyzkungz 7:0dac9d4ff04f 566 package.parameter[1]=IntKp[1];
icyzkungz 7:0dac9d4ff04f 567 package.parameter[2]=FloatKp[0];
icyzkungz 7:0dac9d4ff04f 568 package.parameter[3]=FloatKp[1];
icyzkungz 7:0dac9d4ff04f 569 package.parameter[4]=IntKi[0];
icyzkungz 7:0dac9d4ff04f 570 package.parameter[5]=IntKi[1];
icyzkungz 7:0dac9d4ff04f 571 package.parameter[6]=FloatKi[0];
icyzkungz 7:0dac9d4ff04f 572 package.parameter[7]=FloatKi[1];
icyzkungz 7:0dac9d4ff04f 573 package.parameter[8]=IntKd[0];
icyzkungz 7:0dac9d4ff04f 574 package.parameter[9]=IntKd[1];
icyzkungz 7:0dac9d4ff04f 575 package.parameter[10]=FloatKd[0];
icyzkungz 7:0dac9d4ff04f 576 package.parameter[11]=FloatKd[1];
palmdotax 1:45f1573d65a1 577
palmdotax 1:45f1573d65a1 578
icyzkungz 7:0dac9d4ff04f 579 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 580 com1->sendCommunicatePacket(&package);*/
palmdotax 1:45f1573d65a1 581 break;
icyzkungz 7:0dac9d4ff04f 582 }
palmdotax 1:45f1573d65a1 583 case GET_KI_LEFT: {
palmdotax 2:f873deba2305 584 memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
palmdotax 1:45f1573d65a1 585 uint8_t intKIL[2],floatKIL[2];
palmdotax 2:f873deba2305 586 com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
icyzkungz 7:0dac9d4ff04f 587
icyzkungz 7:0dac9d4ff04f 588
icyzkungz 7:0dac9d4ff04f 589 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 590
palmdotax 1:45f1573d65a1 591 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 592 package.length = 6;
palmdotax 1:45f1573d65a1 593 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 594 package.parameter[0]=intKIL[0];
palmdotax 1:45f1573d65a1 595 package.parameter[1]=intKIL[1];
icyzkungz 7:0dac9d4ff04f 596 package.parameter[2]=floatKIL[0];
palmdotax 1:45f1573d65a1 597 package.parameter[3]=floatKIL[1];
palmdotax 1:45f1573d65a1 598
icyzkungz 7:0dac9d4ff04f 599 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 600 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 601 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 602
palmdotax 1:45f1573d65a1 603 break;
icyzkungz 7:0dac9d4ff04f 604 }
palmdotax 1:45f1573d65a1 605 case GET_KD_LEFT: {
palmdotax 2:f873deba2305 606 memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
palmdotax 1:45f1573d65a1 607 uint8_t intKDL[2],floatKDL[2];
palmdotax 2:f873deba2305 608 com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
icyzkungz 7:0dac9d4ff04f 609
icyzkungz 7:0dac9d4ff04f 610
icyzkungz 7:0dac9d4ff04f 611 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 612
palmdotax 1:45f1573d65a1 613 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 614 package.length = 6;
palmdotax 1:45f1573d65a1 615 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 616 package.parameter[0]=intKDL[0];
palmdotax 1:45f1573d65a1 617 package.parameter[1]=intKDL[1];
icyzkungz 7:0dac9d4ff04f 618 package.parameter[2]=floatKDL[0];
palmdotax 1:45f1573d65a1 619 package.parameter[3]=floatKDL[1];
palmdotax 1:45f1573d65a1 620
icyzkungz 7:0dac9d4ff04f 621 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 622 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 623 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 624
palmdotax 1:45f1573d65a1 625 break;
icyzkungz 7:0dac9d4ff04f 626 }
palmdotax 1:45f1573d65a1 627 case GET_KP_RIGHT: {
palmdotax 2:f873deba2305 628 memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 629 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 630 com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
icyzkungz 7:0dac9d4ff04f 631
icyzkungz 7:0dac9d4ff04f 632
icyzkungz 7:0dac9d4ff04f 633 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 634
palmdotax 1:45f1573d65a1 635 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 636 package.length = 6;
palmdotax 1:45f1573d65a1 637 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 638 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 639 package.parameter[1]=intKDR[1];
icyzkungz 7:0dac9d4ff04f 640 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 641 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 642
icyzkungz 7:0dac9d4ff04f 643 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 644 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 645 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 646
palmdotax 1:45f1573d65a1 647 break;
icyzkungz 7:0dac9d4ff04f 648 }
palmdotax 1:45f1573d65a1 649 case GET_KI_RIGHT: {
palmdotax 2:f873deba2305 650 memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 651 uint8_t intKIR[2],floatKIR[2];
palmdotax 2:f873deba2305 652 com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
icyzkungz 7:0dac9d4ff04f 653
icyzkungz 7:0dac9d4ff04f 654
icyzkungz 7:0dac9d4ff04f 655 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 656
palmdotax 1:45f1573d65a1 657 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 658 package.length = 6;
palmdotax 1:45f1573d65a1 659 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 660 package.parameter[0]=intKIR[0];
palmdotax 1:45f1573d65a1 661 package.parameter[1]=intKIR[1];
icyzkungz 7:0dac9d4ff04f 662 package.parameter[2]=floatKIR[0];
palmdotax 1:45f1573d65a1 663 package.parameter[3]=floatKIR[1];
palmdotax 1:45f1573d65a1 664
icyzkungz 7:0dac9d4ff04f 665 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 666 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 667 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 668
palmdotax 1:45f1573d65a1 669 break;
icyzkungz 7:0dac9d4ff04f 670 }
palmdotax 1:45f1573d65a1 671 case GET_KD_RIGHT: {
palmdotax 2:f873deba2305 672 memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 673 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 674 com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
icyzkungz 7:0dac9d4ff04f 675
icyzkungz 7:0dac9d4ff04f 676
icyzkungz 7:0dac9d4ff04f 677 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 678
palmdotax 1:45f1573d65a1 679 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 680 package.length = 6;
palmdotax 1:45f1573d65a1 681 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 682 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 683 package.parameter[1]=intKDR[1];
icyzkungz 7:0dac9d4ff04f 684 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 685 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 686
icyzkungz 7:0dac9d4ff04f 687 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 688 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 689 com1->sendCommunicatePacket(&package);
icyzkungz 7:0dac9d4ff04f 690
palmdotax 1:45f1573d65a1 691 break;
icyzkungz 7:0dac9d4ff04f 692 }
icyzkungz 7:0dac9d4ff04f 693 case GET_MAX_LIDAR_DEGREE: {
icyzkungz 7:0dac9d4ff04f 694 com.sendMaxMinLidarDegree(lidar_angle_max,lidar_angle_min);
icyzkungz 7:0dac9d4ff04f 695 /*uint8_t intMax[2],floatMax[2];
icyzkungz 7:0dac9d4ff04f 696 uint8_t intMin[2],floatMin[2];
icyzkungz 7:0dac9d4ff04f 697 com.FloatSep(lidar_angle_max,intMax,floatMax);
icyzkungz 7:0dac9d4ff04f 698 com.FloatSep(lidar_angle_min,intMin,floatMin);
icyzkungz 7:0dac9d4ff04f 699 ANDANTE_PROTOCOL_PACKET package;
icyzkungz 7:0dac9d4ff04f 700
icyzkungz 7:0dac9d4ff04f 701 package.robotId = MY_ID;
icyzkungz 7:0dac9d4ff04f 702 package.length = 10;
icyzkungz 7:0dac9d4ff04f 703 package.instructionErrorId = WRITE_DATA;
icyzkungz 7:0dac9d4ff04f 704 package.parameter[0]=intMax[0];
icyzkungz 7:0dac9d4ff04f 705 package.parameter[1]=intMax[1];
icyzkungz 7:0dac9d4ff04f 706 package.parameter[2]=floatMax[0];
icyzkungz 7:0dac9d4ff04f 707 package.parameter[3]=floatMax[1];
icyzkungz 7:0dac9d4ff04f 708 package.parameter[4]=intMin[0];
icyzkungz 7:0dac9d4ff04f 709 package.parameter[5]=intMin[1];
icyzkungz 7:0dac9d4ff04f 710 package.parameter[6]=floatMin[0];
icyzkungz 7:0dac9d4ff04f 711 package.parameter[7]=floatMin[1];
icyzkungz 7:0dac9d4ff04f 712
icyzkungz 7:0dac9d4ff04f 713 ////rs485_dirc1=1;
icyzkungz 7:0dac9d4ff04f 714 wait_us(RS485_DELAY);
icyzkungz 7:0dac9d4ff04f 715 com1->sendCommunicatePacket(&package);*/
icyzkungz 7:0dac9d4ff04f 716 }
icyzkungz 7:0dac9d4ff04f 717 case GET_LIDAR1: {
icyzkungz 7:0dac9d4ff04f 718 com.sendSensorDataAll1(lidar.Data);
icyzkungz 7:0dac9d4ff04f 719 }
icyzkungz 7:0dac9d4ff04f 720 }
icyzkungz 7:0dac9d4ff04f 721 }
icyzkungz 7:0dac9d4ff04f 722 break;
icyzkungz 7:0dac9d4ff04f 723
icyzkungz 7:0dac9d4ff04f 724 }
palmdotax 1:45f1573d65a1 725 }
palmdotax 1:45f1573d65a1 726 }