BE@R lab / Mbed 2 deprecated BEAR_Motion

โปรแกรมของบอร์ด Motion

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of DogPID by Digital B14

main.cpp.orig@23:999dd41eef14, 2016-01-26 (annotated)

Committer:
b0ssiz
Date:
Tue Jan 26 18:34:51 2016 +0000
Revision:
23:999dd41eef14
Merge with P'Gear

Who changed what in which revision?

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