BE@R lab / Mbed 2 deprecated BEAR_Motion

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

Dependencies:   BEAR_Reciever Motor eeprom iSerial mbed

Fork of DogPID by Digital B14

main.cpp@24:d4942687f026, 2016-01-26 (annotated)

Committer:
soulx
Date:
Tue Jan 26 18:32:29 2016 +0000
Revision:
24:d4942687f026
Parent:
21:1c04c4afe3b7
Child:
25:c66a0d029e1f
test data communication

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"
ParinyaT 0:451c27e4d55e 9
ParinyaT 0:451c27e4d55e 10 //*****************************************************/
ParinyaT 1:84167ca00307 11 //--PID parameter--
ParinyaT 1:84167ca00307 12 //-Upper-//
b0ssiz 10:3b3d6bc88677 13 float U_Kc;
b0ssiz 10:3b3d6bc88677 14 float U_Ti;
b0ssiz 10:3b3d6bc88677 15 float U_Td;
ParinyaT 16:c0a1daeb9fa5 16 float U_Ki=U_Kc*U_Ti;
ParinyaT 16:c0a1daeb9fa5 17 float U_Kd=U_Kc*U_Td;
ParinyaT 1:84167ca00307 18 //-lower-//
b0ssiz 10:3b3d6bc88677 19 float L_Kc;
b0ssiz 10:3b3d6bc88677 20 float L_Ti;
b0ssiz 10:3b3d6bc88677 21 float L_Td;
ParinyaT 16:c0a1daeb9fa5 22 float L_Ki=L_Kc*L_Ti;
ParinyaT 16:c0a1daeb9fa5 23 float L_Kd=L_Kc*L_Td;
ParinyaT 0:451c27e4d55e 24 //*****************************************************/
ParinyaT 0:451c27e4d55e 25 // Global //
ParinyaT 11:3dd92d1d542c 26 Ticker Recieve;
ParinyaT 1:84167ca00307 27 //-- Communication --
ParinyaT 0:451c27e4d55e 28 Serial PC(D1,D0);
b0ssiz 17:4c96838e579f 29 Bear_Receiver com(Tx,Rx,1000000);
b0ssiz 14:28e24fcc5a01 30 int16_t MY_ID = 0x01;
ParinyaT 11:3dd92d1d542c 31 //-- Memorry --
ParinyaT 11:3dd92d1d542c 32 EEPROM memory(PB_4,PA_8,0);
b0ssiz 14:28e24fcc5a01 33 uint8_t UpMargin[4];
b0ssiz 14:28e24fcc5a01 34 uint8_t LowMargin[4];
ParinyaT 13:49cb002ad8fd 35 uint8_t Height[4];
ParinyaT 13:49cb002ad8fd 36 uint8_t Wheelpos[4];
ParinyaT 13:49cb002ad8fd 37 uint8_t Mag[24];
ParinyaT 16:c0a1daeb9fa5 38 uint8_t Offset[8];
b0ssiz 14:28e24fcc5a01 39 uint8_t Body_width[4];
ParinyaT 16:c0a1daeb9fa5 40 uint8_t Angle_Range_Up[8];
ParinyaT 16:c0a1daeb9fa5 41 uint8_t Angle_Range_Low[8];
b0ssiz 6:98871feebea0 42 //-- encoder --
ParinyaT 13:49cb002ad8fd 43 float up_angle,low_angle;
ParinyaT 1:84167ca00307 44 int Upper_Position;
ParinyaT 1:84167ca00307 45 int Lower_Position;
ParinyaT 0:451c27e4d55e 46 int data;
ParinyaT 1:84167ca00307 47 SPI ENC(Emosi, Emiso, Esck);
ParinyaT 1:84167ca00307 48 DigitalOut EncA(EncoderA);
ParinyaT 1:84167ca00307 49 DigitalOut EncB(EncoderB);
ParinyaT 0:451c27e4d55e 50 //-- Motor --
ParinyaT 1:84167ca00307 51 int dir;
ParinyaT 1:84167ca00307 52 Motor Upper(PWM_LU,A_LU,B_LU);
ParinyaT 1:84167ca00307 53 Motor Lower(PWM_LL,A_LL,B_LL);
ParinyaT 0:451c27e4d55e 54 //-- PID --
ParinyaT 1:84167ca00307 55 int Upper_SetPoint;
ParinyaT 1:84167ca00307 56 int Lower_SetPoint;
ParinyaT 1:84167ca00307 57 PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
ParinyaT 1:84167ca00307 58 PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);
ParinyaT 0:451c27e4d55e 59 //*****************************************************/
soulx 20:7e6d56655336 60
soulx 20:7e6d56655336 61 DigitalOut myled(LED1);
soulx 20:7e6d56655336 62
soulx 20:7e6d56655336 63
ParinyaT 1:84167ca00307 64 void Read_Encoder(PinName Encoder)
ParinyaT 0:451c27e4d55e 65 {
soulx 7:bf239d051e8c 66 ENC.format(8,0);
soulx 7:bf239d051e8c 67 ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
b0ssiz 6:98871feebea0 68
soulx 7:bf239d051e8c 69 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 70 EncA = 0;
soulx 7:bf239d051e8c 71 } else {
soulx 7:bf239d051e8c 72 EncB = 0;
soulx 7:bf239d051e8c 73 }
soulx 7:bf239d051e8c 74 ENC.write(0x41);
soulx 7:bf239d051e8c 75 ENC.write(0x09);
soulx 7:bf239d051e8c 76 data = ENC.write(0x00);
soulx 7:bf239d051e8c 77 if(Encoder == EncoderA) {
soulx 7:bf239d051e8c 78 EncA = 1;
soulx 7:bf239d051e8c 79 } else {
soulx 7:bf239d051e8c 80 EncB = 1;
soulx 7:bf239d051e8c 81 }
b0ssiz 6:98871feebea0 82
ParinyaT 0:451c27e4d55e 83 }
ParinyaT 0:451c27e4d55e 84 //*****************************************************/
ParinyaT 1:84167ca00307 85 int Get_EnValue(int Val)
ParinyaT 0:451c27e4d55e 86 {
ParinyaT 0:451c27e4d55e 87 int i = 0;
ParinyaT 0:451c27e4d55e 88 static unsigned char codes[] = {
b0ssiz 6:98871feebea0 89 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
b0ssiz 6:98871feebea0 90 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
b0ssiz 6:98871feebea0 91 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
b0ssiz 6:98871feebea0 92 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
b0ssiz 6:98871feebea0 93 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
b0ssiz 6:98871feebea0 94 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
b0ssiz 6:98871feebea0 95 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
b0ssiz 6:98871feebea0 96 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
b0ssiz 6:98871feebea0 97 };
b0ssiz 6:98871feebea0 98
b0ssiz 6:98871feebea0 99 while(Val != codes[i]) {
ParinyaT 1:84167ca00307 100 i++;
ParinyaT 0:451c27e4d55e 101 }
b0ssiz 6:98871feebea0 102
ParinyaT 1:84167ca00307 103 return i;
b0ssiz 6:98871feebea0 104
ParinyaT 0:451c27e4d55e 105 }
ParinyaT 0:451c27e4d55e 106 //*****************************************************/
ParinyaT 1:84167ca00307 107 void SET_UpperPID()
ParinyaT 1:84167ca00307 108 {
ParinyaT 1:84167ca00307 109 Upper.period(0.001);
ParinyaT 1:84167ca00307 110 Up_PID.setMode(0);
ParinyaT 1:84167ca00307 111 Up_PID.setInputLimits(0,127);
ParinyaT 1:84167ca00307 112 Up_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 113 }
ParinyaT 1:84167ca00307 114 //******************************************************/
ParinyaT 1:84167ca00307 115 void SET_LowerPID()
ParinyaT 1:84167ca00307 116 {
ParinyaT 1:84167ca00307 117 Lower.period(0.001);
ParinyaT 1:84167ca00307 118 Low_PID.setMode(0);
b0ssiz 10:3b3d6bc88677 119 Low_PID.setInputLimits(0,127); // set range
ParinyaT 1:84167ca00307 120 Low_PID.setOutputLimits(0,1);
ParinyaT 1:84167ca00307 121 }
b0ssiz 6:98871feebea0 122 //******************************************************/
ParinyaT 1:84167ca00307 123 void Move_Upper()
ParinyaT 1:84167ca00307 124 {
ParinyaT 1:84167ca00307 125 Read_Encoder(EncoderA);
ParinyaT 1:84167ca00307 126 Upper_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 127
ParinyaT 1:84167ca00307 128 Up_PID.setProcessValue(Upper_Position);
b0ssiz 6:98871feebea0 129
soulx 7:bf239d051e8c 130 if(Upper_Position - Upper_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 131 dir = 1;
b0ssiz 6:98871feebea0 132 }
soulx 7:bf239d051e8c 133 if(Upper_Position - Upper_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 134 dir = -1;
b0ssiz 6:98871feebea0 135 }
soulx 7:bf239d051e8c 136 Upper.speed(Up_PID.compute() * dir);
ParinyaT 1:84167ca00307 137 }
ParinyaT 1:84167ca00307 138 //******************************************************/
ParinyaT 1:84167ca00307 139 void Move_Lower()
ParinyaT 1:84167ca00307 140 {
ParinyaT 1:84167ca00307 141 Read_Encoder(EncoderB);
ParinyaT 1:84167ca00307 142 Lower_Position = Get_EnValue(data);
ParinyaT 1:84167ca00307 143
ParinyaT 1:84167ca00307 144 Low_PID.setProcessValue(Lower_Position);
b0ssiz 6:98871feebea0 145
b0ssiz 6:98871feebea0 146 if(Lower_Position - Lower_SetPoint > 0 ) {
ParinyaT 1:84167ca00307 147 dir = 1;
b0ssiz 6:98871feebea0 148 }
b0ssiz 6:98871feebea0 149 if(Lower_Position - Lower_SetPoint < 0 ) {
ParinyaT 1:84167ca00307 150 dir = -1;
b0ssiz 6:98871feebea0 151 }
soulx 7:bf239d051e8c 152 Lower.speed(Low_PID.compute() * dir);
b0ssiz 6:98871feebea0 153 }
ParinyaT 1:84167ca00307 154 //******************************************************/
ParinyaT 1:84167ca00307 155
b0ssiz 15:6ebca0a1aaca 156 void CmdCheck(int16_t id,uint8_t *cmd,uint8_t ins)
b0ssiz 10:3b3d6bc88677 157 {
b0ssiz 14:28e24fcc5a01 158 if(id==MY_ID) {
b0ssiz 14:28e24fcc5a01 159 switch (ins) {
b0ssiz 14:28e24fcc5a01 160 case PING: {
soulx 18:face01c94152 161 break;
b0ssiz 14:28e24fcc5a01 162 }
b0ssiz 14:28e24fcc5a01 163 case WRITE_DATA: {
b0ssiz 14:28e24fcc5a01 164 switch (cmd[0]) {
b0ssiz 14:28e24fcc5a01 165 case ID: {
soulx 18:face01c94152 166 ///
soulx 18:face01c94152 167 MY_ID = (int16_t)cmd[1];
soulx 18:face01c94152 168 break;
b0ssiz 14:28e24fcc5a01 169 }
b0ssiz 14:28e24fcc5a01 170 case MOTOR_UPPER_ANG: {
b0ssiz 14:28e24fcc5a01 171 uint8_t IntUpAngle[2],FloatUpAngle[2];
b0ssiz 14:28e24fcc5a01 172 uint8_t IntLowAngle[2],FloatLowAngle[2];
b0ssiz 14:28e24fcc5a01 173 float int_buffer,float_buffer;
b0ssiz 14:28e24fcc5a01 174
b0ssiz 14:28e24fcc5a01 175 IntUpAngle[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 176 IntUpAngle[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 177 FloatUpAngle[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 178 FloatUpAngle[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 179 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
b0ssiz 14:28e24fcc5a01 180 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER;
b0ssiz 14:28e24fcc5a01 181 up_angle=int_buffer+float_buffer;
b0ssiz 14:28e24fcc5a01 182
b0ssiz 14:28e24fcc5a01 183 IntLowAngle[0]=cmd[5];
b0ssiz 14:28e24fcc5a01 184 IntLowAngle[1]=cmd[6];
b0ssiz 14:28e24fcc5a01 185 FloatLowAngle[0]=cmd[7];
b0ssiz 14:28e24fcc5a01 186 FloatLowAngle[1]=cmd[8];
b0ssiz 14:28e24fcc5a01 187 int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
b0ssiz 14:28e24fcc5a01 188 float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER;
soulx 18:face01c94152 189 low_angle=int_buffer+float_buffer;
soulx 18:face01c94152 190 break;
b0ssiz 14:28e24fcc5a01 191 }
b0ssiz 14:28e24fcc5a01 192 case UP_MARGIN: {
b0ssiz 14:28e24fcc5a01 193 int i;
b0ssiz 14:28e24fcc5a01 194 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 195 UpMargin[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 196 }
soulx 18:face01c94152 197 break;
b0ssiz 14:28e24fcc5a01 198 }
b0ssiz 14:28e24fcc5a01 199 case LOW_MARGIN: {
b0ssiz 14:28e24fcc5a01 200 int i;
b0ssiz 14:28e24fcc5a01 201 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 202 LowMargin[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 203 }
soulx 18:face01c94152 204 break;
b0ssiz 14:28e24fcc5a01 205 }
b0ssiz 14:28e24fcc5a01 206 case KP_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 207 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 208 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 209 float Int,Float;
b0ssiz 14:28e24fcc5a01 210 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 211 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 212 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 213 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 214 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 215 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 216 U_Kc=Int+Float;
soulx 18:face01c94152 217 break;
b0ssiz 14:28e24fcc5a01 218 }
b0ssiz 14:28e24fcc5a01 219 case KI_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 220 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 221 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 222 float Int,Float,KI;
b0ssiz 14:28e24fcc5a01 223 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 224 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 225 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 226 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 227 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 228 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 229 KI=Int+Float;
b0ssiz 14:28e24fcc5a01 230 U_Ti=KI/U_Kc;
soulx 18:face01c94152 231 break;
b0ssiz 14:28e24fcc5a01 232 }
b0ssiz 14:28e24fcc5a01 233 case KD_UPPER_MOTOR: {
b0ssiz 14:28e24fcc5a01 234 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 235 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 236 float Int,Float,KD;
b0ssiz 14:28e24fcc5a01 237 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 238 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 239 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 240 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 241 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 242 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 243 KD=Int+Float;
b0ssiz 14:28e24fcc5a01 244 U_Td=KD/U_Kc;
soulx 18:face01c94152 245 break;
b0ssiz 14:28e24fcc5a01 246 }
b0ssiz 14:28e24fcc5a01 247 case KP_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 248 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 249 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 250 float Int,Float;
b0ssiz 14:28e24fcc5a01 251 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 252 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 253 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 254 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 255 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 256 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 257 L_Kc=Int+Float;
soulx 18:face01c94152 258 break;
b0ssiz 14:28e24fcc5a01 259 }
b0ssiz 14:28e24fcc5a01 260 case KI_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 261 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 262 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 263 float Int,Float,KI;
b0ssiz 14:28e24fcc5a01 264 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 265 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 266 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 267 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 268 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 269 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 270 KI=Int+Float;
b0ssiz 14:28e24fcc5a01 271 L_Ti=KI/L_Kc;
soulx 18:face01c94152 272 break;
b0ssiz 14:28e24fcc5a01 273 }
b0ssiz 14:28e24fcc5a01 274 case KD_LOWER_MOTOR: {
b0ssiz 14:28e24fcc5a01 275 uint8_t int_buffer[2];
b0ssiz 14:28e24fcc5a01 276 uint8_t float_buffer[2];
b0ssiz 14:28e24fcc5a01 277 float Int,Float,KD;
b0ssiz 14:28e24fcc5a01 278 int_buffer[0]=cmd[1];
b0ssiz 14:28e24fcc5a01 279 int_buffer[1]=cmd[2];
b0ssiz 14:28e24fcc5a01 280 float_buffer[0]=cmd[3];
b0ssiz 14:28e24fcc5a01 281 float_buffer[1]=cmd[4];
b0ssiz 14:28e24fcc5a01 282 Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
b0ssiz 14:28e24fcc5a01 283 Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
b0ssiz 14:28e24fcc5a01 284 KD=Int+Float;
b0ssiz 14:28e24fcc5a01 285 L_Td=KD/L_Kc;
soulx 18:face01c94152 286 break;
b0ssiz 14:28e24fcc5a01 287 }
b0ssiz 14:28e24fcc5a01 288 case HEIGHT: {
b0ssiz 14:28e24fcc5a01 289 int i;
b0ssiz 14:28e24fcc5a01 290 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 291 Height[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 292 }
soulx 18:face01c94152 293 break;
b0ssiz 14:28e24fcc5a01 294 }
b0ssiz 14:28e24fcc5a01 295 case WHEELPOS: {
b0ssiz 14:28e24fcc5a01 296 int i;
b0ssiz 14:28e24fcc5a01 297 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 298 Wheelpos[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 299 }
soulx 18:face01c94152 300 break;
b0ssiz 14:28e24fcc5a01 301 }
b0ssiz 14:28e24fcc5a01 302 case MAG_DATA: {
b0ssiz 14:28e24fcc5a01 303 int i;
b0ssiz 14:28e24fcc5a01 304 for(i=0; i<24; i++) {
b0ssiz 14:28e24fcc5a01 305 Mag[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 306 }
soulx 18:face01c94152 307 break;
b0ssiz 14:28e24fcc5a01 308 }
b0ssiz 14:28e24fcc5a01 309 case OFFSET: {
b0ssiz 14:28e24fcc5a01 310 int i;
b0ssiz 14:28e24fcc5a01 311 for(i=0; i<8; i++) {
b0ssiz 14:28e24fcc5a01 312 Offset[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 313 }
soulx 18:face01c94152 314 break;
b0ssiz 14:28e24fcc5a01 315 }
b0ssiz 14:28e24fcc5a01 316 case BODY_WIDTH: {
b0ssiz 14:28e24fcc5a01 317 int i;
b0ssiz 14:28e24fcc5a01 318 for(i=0; i<4; i++) {
b0ssiz 14:28e24fcc5a01 319 Body_width[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 320 }
soulx 18:face01c94152 321 break;
b0ssiz 14:28e24fcc5a01 322 }
b0ssiz 14:28e24fcc5a01 323 case ANGLE_RANGE_UP: {
b0ssiz 14:28e24fcc5a01 324 int i;
ParinyaT 16:c0a1daeb9fa5 325 for(i=0; i<8; i++) {
b0ssiz 14:28e24fcc5a01 326 Angle_Range_Up[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 327 }
soulx 18:face01c94152 328 break;
b0ssiz 14:28e24fcc5a01 329 }
b0ssiz 14:28e24fcc5a01 330 case ANGLE_RANGE_LOW: {
b0ssiz 14:28e24fcc5a01 331 int i;
ParinyaT 16:c0a1daeb9fa5 332 for(i=0; i<8; i++) {
b0ssiz 14:28e24fcc5a01 333 Angle_Range_Low[0+i]=cmd[1+i];
b0ssiz 14:28e24fcc5a01 334 }
soulx 18:face01c94152 335 break;
ParinyaT 13:49cb002ad8fd 336 }
ParinyaT 16:c0a1daeb9fa5 337 // unfinish yet!!!!!!!!!!!!!!!!!
ParinyaT 16:c0a1daeb9fa5 338 case SAVE_EEPROM_DATA: {
soulx 18:face01c94152 339 if (cmd[1]==ID) {
soulx 18:face01c94152 340
ParinyaT 16:c0a1daeb9fa5 341 }
soulx 18:face01c94152 342 break;
ParinyaT 16:c0a1daeb9fa5 343 }
soulx 18:face01c94152 344
soulx 19:3da61e637b2d 345 // break;
ParinyaT 12:6b3b997dd7c2 346 }
b0ssiz 14:28e24fcc5a01 347 case READ_DATA: {
b0ssiz 14:28e24fcc5a01 348 switch (cmd[0]) {
b0ssiz 14:28e24fcc5a01 349 case MOTOR_UPPER_ANG: {
soulx 18:face01c94152 350 uint8_t status;
soulx 18:face01c94152 351 status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
soulx 24:d4942687f026 352 //printf("status = 0x%02x\n\r",status);
soulx 18:face01c94152 353 break;
b0ssiz 14:28e24fcc5a01 354 }
b0ssiz 14:28e24fcc5a01 355 case UP_MARGIN: {
soulx 18:face01c94152 356 com.sendUpMargin(MY_ID,UpMargin);
soulx 18:face01c94152 357 break;
b0ssiz 14:28e24fcc5a01 358 }
b0ssiz 14:28e24fcc5a01 359 case LOW_MARGIN: {
ParinyaT 16:c0a1daeb9fa5 360 com.sendLowMargin(MY_ID,LowMargin);
soulx 18:face01c94152 361 break;
b0ssiz 14:28e24fcc5a01 362 }
b0ssiz 14:28e24fcc5a01 363 case PID_UPPER_MOTOR: {
ParinyaT 16:c0a1daeb9fa5 364 com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd);
soulx 18:face01c94152 365 break;
b0ssiz 14:28e24fcc5a01 366 }
b0ssiz 14:28e24fcc5a01 367 case PID_LOWER_MOTOR: {
ParinyaT 16:c0a1daeb9fa5 368 com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
soulx 18:face01c94152 369 break;
b0ssiz 14:28e24fcc5a01 370 }
b0ssiz 14:28e24fcc5a01 371 case HEIGHT: {
ParinyaT 16:c0a1daeb9fa5 372 com.sendHeight(MY_ID,Height);
soulx 18:face01c94152 373 break;
b0ssiz 14:28e24fcc5a01 374 }
b0ssiz 14:28e24fcc5a01 375 case WHEELPOS: {
ParinyaT 16:c0a1daeb9fa5 376 com.sendWheelPos(MY_ID,Wheelpos);
soulx 18:face01c94152 377 break;
b0ssiz 14:28e24fcc5a01 378 }
b0ssiz 14:28e24fcc5a01 379 case MAG_DATA: {
ParinyaT 16:c0a1daeb9fa5 380 com.sendMagData(MY_ID,Mag);
soulx 18:face01c94152 381 break;
b0ssiz 14:28e24fcc5a01 382 }
b0ssiz 14:28e24fcc5a01 383 case OFFSET: {
ParinyaT 16:c0a1daeb9fa5 384 com.sendOffset(MY_ID,Offset);
soulx 18:face01c94152 385 break;
b0ssiz 14:28e24fcc5a01 386 }
b0ssiz 14:28e24fcc5a01 387 case BODY_WIDTH: {
ParinyaT 16:c0a1daeb9fa5 388 com.sendBodyWidth(MY_ID,Body_width);
soulx 18:face01c94152 389 break;
b0ssiz 14:28e24fcc5a01 390 }
b0ssiz 14:28e24fcc5a01 391 case ANGLE_RANGE_UP: {
soulx 24:d4942687f026 392
ParinyaT 16:c0a1daeb9fa5 393 com.sendUpAngleRange(MY_ID,Angle_Range_Up);
soulx 18:face01c94152 394 break;
b0ssiz 14:28e24fcc5a01 395 }
b0ssiz 14:28e24fcc5a01 396 case ANGLE_RANGE_LOW: {
ParinyaT 16:c0a1daeb9fa5 397 com.sendLowAngleRange(MY_ID,Angle_Range_Low);
soulx 18:face01c94152 398 break;
b0ssiz 14:28e24fcc5a01 399 }
soulx 19:3da61e637b2d 400 // break;
ParinyaT 13:49cb002ad8fd 401 }
ParinyaT 13:49cb002ad8fd 402 }
ParinyaT 13:49cb002ad8fd 403 }
b0ssiz 10:3b3d6bc88677 404 }
b0ssiz 14:28e24fcc5a01 405 }
b0ssiz 10:3b3d6bc88677 406 }
b0ssiz 17:4c96838e579f 407
ParinyaT 11:3dd92d1d542c 408 /******************************************************/
ParinyaT 13:49cb002ad8fd 409 void Start_Up()
ParinyaT 11:3dd92d1d542c 410 {
ParinyaT 11:3dd92d1d542c 411 // wait for reciever
ParinyaT 11:3dd92d1d542c 412 memory.read(0x00,MY_ID);
ParinyaT 11:3dd92d1d542c 413 memory.read(0x04,Upper_SetPoint);
ParinyaT 11:3dd92d1d542c 414 memory.read(0x08,Lower_SetPoint);
ParinyaT 11:3dd92d1d542c 415 memory.read(0x10,U_Kc);
ParinyaT 11:3dd92d1d542c 416 memory.read(0x14,U_Ti);
ParinyaT 11:3dd92d1d542c 417 memory.read(0x18,U_Td);
ParinyaT 11:3dd92d1d542c 418 memory.read(0x1c,L_Kc);
ParinyaT 11:3dd92d1d542c 419 memory.read(0x20,L_Ti);
ParinyaT 11:3dd92d1d542c 420 memory.read(0x24,L_Td);
b0ssiz 14:28e24fcc5a01 421
ParinyaT 11:3dd92d1d542c 422 }
b0ssiz 14:28e24fcc5a01 423 /*******************************************************/
soulx 21:1c04c4afe3b7 424 inline void Rc()
ParinyaT 11:3dd92d1d542c 425 {
soulx 21:1c04c4afe3b7 426 myled =1;
b0ssiz 17:4c96838e579f 427 uint8_t data_array[30];
b0ssiz 14:28e24fcc5a01 428 uint8_t id;
b0ssiz 14:28e24fcc5a01 429 uint8_t ins;
soulx 18:face01c94152 430 uint8_t status;
b0ssiz 14:28e24fcc5a01 431
soulx 20:7e6d56655336 432
soulx 20:7e6d56655336 433
soulx 18:face01c94152 434 status = com.ReceiveCommand(&id,data_array,&ins);
soulx 18:face01c94152 435 if(status == ANDANTE_ERRBIT_NONE) {
soulx 18:face01c94152 436 CmdCheck((int16_t)id,data_array,ins);
soulx 18:face01c94152 437 }
soulx 21:1c04c4afe3b7 438
ParinyaT 11:3dd92d1d542c 439 }
ParinyaT 11:3dd92d1d542c 440 /*******************************************************/
ParinyaT 0:451c27e4d55e 441 int main()
ParinyaT 0:451c27e4d55e 442 {
soulx 20:7e6d56655336 443
soulx 21:1c04c4afe3b7 444 //Recieve.attach(&Rc,0.025);
soulx 20:7e6d56655336 445 //Start_Up();
ParinyaT 1:84167ca00307 446 SET_UpperPID();
ParinyaT 1:84167ca00307 447 SET_LowerPID();
ParinyaT 11:3dd92d1d542c 448
b0ssiz 14:28e24fcc5a01 449 while(1) {
soulx 21:1c04c4afe3b7 450 myled =0;
soulx 21:1c04c4afe3b7 451 //wait_ms(10);
soulx 21:1c04c4afe3b7 452
soulx 20:7e6d56655336 453 ///////////////////////////////////////////////////// start
b0ssiz 14:28e24fcc5a01 454 //Set Set_point
b0ssiz 14:28e24fcc5a01 455 Up_PID.setSetPoint(Upper_SetPoint);
b0ssiz 14:28e24fcc5a01 456 Low_PID.setSetPoint(Lower_SetPoint);
ParinyaT 1:84167ca00307 457
b0ssiz 14:28e24fcc5a01 458 //Control Motor
b0ssiz 14:28e24fcc5a01 459 Move_Upper();
b0ssiz 14:28e24fcc5a01 460 Move_Lower();
soulx 20:7e6d56655336 461 ///////////////////////////////////////////////////// stop =306us
soulx 21:1c04c4afe3b7 462 //uint8_t aaa[1]={10};
soulx 21:1c04c4afe3b7 463 //com.sendBodyWidth(0x01,aaa);
soulx 21:1c04c4afe3b7 464 Rc();
soulx 21:1c04c4afe3b7 465 //wait_ms(1);
soulx 20:7e6d56655336 466 }
ParinyaT 0:451c27e4d55e 467 }
b0ssiz 6:98871feebea0 468
ParinyaT 0:451c27e4d55e 469
ParinyaT 0:451c27e4d55e 470
b0ssiz 6:98871feebea0 471
b0ssiz 6:98871feebea0 472
b0ssiz 6:98871feebea0 473