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