55+
Dependencies: BEAR_Protocol_Edited BufferedSerial Debug MaxSonar PID Process QEI UI iSerial mbed
Fork of CleaningMachine_Betago by
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?
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 | 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 | } |