first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
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?
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" |
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 |