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