Secret_Betago / Mbed 2 deprecated Low_Level

first

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of BEAR_Motion by BE@R lab

main.cpp@34:0cf04acfe422, 2016-02-03 (annotated)

Committer:
soulx
Date:
Wed Feb 03 14:39:39 2016 +0000
Revision:
34:0cf04acfe422
Parent:
32:1f81f3e83889
Child:
36:1561b6d61095
?? pid ????; ????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParinyaT 0:451c27e4d55e 1 //*****************************************************/
b0ssiz 6:98871feebea0 2 // Include //
ParinyaT 0:451c27e4d55e 3 #include "mbed.h"
ParinyaT 0:451c27e4d55e 4 #include "pinconfig.h"
soulx 34:0cf04acfe422 5 //#include "PID.h"
ParinyaT 0:451c27e4d55e 6 #include "Motor.h"
ParinyaT 0:451c27e4d55e 7 #include "eeprom.h"
b0ssiz 6:98871feebea0 8 #include "Receiver.h"
b0ssiz 22:449f31da2d3d 9 #include "Motion_EEPROM_Address.h"
soulx 34:0cf04acfe422 10
soulx 34:0cf04acfe422 11 #include "pidcontrol.h"
soulx 34:0cf04acfe422 12
b0ssiz 31:d6fa5e8e26b3 13 #define EEPROM_DELAY 2
soulx 34:0cf04acfe422 14
soulx 34:0cf04acfe422 15 //#define DEBUG_UP
soulx 34:0cf04acfe422 16 //#define DEBUG_LOW
ParinyaT 0:451c27e4d55e 17 //*****************************************************/
ParinyaT 1:84167ca00307 18 //--PID parameter--
ParinyaT 1:84167ca00307 19 //-Upper-//
soulx 34:0cf04acfe422 20 float U_Kc=0.2;
soulx 34:0cf04acfe422 21 float U_Ki_gain=0.0;
soulx 34:0cf04acfe422 22 float U_Kd_gain=0.0;
soulx 34:0cf04acfe422 23 float U_Ti=0.0;
soulx 34:0cf04acfe422 24 float U_Td=0.0;
ParinyaT 16:c0a1daeb9fa5 25 float U_Ki=U_Kc*U_Ti;
ParinyaT 16:c0a1daeb9fa5 26 float U_Kd=U_Kc*U_Td;
ParinyaT 1:84167ca00307 27 //-lower-//
soulx 34:0cf04acfe422 28 float L_Kc=0.2;
soulx 34:0cf04acfe422 29 float L_Ki_gain=0.0;
soulx 34:0cf04acfe422 30 float L_Kd_gain=0.0;
soulx 34:0cf04acfe422 31 float L_Ti=0.0;
soulx 34:0cf04acfe422 32 float L_Td=0.0;
ParinyaT 16:c0a1daeb9fa5 33 float L_Ki=L_Kc*L_Ti;
ParinyaT 16:c0a1daeb9fa5 34 float L_Kd=L_Kc*L_Td;
ParinyaT 0:451c27e4d55e 35 //*****************************************************/
ParinyaT 0:451c27e4d55e 36 // Global //
ParinyaT 11:3dd92d1d542c 37 Ticker Recieve;
ParinyaT 1:84167ca00307 38 //-- Communication --
ParinyaT 0:451c27e4d55e 39 Serial PC(D1,D0);
b0ssiz 17:4c96838e579f 40 Bear_Receiver com(Tx,Rx,1000000);
b0ssiz 30:3f8e86fa1413 41 int16_t MY_ID = 0x02;
ParinyaT 11:3dd92d1d542c 42 //-- Memorry --
ParinyaT 11:3dd92d1d542c 43 EEPROM memory(PB_4,PA_8,0);
soulx 34:0cf04acfe422 44 float UpMargin;
soulx 34:0cf04acfe422 45 float LowMargin;
ParinyaT 13:49cb002ad8fd 46 uint8_t Height[4];
ParinyaT 13:49cb002ad8fd 47 uint8_t Wheelpos[4];
ParinyaT 13:49cb002ad8fd 48 uint8_t Mag[24];
soulx 26:fbccc263a588 49 uint8_t Offset[8];//={1,2,3,4,5,6,7,8};
b0ssiz 14:28e24fcc5a01 50 uint8_t Body_width[4];
ParinyaT 16:c0a1daeb9fa5 51 uint8_t Angle_Range_Up[8];
ParinyaT 16:c0a1daeb9fa5 52 uint8_t Angle_Range_Low[8];
b0ssiz 22:449f31da2d3d 53 uint8_t UpLinkLength[4];
b0ssiz 22:449f31da2d3d 54 uint8_t LowLinkLength[4];
b0ssiz 6:98871feebea0 55 //-- encoder --
ParinyaT 13:49cb002ad8fd 56 float up_angle,low_angle;
b0ssiz 31:d6fa5e8e26b3 57 float Upper_Position;
b0ssiz 31:d6fa5e8e26b3 58 float Lower_Position;
ParinyaT 0:451c27e4d55e 59 int data;
ParinyaT 1:84167ca00307 60 SPI ENC(Emosi, Emiso, Esck);
ParinyaT 1:84167ca00307 61 DigitalOut EncA(EncoderA);
ParinyaT 1:84167ca00307 62 DigitalOut EncB(EncoderB);
ParinyaT 0:451c27e4d55e 63 //-- Motor --
ParinyaT 1:84167ca00307 64 int dir;
ParinyaT 1:84167ca00307 65 Motor Upper(PWM_LU,A_LU,B_LU);
ParinyaT 1:84167ca00307 66 Motor Lower(PWM_LL,A_LL,B_LL);
ParinyaT 0:451c27e4d55e 67 //-- PID --
soulx 34:0cf04acfe422 68 int Upper_SetPoint=20;
soulx 34:0cf04acfe422 69 int Lower_SetPoint=8;
soulx 34:0cf04acfe422 70 //PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
soulx 34:0cf04acfe422 71 //PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
soulx 34:0cf04acfe422 72
soulx 34:0cf04acfe422 73 PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
soulx 34:0cf04acfe422 74 PID Low_PID(L_Kc, L_Ti, L_Td);
soulx 34:0cf04acfe422 75
ParinyaT 0:451c27e4d55e 76 //*****************************************************/
soulx 34:0cf04acfe422 77 void Start_Up();
soulx 34:0cf04acfe422 78 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
soulx 34:0cf04acfe422 79
soulx 34:0cf04acfe422 80 Timer counterUP;
soulx 34:0cf04acfe422 81 Timer counterLOW;
soulx 20:7e6d56655336 82
soulx 20:7e6d56655336 83 DigitalOut myled(LED1);
soulx 20:7e6d56655336 84
soulx 20:7e6d56655336 85
ParinyaT 1:84167ca00307 86 void Read_Encoder(PinName Encoder)
ParinyaT 0:451c27e4d55e 87 {
soulx 7:bf239d051e8c 88 ENC.format(8,0);
soulx 7:bf239d051e8c 89 ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
b0ssiz 6:98871feebea0 90
soulx 7:bf239d051e8c 91 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 92 EncA = 0;
soulx 7:bf239d051e8c 93 } else {
soulx 7:bf239d051e8c 94 EncB = 0;
soulx 7:bf239d051e8c 95 }
soulx 7:bf239d051e8c 96 ENC.write(0x41);
soulx 7:bf239d051e8c 97 ENC.write(0x09);
soulx 7:bf239d051e8c 98 data = ENC.write(0x00);
soulx 7:bf239d051e8c 99 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 100 EncA = 1;
soulx 7:bf239d051e8c 101 } else {
soulx 7:bf239d051e8c 102 EncB = 1;
soulx 7:bf239d051e8c 103 }
b0ssiz 6:98871feebea0 104
ParinyaT 0:451c27e4d55e 105 }
ParinyaT 0:451c27e4d55e 106 //*****************************************************/
ParinyaT 1:84167ca00307 107 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 108 {
ParinyaT 0:451c27e4d55e 109 int i = 0;
ParinyaT 0:451c27e4d55e 110 static unsigned char codes[] = {
b0ssiz 6:98871feebea0 111 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 112 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 113 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 114 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 115 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 116 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 117 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 118 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 119 };
b0ssiz 6:98871feebea0 120
b0ssiz 6:98871feebea0 121 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 122 i++;
ParinyaT 0:451c27e4d55e 123 }
b0ssiz 6:98871feebea0 124
ParinyaT 1:84167ca00307 125 return i;
b0ssiz 6:98871feebea0 126
ParinyaT 0:451c27e4d55e 127 }
ParinyaT 0:451c27e4d55e 128 //*****************************************************/
ParinyaT 1:84167ca00307 129 void SET_UpperPID()
ParinyaT 1:84167ca00307 130 {
ParinyaT 1:84167ca00307 131 Upper.period(0.001);
soulx 34:0cf04acfe422 132
soulx 34:0cf04acfe422 133 memory.read(ADDRESS_UP_MARGIN,UpMargin);
soulx 34:0cf04acfe422 134 Up_PID.setMargin(UpMargin);
soulx 34:0cf04acfe422 135
soulx 34:0cf04acfe422 136 memory.read(ADDRESS_UPPER_KP,U_Kc);
soulx 34:0cf04acfe422 137 Up_PID.setKp(U_Kc);
soulx 34:0cf04acfe422 138 memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
soulx 34:0cf04acfe422 139 Up_PID.setKi(U_Ki_gain);
soulx 34:0cf04acfe422 140 memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
soulx 34:0cf04acfe422 141 Up_PID.setKd(U_Kd_gain);
soulx 34:0cf04acfe422 142
soulx 34:0cf04acfe422 143 //Up_PID.setMode(0);
soulx 34:0cf04acfe422 144 //Up_PID.setInputLimits(18,62);
soulx 34:0cf04acfe422 145 //Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 146 }
ParinyaT 1:84167ca00307 147 //******************************************************/
ParinyaT 1:84167ca00307 148 void SET_LowerPID()
ParinyaT 1:84167ca00307 149 {
ParinyaT 1:84167ca00307 150 Lower.period(0.001);
soulx 34:0cf04acfe422 151
soulx 34:0cf04acfe422 152 memory.read(ADDRESS_LOW_MARGIN,LowMargin);
soulx 34:0cf04acfe422 153 Low_PID.setMargin(LowMargin);
soulx 34:0cf04acfe422 154
soulx 34:0cf04acfe422 155 memory.read(ADDRESS_LOWER_KP,L_Kc);
soulx 34:0cf04acfe422 156 Low_PID.setKp(L_Kc);
soulx 34:0cf04acfe422 157 memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
soulx 34:0cf04acfe422 158 Low_PID.setKi(L_Ki_gain);
soulx 34:0cf04acfe422 159
soulx 34:0cf04acfe422 160 memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
soulx 34:0cf04acfe422 161 Low_PID.setKd(L_Kd_gain);
soulx 34:0cf04acfe422 162
soulx 34:0cf04acfe422 163 //Low_PID.setMode(0);
soulx 34:0cf04acfe422 164 //Low_PID.setInputLimits(0,127); // set range
soulx 34:0cf04acfe422 165 //Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 166 }
b0ssiz 6:98871feebea0 167 //******************************************************/
ParinyaT 1:84167ca00307 168 void Move_Upper()
ParinyaT 1:84167ca00307 169 {
ParinyaT 1:84167ca00307 170 Read_Encoder(EncoderA);
soulx 34:0cf04acfe422 171 Upper_Position = (float)Get_EnValue(data);
soulx 34:0cf04acfe422 172 #ifdef DEBUG_UP
soulx 34:0cf04acfe422 173 printf("read_encode = 0x%2x \n\r",data);
soulx 34:0cf04acfe422 174 printf("Setpoint = %d\n\r",Upper_SetPoint);
soulx 34:0cf04acfe422 175 printf("Upper_Position = %f\n\r",Upper_Position);
soulx 34:0cf04acfe422 176 #endif
soulx 34:0cf04acfe422 177 Up_PID.setCurrent(Upper_Position);
soulx 34:0cf04acfe422 178 float speed =Up_PID.compute();
soulx 34:0cf04acfe422 179 #ifdef DEBUG_UP
soulx 34:0cf04acfe422 180 printf("E_n= %f\n\r",Up_PID.getErrorNow());
soulx 34:0cf04acfe422 181 printf("Kp = %f\n\r",Up_PID.getKp());
soulx 34:0cf04acfe422 182 printf("speed = %f\n\n\n\r",speed);
soulx 34:0cf04acfe422 183 #endif
soulx 34:0cf04acfe422 184 Upper.speed(speed);
ParinyaT 1:84167ca00307 185 }
ParinyaT 1:84167ca00307 186 //******************************************************/
ParinyaT 1:84167ca00307 187 void Move_Lower()
ParinyaT 1:84167ca00307 188 {
ParinyaT 1:84167ca00307 189 Read_Encoder(EncoderB);
soulx 34:0cf04acfe422 190 Lower_Position = (float)Get_EnValue(data);
soulx 34:0cf04acfe422 191 Low_PID.setCurrent(Lower_Position);
soulx 34:0cf04acfe422 192 #ifdef DEBUG_LOW
soulx 34:0cf04acfe422 193 printf("read_encode = 0x%2x \n\r",data);
soulx 34:0cf04acfe422 194 printf("Setpoint = %d\n\r",Lower_SetPoint);
soulx 34:0cf04acfe422 195 printf("Upper_Position = %f\n\r",Lower_Position);
soulx 34:0cf04acfe422 196 #endif
b0ssiz 6:98871feebea0 197
soulx 34:0cf04acfe422 198 float speed =Low_PID.compute();
soulx 34:0cf04acfe422 199 #ifdef DEBUG_LOW
soulx 34:0cf04acfe422 200 printf("E_n= %f\n\r",Low_PID.getErrorNow());
soulx 34:0cf04acfe422 201 printf("Kp = %f\n\r",Low_PID.getKp());
soulx 34:0cf04acfe422 202 printf("speed = %f\n\n\n\r",speed);
soulx 34:0cf04acfe422 203 #endif
soulx 34:0cf04acfe422 204 Lower.speed(speed);
b0ssiz 6:98871feebea0 205 }
ParinyaT 1:84167ca00307 206 //******************************************************/
ParinyaT 1:84167ca00307 207
soulx 34:0cf04acfe422 208
soulx 34:0cf04acfe422 209 void Rc()
soulx 34:0cf04acfe422 210 {
soulx 34:0cf04acfe422 211 myled =1;
soulx 34:0cf04acfe422 212 uint8_t data_array[30];
soulx 34:0cf04acfe422 213 uint8_t id=0;
soulx 34:0cf04acfe422 214 uint8_t ins=0;
soulx 34:0cf04acfe422 215 uint8_t status=0xFF;
soulx 34:0cf04acfe422 216
soulx 34:0cf04acfe422 217
soulx 34:0cf04acfe422 218
soulx 34:0cf04acfe422 219 status = com.ReceiveCommand(&id,data_array,&ins);
soulx 34:0cf04acfe422 220 //PC.printf("status = 0x%02x\n\r",status);
soulx 34:0cf04acfe422 221 if(status == ANDANTE_ERRBIT_NONE) {
soulx 34:0cf04acfe422 222 CmdCheck((int16_t)id,data_array,ins);
soulx 34:0cf04acfe422 223 }
soulx 34:0cf04acfe422 224
soulx 34:0cf04acfe422 225 }
soulx 34:0cf04acfe422 226 /*******************************************************/
soulx 34:0cf04acfe422 227 int main()
soulx 34:0cf04acfe422 228 {
soulx 34:0cf04acfe422 229 PC.baud(115200);
soulx 34:0cf04acfe422 230 /*
soulx 34:0cf04acfe422 231 while(1)
soulx 34:0cf04acfe422 232 {
soulx 34:0cf04acfe422 233 Read_Encoder(EncoderA);
soulx 34:0cf04acfe422 234 Upper_Position = Get_EnValue(data);
soulx 34:0cf04acfe422 235 Read_Encoder(EncoderB);
soulx 34:0cf04acfe422 236 Lower_Position = Get_EnValue(data);
soulx 34:0cf04acfe422 237 PC.printf("Upper Position : %f\n",Upper_Position);
soulx 34:0cf04acfe422 238 PC.printf("Lower_Position : %f\n",Lower_Position);
soulx 34:0cf04acfe422 239 wait(0.5);
soulx 34:0cf04acfe422 240 }
soulx 34:0cf04acfe422 241 */
soulx 34:0cf04acfe422 242
soulx 34:0cf04acfe422 243
soulx 34:0cf04acfe422 244 //Recieve.attach(&Rc,0.025);
soulx 34:0cf04acfe422 245 //Start_Up();
soulx 34:0cf04acfe422 246
soulx 34:0cf04acfe422 247 SET_UpperPID();
soulx 34:0cf04acfe422 248 SET_LowerPID();
soulx 34:0cf04acfe422 249
soulx 34:0cf04acfe422 250 printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
soulx 34:0cf04acfe422 251 /*
soulx 34:0cf04acfe422 252 while(1)
soulx 34:0cf04acfe422 253 {
soulx 34:0cf04acfe422 254
soulx 34:0cf04acfe422 255 Upper.speed(-1);
soulx 34:0cf04acfe422 256 wait_ms(400);
soulx 34:0cf04acfe422 257 Upper.speed(1);
soulx 34:0cf04acfe422 258 wait_ms(400);
soulx 34:0cf04acfe422 259 Upper.break();
soulx 34:0cf04acfe422 260
soulx 34:0cf04acfe422 261 Lower.speed(-1.0);
soulx 34:0cf04acfe422 262 wait_ms(401);
soulx 34:0cf04acfe422 263 Lower.speed(1.0);
soulx 34:0cf04acfe422 264 wait_ms(401);
soulx 34:0cf04acfe422 265 Lower.break();
soulx 34:0cf04acfe422 266 }
soulx 34:0cf04acfe422 267 */
soulx 34:0cf04acfe422 268
soulx 34:0cf04acfe422 269 // counterUP.start();
soulx 34:0cf04acfe422 270 // counterLOW.start();
soulx 34:0cf04acfe422 271
soulx 34:0cf04acfe422 272 while(1) {
soulx 34:0cf04acfe422 273
soulx 34:0cf04acfe422 274 /*
soulx 34:0cf04acfe422 275 //printf("timer = %d\n\r",counter.read_ms());
soulx 34:0cf04acfe422 276 if(counterUP.read_ms() > 1400) {
soulx 34:0cf04acfe422 277
soulx 34:0cf04acfe422 278 if(Upper_SetPoint < 53) {
soulx 34:0cf04acfe422 279 Upper_SetPoint++;
soulx 34:0cf04acfe422 280 } else
soulx 34:0cf04acfe422 281 Upper_SetPoint =18;
soulx 34:0cf04acfe422 282
soulx 34:0cf04acfe422 283 counterUP.reset();
soulx 34:0cf04acfe422 284
soulx 34:0cf04acfe422 285 }
soulx 34:0cf04acfe422 286
soulx 34:0cf04acfe422 287 if(counterLOW.read_ms() > 700) {
soulx 34:0cf04acfe422 288
soulx 34:0cf04acfe422 289 if(Lower_SetPoint < 100) {
soulx 34:0cf04acfe422 290 Lower_SetPoint++;
soulx 34:0cf04acfe422 291 } else
soulx 34:0cf04acfe422 292 Lower_SetPoint =8;
soulx 34:0cf04acfe422 293
soulx 34:0cf04acfe422 294 counterLOW.reset();
soulx 34:0cf04acfe422 295
soulx 34:0cf04acfe422 296 }
soulx 34:0cf04acfe422 297 */
soulx 34:0cf04acfe422 298 myled =0;
soulx 34:0cf04acfe422 299 //wait_ms(10);
soulx 34:0cf04acfe422 300 ///////////////////////////////////////////////////// start
soulx 34:0cf04acfe422 301 //Set Set_point
soulx 34:0cf04acfe422 302 Up_PID.setGoal(Upper_SetPoint);
soulx 34:0cf04acfe422 303 Low_PID.setGoal(Lower_SetPoint);
soulx 34:0cf04acfe422 304
soulx 34:0cf04acfe422 305 //Control Motor
soulx 34:0cf04acfe422 306 Move_Upper();
soulx 34:0cf04acfe422 307 Move_Lower();
soulx 34:0cf04acfe422 308 ///////////////////////////////////////////////////// stop =306us
soulx 34:0cf04acfe422 309 //uint8_t aaa[1]={10};
soulx 34:0cf04acfe422 310 //com.sendBodyWidth(0x01,aaa);
soulx 34:0cf04acfe422 311 Rc();
soulx 34:0cf04acfe422 312 //wait_ms(1);
soulx 34:0cf04acfe422 313 }
soulx 34:0cf04acfe422 314 }
soulx 34:0cf04acfe422 315
soulx 34:0cf04acfe422 316
soulx 34:0cf04acfe422 317
soulx 34:0cf04acfe422 318
soulx 34:0cf04acfe422 319
soulx 34:0cf04acfe422 320
soulx 34:0cf04acfe422 321
soulx 34:0cf04acfe422 322
soulx 34:0cf04acfe422 323
b0ssiz 30:3f8e86fa1413 324 void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
b0ssiz 10:3b3d6bc88677 325 {
b0ssiz 14:28e24fcc5a01 326 if(id==MY_ID) {
b0ssiz 14:28e24fcc5a01 327 switch (ins) {
b0ssiz 14:28e24fcc5a01 328 case PING: {
soulx 18:face01c94152 329 break;
b0ssiz 14:28e24fcc5a01 330 }
b0ssiz 14:28e24fcc5a01 331 case WRITE_DATA: {
b0ssiz 30:3f8e86fa1413 332 switch (command[0]) {
b0ssiz 14:28e24fcc5a01 333 case ID: {
soulx 18:face01c94152 334 ///
b0ssiz 30:3f8e86fa1413 335 MY_ID = (int16_t)command[1];
soulx 18:face01c94152 336 break;
b0ssiz 14:28e24fcc5a01 337 }
b0ssiz 14:28e24fcc5a01 338 case MOTOR_UPPER_ANG: {
b0ssiz 32:1f81f3e83889 339 uint8_t IntUpAngle[2];
b0ssiz 32:1f81f3e83889 340 uint8_t IntLowAngle[2];
b0ssiz 14:28e24fcc5a01 341
b0ssiz 30:3f8e86fa1413 342 IntUpAngle[0]=command[1];
b0ssiz 30:3f8e86fa1413 343 IntUpAngle[1]=command[2];
soulx 34:0cf04acfe422 344 Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
b0ssiz 30:3f8e86fa1413 345 //printf("Up Angle = %f\n",up_angle);
b0ssiz 30:3f8e86fa1413 346 IntLowAngle[0]=command[5];
b0ssiz 30:3f8e86fa1413 347 IntLowAngle[1]=command[6];
soulx 34:0cf04acfe422 348 Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
b0ssiz 30:3f8e86fa1413 349 //printf("Low Angle = %f\n",low_angle);
soulx 18:face01c94152 350 break;
b0ssiz 14:28e24fcc5a01 351 }
b0ssiz 14:28e24fcc5a01 352 case UP_MARGIN: {
soulx 34:0cf04acfe422 353 uint8_t int_buffer[2];
soulx 34:0cf04acfe422 354 uint8_t float_buffer[2];
soulx 34:0cf04acfe422 355 float Int,Float;
soulx 34:0cf04acfe422 356 int_buffer[0]=command[1];
soulx 34:0cf04acfe422 357 int_buffer[1]=command[2];
soulx 34:0cf04acfe422 358 float_buffer[0]=command[3];
soulx 34:0cf04acfe422 359 float_buffer[1]=command[4];
soulx 34:0cf04acfe422 360 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
soulx 34:0cf04acfe422 361 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
soulx 34:0cf04acfe422 362 UpMargin=Int+Float;
soulx 34:0cf04acfe422 363 Up_PID.setMargin(UpMargin);
soulx 34:0cf04acfe422 364 //printf("Kp Upper : %f\r\n",U_Kc);
soulx 18:face01c94152 365 break;
b0ssiz 14:28e24fcc5a01 366 }
b0ssiz 14:28e24fcc5a01 367 case LOW_MARGIN: {
soulx 34:0cf04acfe422 368 uint8_t int_buffer[2];
soulx 34:0cf04acfe422 369 uint8_t float_buffer[2];
soulx 34:0cf04acfe422 370 float Int,Float;
soulx 34:0cf04acfe422 371 int_buffer[0]=command[1];
soulx 34:0cf04acfe422 372 int_buffer[1]=command[2];
soulx 34:0cf04acfe422 373 float_buffer[0]=command[3];
soulx 34:0cf04acfe422 374 float_buffer[1]=command[4];
soulx 34:0cf04acfe422 375 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
soulx 34:0cf04acfe422 376 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
soulx 34:0cf04acfe422 377 LowMargin=Int+Float;
soulx 34:0cf04acfe422 378 Low_PID.setMargin(LowMargin);
soulx 34:0cf04acfe422 379 //printf("Kp Upper : %f\r\n",U_Kc);
soulx 18:face01c94152 380 break;
b0ssiz 14:28e24fcc5a01 381 }
b0ssiz 14:28e24fcc5a01 382 case KP_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 383 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 384 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 385 float Int,Float;
b0ssiz 30:3f8e86fa1413 386 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 387 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 388 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 389 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 390 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 391 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 392 U_Kc=Int+Float;
b0ssiz 30:3f8e86fa1413 393 //printf("Kp Upper : %f\r\n",U_Kc);
soulx 34:0cf04acfe422 394 Up_PID.setKp(U_Kc);
soulx 18:face01c94152 395 break;
b0ssiz 14:28e24fcc5a01 396 }
b0ssiz 14:28e24fcc5a01 397 case KI_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 398 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 399 uint8_t float_buffer[2];
b0ssiz 30:3f8e86fa1413 400 float Int,Float;
b0ssiz 30:3f8e86fa1413 401 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 402 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 403 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 404 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 405 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 406 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 30:3f8e86fa1413 407 U_Ki_gain=Int+Float;
soulx 34:0cf04acfe422 408 //U_Ti=U_Ki_gain;
soulx 34:0cf04acfe422 409 Up_PID.setKi(U_Ki_gain);
b0ssiz 30:3f8e86fa1413 410 //printf("Ki Upper : %f\r\n",U_Ki_gain);
soulx 18:face01c94152 411 break;
b0ssiz 14:28e24fcc5a01 412 }
b0ssiz 14:28e24fcc5a01 413 case KD_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 414 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 415 uint8_t float_buffer[2];
b0ssiz 30:3f8e86fa1413 416 float Int,Float;
b0ssiz 30:3f8e86fa1413 417 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 418 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 419 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 420 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 421 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 422 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 30:3f8e86fa1413 423 U_Kd_gain=Int+Float;
soulx 34:0cf04acfe422 424 Up_PID.setKd(U_Kd_gain);
soulx 34:0cf04acfe422 425 //U_Td=U_Kd_gain/U_Kc;
b0ssiz 30:3f8e86fa1413 426 //printf("Kd Upper : %f\r\n",U_Kd_gain);
soulx 18:face01c94152 427 break;
b0ssiz 14:28e24fcc5a01 428 }
b0ssiz 14:28e24fcc5a01 429 case KP_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 430 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 431 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 432 float Int,Float;
b0ssiz 30:3f8e86fa1413 433 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 434 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 435 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 436 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 437 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 438 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 439 L_Kc=Int+Float;
b0ssiz 30:3f8e86fa1413 440 //printf("Kp Lower : %f\r\n",L_Kc);
soulx 18:face01c94152 441 break;
b0ssiz 14:28e24fcc5a01 442 }
b0ssiz 14:28e24fcc5a01 443 case KI_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 444 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 445 uint8_t float_buffer[2];
b0ssiz 30:3f8e86fa1413 446 float Int,Float;
b0ssiz 30:3f8e86fa1413 447 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 448 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 449 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 450 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 451 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 452 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 30:3f8e86fa1413 453 L_Ki_gain=Int+Float;
b0ssiz 30:3f8e86fa1413 454 L_Ti=L_Ki_gain/L_Kc;
b0ssiz 30:3f8e86fa1413 455 //printf("Ki Lower : %f\r\n",L_Ki_gain);
soulx 18:face01c94152 456 break;
b0ssiz 14:28e24fcc5a01 457 }
b0ssiz 14:28e24fcc5a01 458 case KD_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 459 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 460 uint8_t float_buffer[2];
b0ssiz 30:3f8e86fa1413 461 float Int,Float;
b0ssiz 30:3f8e86fa1413 462 int_buffer[0]=command[1];
b0ssiz 30:3f8e86fa1413 463 int_buffer[1]=command[2];
b0ssiz 30:3f8e86fa1413 464 float_buffer[0]=command[3];
b0ssiz 30:3f8e86fa1413 465 float_buffer[1]=command[4];
b0ssiz 14:28e24fcc5a01 466 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 467 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 30:3f8e86fa1413 468 L_Kd_gain=Int+Float;
b0ssiz 30:3f8e86fa1413 469 L_Td=L_Kd_gain/L_Kc;
b0ssiz 30:3f8e86fa1413 470 //printf("Kd Lower : %f\r\n",L_Kd_gain);
soulx 18:face01c94152 471 break;
b0ssiz 14:28e24fcc5a01 472 }
b0ssiz 14:28e24fcc5a01 473 case HEIGHT: {
b0ssiz 14:28e24fcc5a01 474 int i;
b0ssiz 14:28e24fcc5a01 475 for(i=0; i<4; i++) {
b0ssiz 30:3f8e86fa1413 476 Height[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 477 }
soulx 18:face01c94152 478 break;
b0ssiz 14:28e24fcc5a01 479 }
b0ssiz 14:28e24fcc5a01 480 case WHEELPOS: {
b0ssiz 14:28e24fcc5a01 481 int i;
b0ssiz 14:28e24fcc5a01 482 for(i=0; i<4; i++) {
b0ssiz 30:3f8e86fa1413 483 Wheelpos[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 484 }
soulx 18:face01c94152 485 break;
b0ssiz 14:28e24fcc5a01 486 }
b0ssiz 14:28e24fcc5a01 487 case MAG_DATA: {
b0ssiz 14:28e24fcc5a01 488 int i;
b0ssiz 14:28e24fcc5a01 489 for(i=0; i<24; i++) {
b0ssiz 30:3f8e86fa1413 490 Mag[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 491 }
soulx 18:face01c94152 492 break;
b0ssiz 14:28e24fcc5a01 493 }
b0ssiz 14:28e24fcc5a01 494 case OFFSET: {
b0ssiz 14:28e24fcc5a01 495 int i;
b0ssiz 14:28e24fcc5a01 496 for(i=0; i<8; i++) {
b0ssiz 30:3f8e86fa1413 497 Offset[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 498 }
soulx 18:face01c94152 499 break;
b0ssiz 14:28e24fcc5a01 500 }
b0ssiz 14:28e24fcc5a01 501 case BODY_WIDTH: {
b0ssiz 14:28e24fcc5a01 502 int i;
b0ssiz 14:28e24fcc5a01 503 for(i=0; i<4; i++) {
b0ssiz 30:3f8e86fa1413 504 Body_width[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 505 }
soulx 18:face01c94152 506 break;
b0ssiz 14:28e24fcc5a01 507 }
b0ssiz 14:28e24fcc5a01 508 case ANGLE_RANGE_UP: {
b0ssiz 14:28e24fcc5a01 509 int i;
ParinyaT 16:c0a1daeb9fa5 510 for(i=0; i<8; i++) {
b0ssiz 30:3f8e86fa1413 511 Angle_Range_Up[i]=command[1+i];
b0ssiz 32:1f81f3e83889 512 //printf("%d Angle = 0x%02x\r\n",i,Angle_Range_Up[i]);
b0ssiz 14:28e24fcc5a01 513 }
soulx 18:face01c94152 514 break;
b0ssiz 14:28e24fcc5a01 515 }
b0ssiz 14:28e24fcc5a01 516 case ANGLE_RANGE_LOW: {
b0ssiz 14:28e24fcc5a01 517 int i;
ParinyaT 16:c0a1daeb9fa5 518 for(i=0; i<8; i++) {
b0ssiz 30:3f8e86fa1413 519 Angle_Range_Low[0+i]=command[1+i];
b0ssiz 14:28e24fcc5a01 520 }
soulx 18:face01c94152 521 break;
ParinyaT 13:49cb002ad8fd 522 }
b0ssiz 22:449f31da2d3d 523
b0ssiz 22:449f31da2d3d 524 case UP_LINK_LENGTH: {
b0ssiz 22:449f31da2d3d 525 int i;
b0ssiz 22:449f31da2d3d 526 for(i=0; i<4; i++) {
b0ssiz 30:3f8e86fa1413 527 UpLinkLength[i]=command[1+i];
b0ssiz 22:449f31da2d3d 528 }
b0ssiz 22:449f31da2d3d 529 break;
b0ssiz 22:449f31da2d3d 530 }
b0ssiz 22:449f31da2d3d 531 case LOW_LINK_LENGTH: {
b0ssiz 22:449f31da2d3d 532 int i;
b0ssiz 22:449f31da2d3d 533 for(i=0; i<4; i++) {
b0ssiz 30:3f8e86fa1413 534 LowLinkLength[i]=command[1+i];
b0ssiz 22:449f31da2d3d 535 }
b0ssiz 22:449f31da2d3d 536 break;
b0ssiz 22:449f31da2d3d 537 }
ParinyaT 16:c0a1daeb9fa5 538 // unfinish yet!!!!!!!!!!!!!!!!!
ParinyaT 16:c0a1daeb9fa5 539 case SAVE_EEPROM_DATA: {
b0ssiz 22:449f31da2d3d 540 if(id==0x01) {
b0ssiz 22:449f31da2d3d 541
b0ssiz 30:3f8e86fa1413 542 if (command[1]==HEIGHT) {
b0ssiz 22:449f31da2d3d 543 int32_t data_buff;
b0ssiz 22:449f31da2d3d 544 data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
b0ssiz 22:449f31da2d3d 545 memory.write(ADDRESS_HEIGHT,data_buff);
b0ssiz 31:d6fa5e8e26b3 546 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 547
b0ssiz 30:3f8e86fa1413 548 } else if(command[1]==BODY_WIDTH) {
b0ssiz 22:449f31da2d3d 549 int32_t data_buff;
b0ssiz 22:449f31da2d3d 550 data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width);
b0ssiz 22:449f31da2d3d 551 memory.write(ADDRESS_BODY_WIDTH,data_buff);
b0ssiz 31:d6fa5e8e26b3 552 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 553
b0ssiz 30:3f8e86fa1413 554 } else if(command[1]==OFFSET) {
b0ssiz 22:449f31da2d3d 555 uint8_t y_offset_array[4];
b0ssiz 22:449f31da2d3d 556 uint8_t z_offset_array[4];
b0ssiz 22:449f31da2d3d 557 int32_t y_data_buffer,z_data_buffer;
b0ssiz 22:449f31da2d3d 558 for(int i=0; i<4; i++) {
b0ssiz 22:449f31da2d3d 559 y_offset_array[i]=Offset[i];
b0ssiz 22:449f31da2d3d 560 z_offset_array[i]=Offset[i+4];
b0ssiz 22:449f31da2d3d 561 }
b0ssiz 22:449f31da2d3d 562 y_data_buffer = Utilities::ConvertUInt8ArrayToInt32(y_offset_array);
b0ssiz 22:449f31da2d3d 563 z_data_buffer = Utilities::ConvertUInt8ArrayToInt32(z_offset_array);
b0ssiz 22:449f31da2d3d 564 memory.write(ADDRESS_OFFSET,y_data_buffer);
b0ssiz 31:d6fa5e8e26b3 565 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 566 memory.write(ADDRESS_OFFSET+4,z_data_buffer);
b0ssiz 31:d6fa5e8e26b3 567 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 568
b0ssiz 30:3f8e86fa1413 569 } else if(command[1]==MAG_DATA) {
b0ssiz 22:449f31da2d3d 570 uint8_t x_max_array[4];
b0ssiz 22:449f31da2d3d 571 uint8_t x_min_array[4];
b0ssiz 22:449f31da2d3d 572 uint8_t y_max_array[4];
b0ssiz 22:449f31da2d3d 573 uint8_t y_min_array[4];
b0ssiz 22:449f31da2d3d 574 uint8_t z_max_array[4];
b0ssiz 22:449f31da2d3d 575 uint8_t z_min_array[4];
b0ssiz 22:449f31da2d3d 576 int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer;
b0ssiz 22:449f31da2d3d 577 for(int i=0; i<4; i++) {
b0ssiz 22:449f31da2d3d 578 x_max_array[i]=Mag[i];
b0ssiz 22:449f31da2d3d 579 x_min_array[i]=Mag[i+4];
b0ssiz 22:449f31da2d3d 580 y_max_array[i]=Mag[i+8];
b0ssiz 22:449f31da2d3d 581 y_min_array[i]=Mag[i+12];
b0ssiz 22:449f31da2d3d 582 z_max_array[i]=Mag[i+16];
b0ssiz 22:449f31da2d3d 583 z_min_array[i]=Mag[i+20];
b0ssiz 22:449f31da2d3d 584 }
b0ssiz 22:449f31da2d3d 585 x_max_buffer = Utilities::ConvertUInt8ArrayToInt32(x_max_array);
b0ssiz 22:449f31da2d3d 586 x_min_buffer = Utilities::ConvertUInt8ArrayToInt32(x_min_array);
b0ssiz 22:449f31da2d3d 587 y_max_buffer = Utilities::ConvertUInt8ArrayToInt32(y_max_array);
b0ssiz 22:449f31da2d3d 588 y_min_buffer = Utilities::ConvertUInt8ArrayToInt32(y_min_array);
b0ssiz 22:449f31da2d3d 589 z_max_buffer = Utilities::ConvertUInt8ArrayToInt32(z_max_array);
b0ssiz 22:449f31da2d3d 590 z_min_buffer = Utilities::ConvertUInt8ArrayToInt32(z_min_array);
b0ssiz 22:449f31da2d3d 591 memory.write(ADDRESS_MAG_DATA,x_max_buffer);
b0ssiz 31:d6fa5e8e26b3 592 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 593 memory.write(ADDRESS_MAG_DATA+4,x_min_buffer);
b0ssiz 31:d6fa5e8e26b3 594 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 595 memory.write(ADDRESS_MAG_DATA+8,y_max_buffer);
b0ssiz 31:d6fa5e8e26b3 596 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 597 memory.write(ADDRESS_MAG_DATA+12,y_min_buffer);
b0ssiz 31:d6fa5e8e26b3 598 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 599 memory.write(ADDRESS_MAG_DATA+16,z_max_buffer);
b0ssiz 31:d6fa5e8e26b3 600 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 601 memory.write(ADDRESS_MAG_DATA+20,z_min_buffer);
b0ssiz 31:d6fa5e8e26b3 602 wait_ms(EEPROM_DELAY);
b0ssiz 28:b3509fd32b00 603
b0ssiz 22:449f31da2d3d 604 }
b0ssiz 22:449f31da2d3d 605
soulx 27:718fc94e40ad 606 }
soulx 27:718fc94e40ad 607 // else {
b0ssiz 30:3f8e86fa1413 608 if (command[1]==ID) {
soulx 27:718fc94e40ad 609 memory.write(ADDRESS_ID,id);
b0ssiz 31:d6fa5e8e26b3 610 wait_ms(EEPROM_DELAY);
soulx 18:face01c94152 611
b0ssiz 30:3f8e86fa1413 612 } else if(command[1]==UP_MARGIN) {
soulx 34:0cf04acfe422 613 memory.write(ADDRESS_UP_MARGIN,UpMargin);
b0ssiz 31:d6fa5e8e26b3 614 wait_ms(EEPROM_DELAY);
b0ssiz 28:b3509fd32b00 615 //printf("save OK!!\n\r");
b0ssiz 22:449f31da2d3d 616
b0ssiz 30:3f8e86fa1413 617 } else if (command[1]==LOW_MARGIN) {
soulx 34:0cf04acfe422 618 memory.write(ADDRESS_LOW_MARGIN,LowMargin);
b0ssiz 31:d6fa5e8e26b3 619 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 620
b0ssiz 30:3f8e86fa1413 621 } else if (command[1]==PID_UPPER_MOTOR) {
soulx 27:718fc94e40ad 622 memory.write(ADDRESS_UPPER_KP,U_Kc);
b0ssiz 30:3f8e86fa1413 623 //printf("U_Write : %f\r\n",U_Kc);
b0ssiz 31:d6fa5e8e26b3 624 wait_ms(EEPROM_DELAY);
b0ssiz 30:3f8e86fa1413 625 memory.write(ADDRESS_UPPER_KI,U_Ki_gain);
b0ssiz 30:3f8e86fa1413 626 //printf("U_Write : %f\r\n",U_Ki_gain);
b0ssiz 31:d6fa5e8e26b3 627 wait_ms(EEPROM_DELAY);
b0ssiz 30:3f8e86fa1413 628 memory.write(ADDRESS_UPPER_KD,U_Kd_gain);
b0ssiz 30:3f8e86fa1413 629 //printf("U_Write : %f\r\n",U_Kd_gain);
b0ssiz 31:d6fa5e8e26b3 630 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 631
b0ssiz 30:3f8e86fa1413 632 } else if (command[1]==PID_LOWER_MOTOR) {
soulx 27:718fc94e40ad 633 memory.write(ADDRESS_LOWER_KP,L_Kc);
b0ssiz 30:3f8e86fa1413 634 //printf("L_Write : %f\r\n",L_Kc);
b0ssiz 31:d6fa5e8e26b3 635 wait_ms(EEPROM_DELAY);
b0ssiz 30:3f8e86fa1413 636 memory.write(ADDRESS_LOWER_KI,L_Ki_gain);
b0ssiz 30:3f8e86fa1413 637 //printf("L_Write : %f\r\n",L_Ki_gain);
b0ssiz 31:d6fa5e8e26b3 638 wait_ms(EEPROM_DELAY);
b0ssiz 30:3f8e86fa1413 639 memory.write(ADDRESS_LOWER_KD,L_Kd_gain);
b0ssiz 30:3f8e86fa1413 640 //printf("L_Write : %f\r\n",L_Kd_gain);
b0ssiz 31:d6fa5e8e26b3 641 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 642
b0ssiz 30:3f8e86fa1413 643 } else if (command[1]==ANGLE_RANGE_UP) {
soulx 27:718fc94e40ad 644 uint8_t max_array[4];
soulx 27:718fc94e40ad 645 uint8_t min_array[4];
soulx 27:718fc94e40ad 646 int32_t max_data_buffer,min_data_buffer;
soulx 27:718fc94e40ad 647 for(int i=0; i<4; i++) {
soulx 27:718fc94e40ad 648 max_array[i]=Angle_Range_Up[i];
soulx 27:718fc94e40ad 649 min_array[i]=Angle_Range_Up[i+4];
soulx 27:718fc94e40ad 650 }
soulx 27:718fc94e40ad 651 max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array);
soulx 27:718fc94e40ad 652 min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array);
soulx 27:718fc94e40ad 653 memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer);
b0ssiz 31:d6fa5e8e26b3 654 wait_ms(EEPROM_DELAY);
soulx 27:718fc94e40ad 655 memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
b0ssiz 31:d6fa5e8e26b3 656 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 657
b0ssiz 30:3f8e86fa1413 658 } else if (command[1]==ANGLE_RANGE_LOW) {
soulx 27:718fc94e40ad 659 uint8_t max_array[4];
soulx 27:718fc94e40ad 660 uint8_t min_array[4];
soulx 27:718fc94e40ad 661 int32_t max_data_buffer,min_data_buffer;
soulx 27:718fc94e40ad 662 for(int i=0; i<4; i++) {
soulx 27:718fc94e40ad 663 max_array[i]=Angle_Range_Low[i];
soulx 27:718fc94e40ad 664 min_array[i]=Angle_Range_Low[i+4];
soulx 27:718fc94e40ad 665 }
soulx 27:718fc94e40ad 666 max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array);
soulx 27:718fc94e40ad 667 min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array);
soulx 27:718fc94e40ad 668 memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer);
b0ssiz 31:d6fa5e8e26b3 669 wait_ms(EEPROM_DELAY);
soulx 27:718fc94e40ad 670 memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
b0ssiz 31:d6fa5e8e26b3 671 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 672
b0ssiz 30:3f8e86fa1413 673 } else if (command[1]==UP_LINK_LENGTH) {
soulx 27:718fc94e40ad 674 int32_t data_buff;
soulx 27:718fc94e40ad 675 data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength);
soulx 27:718fc94e40ad 676 memory.write(ADDRESS_UP_LINK_LENGTH,data_buff);
b0ssiz 31:d6fa5e8e26b3 677 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 678
b0ssiz 30:3f8e86fa1413 679 } else if (command[1]==LOW_LINK_LENGTH) {
soulx 27:718fc94e40ad 680 int32_t data_buff;
soulx 27:718fc94e40ad 681 data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength);
soulx 27:718fc94e40ad 682 memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff);
b0ssiz 31:d6fa5e8e26b3 683 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 684
b0ssiz 30:3f8e86fa1413 685 } else if (command[1]==WHEELPOS) {
soulx 27:718fc94e40ad 686 int32_t data_buff;
soulx 27:718fc94e40ad 687 data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos);
soulx 27:718fc94e40ad 688 memory.write(ADDRESS_WHEELPOS,data_buff);
b0ssiz 31:d6fa5e8e26b3 689 wait_ms(EEPROM_DELAY);
b0ssiz 22:449f31da2d3d 690 }
soulx 27:718fc94e40ad 691 break;
ParinyaT 13:49cb002ad8fd 692 }
b0ssiz 28:b3509fd32b00 693 break;
b0ssiz 28:b3509fd32b00 694 }
b0ssiz 28:b3509fd32b00 695 break;
b0ssiz 28:b3509fd32b00 696 }
b0ssiz 28:b3509fd32b00 697 case READ_DATA: {
b0ssiz 30:3f8e86fa1413 698 switch (command[0]) {
b0ssiz 28:b3509fd32b00 699 case MOTOR_UPPER_ANG: {
b0ssiz 31:d6fa5e8e26b3 700 com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
b0ssiz 28:b3509fd32b00 701 break;
b0ssiz 28:b3509fd32b00 702 }
b0ssiz 28:b3509fd32b00 703 case UP_MARGIN: {
soulx 34:0cf04acfe422 704 memory.read(ADDRESS_UP_MARGIN,UpMargin);
b0ssiz 28:b3509fd32b00 705 com.sendUpMargin(MY_ID,UpMargin);
b0ssiz 28:b3509fd32b00 706 break;
b0ssiz 28:b3509fd32b00 707 }
b0ssiz 28:b3509fd32b00 708 case LOW_MARGIN: {
soulx 34:0cf04acfe422 709 memory.read(ADDRESS_LOW_MARGIN,LowMargin);
b0ssiz 28:b3509fd32b00 710 com.sendLowMargin(MY_ID,LowMargin);
b0ssiz 28:b3509fd32b00 711 break;
b0ssiz 28:b3509fd32b00 712 }
b0ssiz 28:b3509fd32b00 713 case PID_UPPER_MOTOR: {
b0ssiz 28:b3509fd32b00 714 memory.read(ADDRESS_UPPER_KP,U_Kc);
b0ssiz 30:3f8e86fa1413 715 memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
b0ssiz 30:3f8e86fa1413 716 memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
b0ssiz 30:3f8e86fa1413 717 com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki_gain,U_Kd_gain);
b0ssiz 30:3f8e86fa1413 718 /*
b0ssiz 30:3f8e86fa1413 719 printf("After read Kp : %f\r\n",U_Kc);
b0ssiz 30:3f8e86fa1413 720 printf("After read Ki : %f\r\n",U_Ki_gain);
b0ssiz 30:3f8e86fa1413 721 printf("After read Kd : %f\r\n",U_Kd_gain);
b0ssiz 30:3f8e86fa1413 722 */
b0ssiz 28:b3509fd32b00 723 break;
b0ssiz 28:b3509fd32b00 724 }
b0ssiz 28:b3509fd32b00 725 case PID_LOWER_MOTOR: {
b0ssiz 28:b3509fd32b00 726 memory.read(ADDRESS_LOWER_KP,L_Kc);
b0ssiz 30:3f8e86fa1413 727 memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
b0ssiz 30:3f8e86fa1413 728 memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
b0ssiz 30:3f8e86fa1413 729 com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki_gain,L_Kd_gain);
b0ssiz 30:3f8e86fa1413 730 /*
b0ssiz 30:3f8e86fa1413 731 printf("After read L_Kp : %f\r\n",L_Kc);
b0ssiz 30:3f8e86fa1413 732 printf("After read L_Ki : %f\r\n",L_Ki_gain);
b0ssiz 30:3f8e86fa1413 733 printf("After read L_Kd : %f\r\n",L_Kd_gain);
b0ssiz 30:3f8e86fa1413 734 */
b0ssiz 28:b3509fd32b00 735 break;
b0ssiz 28:b3509fd32b00 736 }
b0ssiz 28:b3509fd32b00 737 case HEIGHT: {
b0ssiz 28:b3509fd32b00 738 int32_t data_buff;
b0ssiz 28:b3509fd32b00 739 memory.read(ADDRESS_HEIGHT,data_buff);
b0ssiz 28:b3509fd32b00 740 Utilities::ConvertInt32ToUInt8Array(data_buff,Height);
b0ssiz 28:b3509fd32b00 741 com.sendHeight(MY_ID,Height);
b0ssiz 28:b3509fd32b00 742 break;
b0ssiz 28:b3509fd32b00 743 }
b0ssiz 28:b3509fd32b00 744 case WHEELPOS: {
b0ssiz 28:b3509fd32b00 745 int32_t data_buff;
b0ssiz 28:b3509fd32b00 746 memory.read(ADDRESS_WHEELPOS,data_buff);
b0ssiz 28:b3509fd32b00 747 Utilities::ConvertInt32ToUInt8Array(data_buff,Wheelpos);
b0ssiz 28:b3509fd32b00 748 com.sendWheelPos(MY_ID,Wheelpos);
b0ssiz 28:b3509fd32b00 749 break;
b0ssiz 28:b3509fd32b00 750 }
b0ssiz 28:b3509fd32b00 751 case MAG_DATA: {
b0ssiz 28:b3509fd32b00 752 uint8_t x_max_array[4];
b0ssiz 28:b3509fd32b00 753 uint8_t x_min_array[4];
b0ssiz 28:b3509fd32b00 754 uint8_t y_max_array[4];
b0ssiz 28:b3509fd32b00 755 uint8_t y_min_array[4];
b0ssiz 28:b3509fd32b00 756 uint8_t z_max_array[4];
b0ssiz 28:b3509fd32b00 757 uint8_t z_min_array[4];
b0ssiz 28:b3509fd32b00 758 int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer;
b0ssiz 28:b3509fd32b00 759 memory.read(ADDRESS_MAG_DATA,x_max_buffer);
b0ssiz 28:b3509fd32b00 760 memory.read(ADDRESS_MAG_DATA+4,x_min_buffer);
b0ssiz 28:b3509fd32b00 761 memory.read(ADDRESS_MAG_DATA+8,y_max_buffer);
b0ssiz 28:b3509fd32b00 762 memory.read(ADDRESS_MAG_DATA+12,y_min_buffer);
b0ssiz 28:b3509fd32b00 763 memory.read(ADDRESS_MAG_DATA+16,z_max_buffer);
b0ssiz 28:b3509fd32b00 764 memory.read(ADDRESS_MAG_DATA+20,z_min_buffer);
b0ssiz 28:b3509fd32b00 765 Utilities::ConvertInt32ToUInt8Array(x_max_buffer,x_max_array);
b0ssiz 28:b3509fd32b00 766 Utilities::ConvertInt32ToUInt8Array(x_min_buffer,x_min_array);
b0ssiz 28:b3509fd32b00 767 Utilities::ConvertInt32ToUInt8Array(y_max_buffer,y_max_array);
b0ssiz 28:b3509fd32b00 768 Utilities::ConvertInt32ToUInt8Array(y_min_buffer,y_min_array);
b0ssiz 28:b3509fd32b00 769 Utilities::ConvertInt32ToUInt8Array(z_max_buffer,z_max_array);
b0ssiz 28:b3509fd32b00 770 Utilities::ConvertInt32ToUInt8Array(z_min_buffer,z_min_array);
b0ssiz 28:b3509fd32b00 771 for(int i=0; i<4; i++) {
b0ssiz 28:b3509fd32b00 772 Mag[i]=x_max_array[i];
b0ssiz 28:b3509fd32b00 773 Mag[i+4]=x_min_array[i];
b0ssiz 28:b3509fd32b00 774 Mag[i+8]=y_max_array[i];
b0ssiz 28:b3509fd32b00 775 Mag[i+12]=y_min_array[i];
b0ssiz 28:b3509fd32b00 776 Mag[i+16]=z_max_array[i];
b0ssiz 28:b3509fd32b00 777 Mag[i+20]=z_min_array[i];
b0ssiz 28:b3509fd32b00 778 }
b0ssiz 28:b3509fd32b00 779 com.sendMagData(MY_ID,Mag);
b0ssiz 28:b3509fd32b00 780 break;
b0ssiz 28:b3509fd32b00 781 }
b0ssiz 28:b3509fd32b00 782 case OFFSET: {
b0ssiz 28:b3509fd32b00 783 uint8_t y_offset_array[4];
b0ssiz 28:b3509fd32b00 784 uint8_t z_offset_array[4];
b0ssiz 28:b3509fd32b00 785 int32_t y_data_buffer,z_data_buffer;
b0ssiz 28:b3509fd32b00 786 memory.read(ADDRESS_OFFSET,y_data_buffer);
b0ssiz 28:b3509fd32b00 787 memory.read(ADDRESS_OFFSET+4,z_data_buffer);
b0ssiz 28:b3509fd32b00 788 Utilities::ConvertInt32ToUInt8Array(y_data_buffer,y_offset_array);
b0ssiz 28:b3509fd32b00 789 Utilities::ConvertInt32ToUInt8Array(z_data_buffer,z_offset_array);
b0ssiz 28:b3509fd32b00 790 for(int i=0; i<4; i++) {
b0ssiz 28:b3509fd32b00 791 Offset[i]=y_offset_array[i];
b0ssiz 28:b3509fd32b00 792 Offset[i+4]=z_offset_array[i];
b0ssiz 28:b3509fd32b00 793 }
b0ssiz 28:b3509fd32b00 794 com.sendOffset(MY_ID,Offset);
b0ssiz 28:b3509fd32b00 795 break;
b0ssiz 28:b3509fd32b00 796 }
b0ssiz 28:b3509fd32b00 797 case BODY_WIDTH: {
b0ssiz 28:b3509fd32b00 798 int32_t data_buff;
b0ssiz 28:b3509fd32b00 799 memory.read(ADDRESS_BODY_WIDTH,data_buff);
b0ssiz 28:b3509fd32b00 800 Utilities::ConvertInt32ToUInt8Array(data_buff,Body_width);
b0ssiz 28:b3509fd32b00 801 com.sendBodyWidth(MY_ID,Body_width);
b0ssiz 28:b3509fd32b00 802 break;
b0ssiz 28:b3509fd32b00 803 }
b0ssiz 28:b3509fd32b00 804 case ANGLE_RANGE_UP: {
b0ssiz 28:b3509fd32b00 805 uint8_t max_array[4];
b0ssiz 28:b3509fd32b00 806 uint8_t min_array[4];
b0ssiz 28:b3509fd32b00 807 int32_t max_data_buffer,min_data_buffer;
b0ssiz 28:b3509fd32b00 808 memory.read(ADDRESS_ANGLE_RANGE_UP,max_data_buffer);
b0ssiz 28:b3509fd32b00 809 memory.read(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
b0ssiz 28:b3509fd32b00 810 Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array);
b0ssiz 28:b3509fd32b00 811 Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array);
b0ssiz 28:b3509fd32b00 812 for(int i=0; i<4; i++) {
b0ssiz 28:b3509fd32b00 813 Angle_Range_Up[i]=max_array[i];
b0ssiz 28:b3509fd32b00 814 Angle_Range_Up[i+4]=min_array[i];
b0ssiz 28:b3509fd32b00 815 }
b0ssiz 28:b3509fd32b00 816 com.sendUpAngleRange(MY_ID,Angle_Range_Up);
b0ssiz 28:b3509fd32b00 817 break;
b0ssiz 28:b3509fd32b00 818 }
b0ssiz 28:b3509fd32b00 819 case ANGLE_RANGE_LOW: {
b0ssiz 28:b3509fd32b00 820 uint8_t max_array[4];
b0ssiz 28:b3509fd32b00 821 uint8_t min_array[4];
b0ssiz 28:b3509fd32b00 822 int32_t max_data_buffer,min_data_buffer;
b0ssiz 28:b3509fd32b00 823 memory.read(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer);
b0ssiz 28:b3509fd32b00 824 memory.read(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
b0ssiz 28:b3509fd32b00 825 Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array);
b0ssiz 28:b3509fd32b00 826 Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array);
b0ssiz 28:b3509fd32b00 827 for(int i=0; i<4; i++) {
b0ssiz 28:b3509fd32b00 828 Angle_Range_Low[i]=max_array[i];
b0ssiz 28:b3509fd32b00 829 Angle_Range_Low[i+4]=min_array[i];
b0ssiz 28:b3509fd32b00 830 }
b0ssiz 28:b3509fd32b00 831 com.sendLowAngleRange(MY_ID,Angle_Range_Low);
b0ssiz 28:b3509fd32b00 832 break;
b0ssiz 28:b3509fd32b00 833 }
b0ssiz 28:b3509fd32b00 834 case UP_LINK_LENGTH: {
b0ssiz 28:b3509fd32b00 835 int32_t data_buff;
b0ssiz 28:b3509fd32b00 836 memory.read(ADDRESS_UP_LINK_LENGTH,data_buff);
b0ssiz 28:b3509fd32b00 837 Utilities::ConvertInt32ToUInt8Array(data_buff,UpLinkLength);
b0ssiz 28:b3509fd32b00 838 com.sendUpLinkLength(MY_ID,UpLinkLength);
b0ssiz 28:b3509fd32b00 839 break;
b0ssiz 28:b3509fd32b00 840 }
b0ssiz 28:b3509fd32b00 841 case LOW_LINK_LENGTH: {
b0ssiz 28:b3509fd32b00 842 int32_t data_buff;
b0ssiz 28:b3509fd32b00 843 memory.read(ADDRESS_LOW_LINK_LENGTH,data_buff);
b0ssiz 28:b3509fd32b00 844 Utilities::ConvertInt32ToUInt8Array(data_buff,LowLinkLength);
b0ssiz 28:b3509fd32b00 845 com.sendLowLinkLength(MY_ID,LowLinkLength);
b0ssiz 28:b3509fd32b00 846 break;
b0ssiz 28:b3509fd32b00 847 }
b0ssiz 28:b3509fd32b00 848 break;
ParinyaT 13:49cb002ad8fd 849 }
ParinyaT 13:49cb002ad8fd 850 }
b0ssiz 10:3b3d6bc88677 851 }
b0ssiz 14:28e24fcc5a01 852 }
b0ssiz 10:3b3d6bc88677 853 }
b0ssiz 17:4c96838e579f 854
b0ssiz 28:b3509fd32b00 855
ParinyaT 11:3dd92d1d542c 856 /******************************************************/
ParinyaT 13:49cb002ad8fd 857 void Start_Up()
ParinyaT 11:3dd92d1d542c 858 {
ParinyaT 11:3dd92d1d542c 859 // wait for reciever
b0ssiz 22:449f31da2d3d 860 memory.read(ADDRESS_ID,MY_ID);
b0ssiz 22:449f31da2d3d 861 memory.read(ADDRESS_UPPER_KP,U_Kc);
b0ssiz 22:449f31da2d3d 862 memory.read(ADDRESS_UPPER_KI,U_Ti);
b0ssiz 22:449f31da2d3d 863 memory.read(ADDRESS_UPPER_KD,U_Td);
b0ssiz 22:449f31da2d3d 864 memory.read(ADDRESS_LOWER_KP,L_Kc);
b0ssiz 22:449f31da2d3d 865 memory.read(ADDRESS_LOWER_KI,L_Ti);
b0ssiz 22:449f31da2d3d 866 memory.read(ADDRESS_LOWER_KD,L_Td);
b0ssiz 14:28e24fcc5a01 867
ParinyaT 11:3dd92d1d542c 868 }
soulx 34:0cf04acfe422 869 /*****************************************************/