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@28:b3509fd32b00, 2016-01-26 (annotated)

Committer:
b0ssiz
Date:
Tue Jan 26 20:47:53 2016 +0000
Revision:
28:b3509fd32b00
Parent:
27:718fc94e40ad
Child:
29:5db0a9261161
Debug EEPROM and Add time delay

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