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@22:449f31da2d3d, 2016-01-26 (annotated)

Committer:
b0ssiz
Date:
Tue Jan 26 18:16:11 2016 +0000
Revision:
22:449f31da2d3d
Parent:
19:3da61e637b2d
Child:
23:999dd41eef14
Update; - function save to EEPROM; - Read from EEPROM; - Update Motion_EEPROM_Address.h

Who changed what in which revision?

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