Betago / Mbed 2 deprecated clean_V1

v1

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

Fork of CleaningMachine_Betago by palm and chin

main.cpp@1:45f1573d65a1, 2016-03-21 (annotated)

Committer:
palmdotax
Date:
Mon Mar 21 20:21:12 2016 +0000
Revision:
1:45f1573d65a1
Parent:
0:84f05cd2f197
Child:
2:f873deba2305
aaaaa

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