Secret_Betago / Mbed 2 deprecated BEAR_Motion_Editing4Betago

test

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of BEAR_Motion by BE@R lab

main.cpp@33:65cfa4b72195, 2016-02-03 (annotated)

Committer:
ParinyaT
Date:
Wed Feb 03 14:23:47 2016 +0000
Revision:
33:65cfa4b72195
Parent:
32:1f81f3e83889
Child:
36:1561b6d61095
Fixed bug Encoder left and right;

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