palm and chin / Mbed 2 deprecated CleaningMachine_Betago

55+

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

Fork of CleaningMachine_Betago by Betago

main.cpp@3:edaab92dbd2f, 2016-05-24 (annotated)

Committer:
palmdotax
Date:
Tue May 24 10:33:21 2016 +0000
Revision:
3:edaab92dbd2f
Parent:
2:f873deba2305 
55+

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 2:f873deba2305 54 BufferedSerial PC(SERIAL_TX,SERIAL_RX);
palmdotax 2:f873deba2305 55 //Serial PC(SERIAL_TX,SERIAL_RX);
palmdotax 1:45f1573d65a1 56 Bear_Receiver com(SERIAL_TX,SERIAL_RX,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 2:f873deba2305 66 float distances = 0;
palmdotax 2:f873deba2305 67 float angle = 0;
palmdotax 2:f873deba2305 68 bool startBit = 0;
palmdotax 2:f873deba2305 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 2:f873deba2305 143 if (IS_OK(lidar.waitPoint())) {
palmdotax 2:f873deba2305 144 distances = lidar.getCurrentPoint().distance; //distance value in mm unit
palmdotax 2:f873deba2305 145 angle = lidar.getCurrentPoint().angle; //anglue value in degree
palmdotax 2:f873deba2305 146 startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan
palmdotax 2:f873deba2305 147 quality = lidar.getCurrentPoint().quality; //quality of the current measurement
palmdotax 2:f873deba2305 148 PC.printf("Distance = %.2f cm\n",distances/10);
palmdotax 2:f873deba2305 149 wait_ms(100);
palmdotax 2:f873deba2305 150 } else {
palmdotax 2:f873deba2305 151 // analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor
palmdotax 2:f873deba2305 152 // rplidar_response_device_info_t info;
palmdotax 2:f873deba2305 153 // if (IS_OK(lidar.getDeviceInfo(info, 100))) {
palmdotax 2:f873deba2305 154 lidar.startScan();
palmdotax 2:f873deba2305 155 // motor=1;
palmdotax 2:f873deba2305 156 // start motor rotating at max allowed speed
palmdotax 2:f873deba2305 157 // analogWrite(RPLIDAR_MOTOR, 255);
palmdotax 2:f873deba2305 158 //delay(1000);
palmdotax 2:f873deba2305 159 // }
palmdotax 2:f873deba2305 160 }
palmdotax 2:f873deba2305 161
palmdotax 2:f873deba2305 162 }
palmdotax 1:45f1573d65a1 163 double map(double x, double in_min, double in_max, double out_min, double out_max)
palmdotax 1:45f1573d65a1 164 {
palmdotax 1:45f1573d65a1 165 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
palmdotax 1:45f1573d65a1 166
palmdotax 1:45f1573d65a1 167 }
palmdotax 1:45f1573d65a1 168 void PID_m1()//left
palmdotax 1:45f1573d65a1 169 {
palmdotax 1:45f1573d65a1 170 setp1=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 1:45f1573d65a1 171 P1.setSetPoint(setp1);
palmdotax 1:45f1573d65a1 172 times=timerStart.read();
palmdotax 1:45f1573d65a1 173 if(times==1)// m/s
palmdotax 1:45f1573d65a1 174 {
palmdotax 1:45f1573d65a1 175 getvelo1();
palmdotax 1:45f1573d65a1 176 //pc.printf("TIME \n");
palmdotax 1:45f1573d65a1 177 times=0;
palmdotax 1:45f1573d65a1 178 pulse_1=0;
palmdotax 1:45f1573d65a1 179 }
palmdotax 1:45f1573d65a1 180 P1.setProcessValue(valocity1);
palmdotax 1:45f1573d65a1 181 outPID=P1.compute();
palmdotax 1:45f1573d65a1 182 //pc.printf("outPID=%f \n",outPID);
palmdotax 1:45f1573d65a1 183 m1.movespeed_1(setp1,outPID);
palmdotax 1:45f1573d65a1 184 }
palmdotax 1:45f1573d65a1 185 void PID_m2()//right
palmdotax 1:45f1573d65a1 186 {
palmdotax 1:45f1573d65a1 187 setp2=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 1:45f1573d65a1 188 P2.setSetPoint(setp2);
palmdotax 1:45f1573d65a1 189 times=timerStart.read();
palmdotax 1:45f1573d65a1 190 if(times==1)// m/s
palmdotax 1:45f1573d65a1 191 {
palmdotax 1:45f1573d65a1 192 getvelo2();
palmdotax 1:45f1573d65a1 193 //pc.printf("TIME \n");
palmdotax 1:45f1573d65a1 194 times=0;
palmdotax 1:45f1573d65a1 195 pulse_2=0;
palmdotax 1:45f1573d65a1 196 }
palmdotax 1:45f1573d65a1 197 P2.setProcessValue(valocity2);
palmdotax 1:45f1573d65a1 198 outPID=P2.compute();
palmdotax 1:45f1573d65a1 199 //pc.printf("outPID=%f \n",outPID);
palmdotax 2:f873deba2305 200 m1.movespeed_2(setp2,outPID);
palmdotax 1:45f1573d65a1 201 }
palmdotax 1:45f1573d65a1 202
palmdotax 1:45f1573d65a1 203
palmdotax 2:f873deba2305 204 void RC()
palmdotax 1:45f1573d65a1 205 {
palmdotax 1:45f1573d65a1 206 myled =1;
palmdotax 1:45f1573d65a1 207 uint8_t data_array[30];
palmdotax 1:45f1573d65a1 208 uint8_t id=0;
palmdotax 1:45f1573d65a1 209 uint8_t ins=0;
palmdotax 1:45f1573d65a1 210 uint8_t status=0xFF;
palmdotax 1:45f1573d65a1 211
palmdotax 1:45f1573d65a1 212
palmdotax 1:45f1573d65a1 213
palmdotax 1:45f1573d65a1 214 status = com.ReceiveCommand(&id,data_array,&ins);
palmdotax 1:45f1573d65a1 215 PC.printf("status = 0x%02x\n\r",status);
palmdotax 1:45f1573d65a1 216 if(status == ANDANTE_ERRBIT_NONE) {
palmdotax 1:45f1573d65a1 217 CmdCheck((int16_t)id,data_array,ins);
palmdotax 1:45f1573d65a1 218 PC.printf("s******************************");
palmdotax 1:45f1573d65a1 219 }
palmdotax 1:45f1573d65a1 220
palmdotax 1:45f1573d65a1 221 }
palmdotax 1:45f1573d65a1 222 /*******************************************************/
icyzkungz 0:84f05cd2f197 223 int main()
icyzkungz 0:84f05cd2f197 224 {
palmdotax 1:45f1573d65a1 225 PC.baud(115200);
palmdotax 2:f873deba2305 226 lidar.begin();
palmdotax 1:45f1573d65a1 227 printf("******************");
palmdotax 2:f873deba2305 228
palmdotax 2:f873deba2305 229
palmdotax 2:f873deba2305 230
palmdotax 2:f873deba2305 231 encoderA_1.rise(&EncoderA_1);
palmdotax 2:f873deba2305 232 timerStart.start();
palmdotax 2:f873deba2305 233 P1.setMode(1);
palmdotax 2:f873deba2305 234 P1.setBias(0);
palmdotax 2:f873deba2305 235 // pc.printf("READY \n");
palmdotax 2:f873deba2305 236 //pc.attach(&Rx_interrupt, Serial::RxIrq);
palmdotax 1:45f1573d65a1 237 while(1) {
palmdotax 1:45f1573d65a1 238
palmdotax 2:f873deba2305 239
palmdotax 2:f873deba2305 240 get_rplidar();
palmdotax 2:f873deba2305 241 RC();
palmdotax 1:45f1573d65a1 242 //wait_ms(1);
icyzkungz 0:84f05cd2f197 243 }
icyzkungz 0:84f05cd2f197 244 }
icyzkungz 0:84f05cd2f197 245
palmdotax 1:45f1573d65a1 246
palmdotax 1:45f1573d65a1 247
icyzkungz 0:84f05cd2f197 248
palmdotax 1:45f1573d65a1 249
icyzkungz 0:84f05cd2f197 250
icyzkungz 0:84f05cd2f197 251
icyzkungz 0:84f05cd2f197 252
icyzkungz 0:84f05cd2f197 253
palmdotax 1:45f1573d65a1 254 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
palmdotax 1:45f1573d65a1 255 {
palmdotax 1:45f1573d65a1 256 PC.printf("cmdcheck\n");
palmdotax 1:45f1573d65a1 257 if(id==MY_ID) {
palmdotax 1:45f1573d65a1 258 switch (ins) {
palmdotax 1:45f1573d65a1 259 case PING: {
palmdotax 1:45f1573d65a1 260 break;
palmdotax 1:45f1573d65a1 261 }
palmdotax 1:45f1573d65a1 262 case WRITE_DATA: {
palmdotax 1:45f1573d65a1 263 switch (command[0]) {
palmdotax 1:45f1573d65a1 264 case ID: {
palmdotax 1:45f1573d65a1 265 ///
palmdotax 1:45f1573d65a1 266 MY_ID = (int16_t)command[1];
palmdotax 1:45f1573d65a1 267 break;
palmdotax 1:45f1573d65a1 268 }
palmdotax 1:45f1573d65a1 269 case SET_VELOCITY_LEFT: {
palmdotax 1:45f1573d65a1 270 //
palmdotax 1:45f1573d65a1 271 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 272 float Int;
palmdotax 1:45f1573d65a1 273 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 274 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 275 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 276 VL=Int/1000;
palmdotax 1:45f1573d65a1 277 PC.printf("VL=%f /n",VL);
palmdotax 1:45f1573d65a1 278 break;
palmdotax 1:45f1573d65a1 279 }
palmdotax 1:45f1573d65a1 280 case SET_VELOCITY_RIGHT: {
palmdotax 1:45f1573d65a1 281 //
palmdotax 1:45f1573d65a1 282 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 283 float Int;
palmdotax 1:45f1573d65a1 284 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 285 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 286 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 287 VR=Int/1000;
palmdotax 1:45f1573d65a1 288 break;
palmdotax 1:45f1573d65a1 289 }
palmdotax 1:45f1573d65a1 290 case SET_VELOCITY_MAX_LEFT: {
palmdotax 1:45f1573d65a1 291 //
palmdotax 1:45f1573d65a1 292 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 293 float Int;
palmdotax 1:45f1573d65a1 294 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 295 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 296 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 297 VLmax=Int/1000;
palmdotax 1:45f1573d65a1 298 break;
palmdotax 1:45f1573d65a1 299 }
palmdotax 1:45f1573d65a1 300 case SET_VELOCITY_MAX_RIGHT: {
palmdotax 1:45f1573d65a1 301 //
palmdotax 1:45f1573d65a1 302 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 303 float Int;
palmdotax 1:45f1573d65a1 304 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 305 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 306 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 307 VRmax=Int/1000;
palmdotax 1:45f1573d65a1 308 PC.printf("VRmax = %f",VRmax);
palmdotax 1:45f1573d65a1 309 PC.printf("*****************************");
palmdotax 1:45f1573d65a1 310 break;
palmdotax 1:45f1573d65a1 311 }
palmdotax 2:f873deba2305 312 //save to rom
palmdotax 1:45f1573d65a1 313 case SET_KP_LEFT: {
palmdotax 1:45f1573d65a1 314 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 315 float Int;
palmdotax 1:45f1573d65a1 316 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 317 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 318 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 319 KP_LEFT=Int/1000;
palmdotax 2:f873deba2305 320 int32_t data_buff;
palmdotax 2:f873deba2305 321 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 322 memory.write(ADDRESS_LEFT_KP,data_buff);
palmdotax 2:f873deba2305 323 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 324 break;
palmdotax 1:45f1573d65a1 325 }
palmdotax 1:45f1573d65a1 326 case SET_KI_LEFT: {
palmdotax 1:45f1573d65a1 327 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 328 float Int;
palmdotax 1:45f1573d65a1 329 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 330 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 331 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 332 KI_LEFT=Int/1000;
palmdotax 2:f873deba2305 333 int32_t data_buff;
palmdotax 2:f873deba2305 334 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 335 memory.write(ADDRESS_LEFT_KI ,data_buff);
palmdotax 2:f873deba2305 336 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 337 break;
palmdotax 1:45f1573d65a1 338 }
palmdotax 1:45f1573d65a1 339 case SET_KD_LEFT: {
palmdotax 1:45f1573d65a1 340 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 341 float Int;
palmdotax 1:45f1573d65a1 342 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 343 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 344 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 345 KD_LEFT=Int/1000;
palmdotax 2:f873deba2305 346 int32_t data_buff;
palmdotax 2:f873deba2305 347 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 348 memory.write(ADDRESS_LEFT_KD,data_buff);
palmdotax 2:f873deba2305 349 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 350 break;
palmdotax 1:45f1573d65a1 351 }
palmdotax 1:45f1573d65a1 352 case SET_KP_RIGHT: {
palmdotax 1:45f1573d65a1 353 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 354 float Int;
palmdotax 1:45f1573d65a1 355 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 356 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 357 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 358 KP_RIGHT=Int/1000;
palmdotax 2:f873deba2305 359 int32_t data_buff;
palmdotax 2:f873deba2305 360 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 361 memory.write(ADDRESS_RIGHT_KP,data_buff);
palmdotax 2:f873deba2305 362 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 363 break;
palmdotax 1:45f1573d65a1 364 }
palmdotax 1:45f1573d65a1 365 case SET_KI_RIGHT: {
palmdotax 1:45f1573d65a1 366 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 367 float Int;
palmdotax 1:45f1573d65a1 368 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 369 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 370 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 371 KI_RIGHT=Int/1000;
palmdotax 2:f873deba2305 372 int32_t data_buff;
palmdotax 2:f873deba2305 373 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 374 memory.write(ADDRESS_RIGHT_KI,data_buff);
palmdotax 2:f873deba2305 375 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 376 break;
palmdotax 1:45f1573d65a1 377 }
palmdotax 1:45f1573d65a1 378 case SET_KD_RIGHT: {
palmdotax 1:45f1573d65a1 379 uint8_t int_buffer[2];
palmdotax 1:45f1573d65a1 380 float Int;
palmdotax 1:45f1573d65a1 381 int_buffer[0]=command[1];
palmdotax 1:45f1573d65a1 382 int_buffer[1]=command[2];
palmdotax 1:45f1573d65a1 383 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
palmdotax 1:45f1573d65a1 384 KD_RIGHT=Int/1000;
palmdotax 2:f873deba2305 385 int32_t data_buff;
palmdotax 2:f873deba2305 386 data_buff = Utilities::ConvertUInt8ArrayToInt32(int_buffer);
palmdotax 2:f873deba2305 387 memory.write(ADDRESS_RIGHT_KD,data_buff);
palmdotax 2:f873deba2305 388 wait_ms(EEPROM_DELAY);
palmdotax 1:45f1573d65a1 389 break;
palmdotax 1:45f1573d65a1 390 }
palmdotax 1:45f1573d65a1 391 }
palmdotax 1:45f1573d65a1 392 } break;
palmdotax 1:45f1573d65a1 393 case READ_DATA: {
palmdotax 1:45f1573d65a1 394 switch (command[0]) {
palmdotax 1:45f1573d65a1 395 case GET_LIDAR: {
palmdotax 1:45f1573d65a1 396
palmdotax 3:edaab92dbd2f 397
palmdotax 1:45f1573d65a1 398 break;
palmdotax 1:45f1573d65a1 399 }
palmdotax 1:45f1573d65a1 400 case GET_BATTERY: {
palmdotax 1:45f1573d65a1 401
palmdotax 1:45f1573d65a1 402 break;
palmdotax 1:45f1573d65a1 403 }
palmdotax 1:45f1573d65a1 404 case GET_VELOCITY_LEFT: {
palmdotax 1:45f1573d65a1 405 uint8_t intVelo_L[2],floatVelo_L[2];
palmdotax 1:45f1573d65a1 406 com.FloatSep(valocity1,intVelo_L,floatVelo_L);
palmdotax 1:45f1573d65a1 407
palmdotax 1:45f1573d65a1 408
palmdotax 1:45f1573d65a1 409 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 410
palmdotax 1:45f1573d65a1 411 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 412 package.length = 6;
palmdotax 1:45f1573d65a1 413 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 414 package.parameter[0]=intVelo_L[0];
palmdotax 1:45f1573d65a1 415 package.parameter[1]=intVelo_L[1];
palmdotax 1:45f1573d65a1 416 package.parameter[2]=floatVelo_L[0];
palmdotax 1:45f1573d65a1 417 package.parameter[3]=floatVelo_L[1];
palmdotax 1:45f1573d65a1 418
palmdotax 1:45f1573d65a1 419 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 420 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 421 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 422
palmdotax 1:45f1573d65a1 423
palmdotax 1:45f1573d65a1 424 break;
palmdotax 1:45f1573d65a1 425 }
palmdotax 1:45f1573d65a1 426 case GET_VELOCITY_RIGHT : {
palmdotax 1:45f1573d65a1 427 uint8_t intVelo_R[2],floatVelo_R[2];
palmdotax 1:45f1573d65a1 428 com.FloatSep(valocity2,intVelo_R,floatVelo_R);
palmdotax 1:45f1573d65a1 429
palmdotax 1:45f1573d65a1 430
palmdotax 1:45f1573d65a1 431 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 432
palmdotax 1:45f1573d65a1 433 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 434 package.length = 6;
palmdotax 1:45f1573d65a1 435 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 436 package.parameter[0]=intVelo_R[0];
palmdotax 1:45f1573d65a1 437 package.parameter[1]=intVelo_R[1];
palmdotax 1:45f1573d65a1 438 package.parameter[2]=floatVelo_R[0];
palmdotax 1:45f1573d65a1 439 package.parameter[3]=floatVelo_R[1];
palmdotax 1:45f1573d65a1 440
palmdotax 1:45f1573d65a1 441 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 442 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 443 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 444
palmdotax 1:45f1573d65a1 445 break;
palmdotax 1:45f1573d65a1 446 }
palmdotax 1:45f1573d65a1 447 case GET_KP_LEFT: {
palmdotax 2:f873deba2305 448 memory.read(ADDRESS_LEFT_KP ,KP_LEFT_BUFF);
palmdotax 1:45f1573d65a1 449 uint8_t intKPL[2],floatKPL[2];
palmdotax 2:f873deba2305 450 com.FloatSep(KP_LEFT_BUFF,intKPL,floatKPL);
palmdotax 1:45f1573d65a1 451
palmdotax 1:45f1573d65a1 452
palmdotax 1:45f1573d65a1 453 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 454
palmdotax 1:45f1573d65a1 455 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 456 package.length = 6;
palmdotax 1:45f1573d65a1 457 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 458 package.parameter[0]=intKPL[0];
palmdotax 1:45f1573d65a1 459 package.parameter[1]=intKPL[1];
palmdotax 1:45f1573d65a1 460 package.parameter[2]=floatKPL[0];
palmdotax 1:45f1573d65a1 461 package.parameter[3]=floatKPL[1];
palmdotax 1:45f1573d65a1 462
palmdotax 1:45f1573d65a1 463 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 464 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 465 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 466
palmdotax 1:45f1573d65a1 467 break;
palmdotax 1:45f1573d65a1 468 }
palmdotax 1:45f1573d65a1 469 case GET_KI_LEFT: {
palmdotax 2:f873deba2305 470 memory.read(ADDRESS_LEFT_KP ,KI_LEFT_BUFF);
palmdotax 1:45f1573d65a1 471 uint8_t intKIL[2],floatKIL[2];
palmdotax 2:f873deba2305 472 com.FloatSep(KI_LEFT_BUFF,intKIL,floatKIL);
palmdotax 1:45f1573d65a1 473
palmdotax 1:45f1573d65a1 474
palmdotax 1:45f1573d65a1 475 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 476
palmdotax 1:45f1573d65a1 477 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 478 package.length = 6;
palmdotax 1:45f1573d65a1 479 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 480 package.parameter[0]=intKIL[0];
palmdotax 1:45f1573d65a1 481 package.parameter[1]=intKIL[1];
palmdotax 1:45f1573d65a1 482 package.parameter[2]=floatKIL[0];
palmdotax 1:45f1573d65a1 483 package.parameter[3]=floatKIL[1];
palmdotax 1:45f1573d65a1 484
palmdotax 1:45f1573d65a1 485 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 486 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 487 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 488
palmdotax 1:45f1573d65a1 489 break;
palmdotax 1:45f1573d65a1 490 }
palmdotax 1:45f1573d65a1 491 case GET_KD_LEFT: {
palmdotax 2:f873deba2305 492 memory.read(ADDRESS_LEFT_KP ,KD_LEFT_BUFF);
palmdotax 1:45f1573d65a1 493 uint8_t intKDL[2],floatKDL[2];
palmdotax 2:f873deba2305 494 com.FloatSep(KD_LEFT_BUFF,intKDL,floatKDL);
palmdotax 1:45f1573d65a1 495
palmdotax 1:45f1573d65a1 496
palmdotax 1:45f1573d65a1 497 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 498
palmdotax 1:45f1573d65a1 499 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 500 package.length = 6;
palmdotax 1:45f1573d65a1 501 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 502 package.parameter[0]=intKDL[0];
palmdotax 1:45f1573d65a1 503 package.parameter[1]=intKDL[1];
palmdotax 1:45f1573d65a1 504 package.parameter[2]=floatKDL[0];
palmdotax 1:45f1573d65a1 505 package.parameter[3]=floatKDL[1];
palmdotax 1:45f1573d65a1 506
palmdotax 1:45f1573d65a1 507 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 508 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 509 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 510
palmdotax 1:45f1573d65a1 511 break;
palmdotax 1:45f1573d65a1 512 }
palmdotax 1:45f1573d65a1 513 case GET_KP_RIGHT: {
palmdotax 2:f873deba2305 514 memory.read(ADDRESS_LEFT_KP ,KP_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 515 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 516 com.FloatSep(KP_RIGHT_BUFF,intKDR,floatKDR);
palmdotax 1:45f1573d65a1 517
palmdotax 1:45f1573d65a1 518
palmdotax 1:45f1573d65a1 519 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 520
palmdotax 1:45f1573d65a1 521 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 522 package.length = 6;
palmdotax 1:45f1573d65a1 523 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 524 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 525 package.parameter[1]=intKDR[1];
palmdotax 1:45f1573d65a1 526 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 527 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 528
palmdotax 1:45f1573d65a1 529 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 530 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 531 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 532
palmdotax 1:45f1573d65a1 533 break;
palmdotax 1:45f1573d65a1 534 }
palmdotax 1:45f1573d65a1 535 case GET_KI_RIGHT: {
palmdotax 2:f873deba2305 536 memory.read(ADDRESS_LEFT_KP ,KI_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 537 uint8_t intKIR[2],floatKIR[2];
palmdotax 2:f873deba2305 538 com.FloatSep(KI_RIGHT_BUFF,intKIR,floatKIR);
palmdotax 1:45f1573d65a1 539
palmdotax 1:45f1573d65a1 540
palmdotax 1:45f1573d65a1 541 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 542
palmdotax 1:45f1573d65a1 543 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 544 package.length = 6;
palmdotax 1:45f1573d65a1 545 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 546 package.parameter[0]=intKIR[0];
palmdotax 1:45f1573d65a1 547 package.parameter[1]=intKIR[1];
palmdotax 1:45f1573d65a1 548 package.parameter[2]=floatKIR[0];
palmdotax 1:45f1573d65a1 549 package.parameter[3]=floatKIR[1];
palmdotax 1:45f1573d65a1 550
palmdotax 1:45f1573d65a1 551 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 552 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 553 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 554
palmdotax 1:45f1573d65a1 555 break;
palmdotax 1:45f1573d65a1 556 }
palmdotax 1:45f1573d65a1 557 case GET_KD_RIGHT: {
palmdotax 2:f873deba2305 558 memory.read(ADDRESS_LEFT_KP ,KD_RIGHT_BUFF);
palmdotax 1:45f1573d65a1 559 uint8_t intKDR[2],floatKDR[2];
palmdotax 2:f873deba2305 560 com.FloatSep(KD_RIGHT_BUFF,intKDR,floatKDR);
palmdotax 1:45f1573d65a1 561
palmdotax 1:45f1573d65a1 562
palmdotax 1:45f1573d65a1 563 ANDANTE_PROTOCOL_PACKET package;
palmdotax 1:45f1573d65a1 564
palmdotax 1:45f1573d65a1 565 package.robotId = MY_ID;
palmdotax 1:45f1573d65a1 566 package.length = 6;
palmdotax 1:45f1573d65a1 567 package.instructionErrorId = WRITE_DATA;
palmdotax 1:45f1573d65a1 568 package.parameter[0]=intKDR[0];
palmdotax 1:45f1573d65a1 569 package.parameter[1]=intKDR[1];
palmdotax 1:45f1573d65a1 570 package.parameter[2]=floatKDR[0];
palmdotax 1:45f1573d65a1 571 package.parameter[3]=floatKDR[1];
palmdotax 1:45f1573d65a1 572
palmdotax 1:45f1573d65a1 573 rs485_dirc1=1;
palmdotax 1:45f1573d65a1 574 wait_us(RS485_DELAY);
palmdotax 1:45f1573d65a1 575 com1->sendCommunicatePacket(&package);
palmdotax 1:45f1573d65a1 576
palmdotax 1:45f1573d65a1 577 break;
palmdotax 1:45f1573d65a1 578 }
palmdotax 1:45f1573d65a1 579 }
palmdotax 1:45f1573d65a1 580 }break;
palmdotax 1:45f1573d65a1 581
palmdotax 1:45f1573d65a1 582 }
palmdotax 1:45f1573d65a1 583 }
palmdotax 1:45f1573d65a1 584 }