v2
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of clean_V1 by
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
- Child:
- 4:de5a65c17664
55+
Who changed what in which revision?
User | Revision | Line number | New 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 | } |