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@37:8719223998d8, 2016-06-07 (annotated)

Committer:
icyzkungz
Date:
Tue Jun 07 07:00:11 2016 +0000
Revision:
37:8719223998d8
Parent:
36:1561b6d61095 
update protocol.h

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