![](/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@10:3b3d6bc88677, 2016-01-16 (annotated)
- Committer:
- b0ssiz
- Date:
- Sat Jan 16 12:06:59 2016 +0000
- Revision:
- 10:3b3d6bc88677
- Parent:
- 7:bf239d051e8c
- Child:
- 11:3dd92d1d542c
Update receiver
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 | 1:84167ca00307 | 16 | //-lower-// |
b0ssiz | 10:3b3d6bc88677 | 17 | float L_Kc; |
b0ssiz | 10:3b3d6bc88677 | 18 | float L_Ti; |
b0ssiz | 10:3b3d6bc88677 | 19 | float L_Td; |
ParinyaT | 0:451c27e4d55e | 20 | |
ParinyaT | 0:451c27e4d55e | 21 | //*****************************************************/ |
ParinyaT | 0:451c27e4d55e | 22 | // Global // |
ParinyaT | 1:84167ca00307 | 23 | //-- Communication -- |
ParinyaT | 0:451c27e4d55e | 24 | Serial PC(D1,D0); |
ParinyaT | 1:84167ca00307 | 25 | ANDANTE_PROTOCOL_PACKET command; |
b0ssiz | 10:3b3d6bc88677 | 26 | Bear_Receiver com(Tx,Rx,115200); |
b0ssiz | 10:3b3d6bc88677 | 27 | #define MY_ID 0x01 |
b0ssiz | 6:98871feebea0 | 28 | //-- encoder -- |
ParinyaT | 1:84167ca00307 | 29 | int Upper_Position; |
ParinyaT | 1:84167ca00307 | 30 | int Lower_Position; |
ParinyaT | 0:451c27e4d55e | 31 | int data; |
ParinyaT | 1:84167ca00307 | 32 | SPI ENC(Emosi, Emiso, Esck); |
ParinyaT | 1:84167ca00307 | 33 | DigitalOut EncA(EncoderA); |
ParinyaT | 1:84167ca00307 | 34 | DigitalOut EncB(EncoderB); |
ParinyaT | 0:451c27e4d55e | 35 | //-- Motor -- |
ParinyaT | 1:84167ca00307 | 36 | int dir; |
ParinyaT | 1:84167ca00307 | 37 | Motor Upper(PWM_LU,A_LU,B_LU); |
ParinyaT | 1:84167ca00307 | 38 | Motor Lower(PWM_LL,A_LL,B_LL); |
ParinyaT | 0:451c27e4d55e | 39 | //-- PID -- |
ParinyaT | 1:84167ca00307 | 40 | int Upper_SetPoint; |
ParinyaT | 1:84167ca00307 | 41 | int Lower_SetPoint; |
ParinyaT | 1:84167ca00307 | 42 | PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate |
ParinyaT | 1:84167ca00307 | 43 | PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); |
ParinyaT | 0:451c27e4d55e | 44 | //*****************************************************/ |
ParinyaT | 1:84167ca00307 | 45 | void Read_Encoder(PinName Encoder) |
ParinyaT | 0:451c27e4d55e | 46 | { |
soulx | 7:bf239d051e8c | 47 | ENC.format(8,0); |
soulx | 7:bf239d051e8c | 48 | ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k |
b0ssiz | 6:98871feebea0 | 49 | |
soulx | 7:bf239d051e8c | 50 | if(Encoder == EncoderA) { |
soulx | 7:bf239d051e8c | 51 | EncA = 0; |
soulx | 7:bf239d051e8c | 52 | } else { |
soulx | 7:bf239d051e8c | 53 | EncB = 0; |
soulx | 7:bf239d051e8c | 54 | } |
soulx | 7:bf239d051e8c | 55 | ENC.write(0x41); |
soulx | 7:bf239d051e8c | 56 | ENC.write(0x09); |
soulx | 7:bf239d051e8c | 57 | data = ENC.write(0x00); |
soulx | 7:bf239d051e8c | 58 | if(Encoder == EncoderA) { |
soulx | 7:bf239d051e8c | 59 | EncA = 1; |
soulx | 7:bf239d051e8c | 60 | } else { |
soulx | 7:bf239d051e8c | 61 | EncB = 1; |
soulx | 7:bf239d051e8c | 62 | } |
b0ssiz | 6:98871feebea0 | 63 | |
ParinyaT | 0:451c27e4d55e | 64 | } |
ParinyaT | 0:451c27e4d55e | 65 | //*****************************************************/ |
ParinyaT | 1:84167ca00307 | 66 | int Get_EnValue(int Val) |
ParinyaT | 0:451c27e4d55e | 67 | { |
ParinyaT | 0:451c27e4d55e | 68 | int i = 0; |
ParinyaT | 0:451c27e4d55e | 69 | static unsigned char codes[] = { |
b0ssiz | 6:98871feebea0 | 70 | 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, |
b0ssiz | 6:98871feebea0 | 71 | 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, |
b0ssiz | 6:98871feebea0 | 72 | 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, |
b0ssiz | 6:98871feebea0 | 73 | 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, |
b0ssiz | 6:98871feebea0 | 74 | 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, |
b0ssiz | 6:98871feebea0 | 75 | 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, |
b0ssiz | 6:98871feebea0 | 76 | 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, |
b0ssiz | 6:98871feebea0 | 77 | 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 |
b0ssiz | 6:98871feebea0 | 78 | }; |
b0ssiz | 6:98871feebea0 | 79 | |
b0ssiz | 6:98871feebea0 | 80 | while(Val != codes[i]) { |
ParinyaT | 1:84167ca00307 | 81 | i++; |
ParinyaT | 0:451c27e4d55e | 82 | } |
b0ssiz | 6:98871feebea0 | 83 | |
ParinyaT | 1:84167ca00307 | 84 | return i; |
b0ssiz | 6:98871feebea0 | 85 | |
ParinyaT | 0:451c27e4d55e | 86 | } |
ParinyaT | 0:451c27e4d55e | 87 | //*****************************************************/ |
ParinyaT | 1:84167ca00307 | 88 | void SET_UpperPID() |
ParinyaT | 1:84167ca00307 | 89 | { |
ParinyaT | 1:84167ca00307 | 90 | Upper.period(0.001); |
ParinyaT | 1:84167ca00307 | 91 | Up_PID.setMode(0); |
ParinyaT | 1:84167ca00307 | 92 | Up_PID.setInputLimits(0,127); |
ParinyaT | 1:84167ca00307 | 93 | Up_PID.setOutputLimits(0,1); |
ParinyaT | 1:84167ca00307 | 94 | } |
ParinyaT | 1:84167ca00307 | 95 | //******************************************************/ |
ParinyaT | 1:84167ca00307 | 96 | void SET_LowerPID() |
ParinyaT | 1:84167ca00307 | 97 | { |
ParinyaT | 1:84167ca00307 | 98 | Lower.period(0.001); |
ParinyaT | 1:84167ca00307 | 99 | Low_PID.setMode(0); |
b0ssiz | 10:3b3d6bc88677 | 100 | Low_PID.setInputLimits(0,127); // set range |
ParinyaT | 1:84167ca00307 | 101 | Low_PID.setOutputLimits(0,1); |
ParinyaT | 1:84167ca00307 | 102 | } |
b0ssiz | 6:98871feebea0 | 103 | //******************************************************/ |
ParinyaT | 1:84167ca00307 | 104 | void Move_Upper() |
ParinyaT | 1:84167ca00307 | 105 | { |
ParinyaT | 1:84167ca00307 | 106 | Read_Encoder(EncoderA); |
ParinyaT | 1:84167ca00307 | 107 | Upper_Position = Get_EnValue(data); |
ParinyaT | 1:84167ca00307 | 108 | |
ParinyaT | 1:84167ca00307 | 109 | Up_PID.setProcessValue(Upper_Position); |
b0ssiz | 6:98871feebea0 | 110 | |
soulx | 7:bf239d051e8c | 111 | if(Upper_Position - Upper_SetPoint > 0 ) { |
ParinyaT | 1:84167ca00307 | 112 | dir = 1; |
b0ssiz | 6:98871feebea0 | 113 | } |
soulx | 7:bf239d051e8c | 114 | if(Upper_Position - Upper_SetPoint < 0 ) { |
ParinyaT | 1:84167ca00307 | 115 | dir = -1; |
b0ssiz | 6:98871feebea0 | 116 | } |
soulx | 7:bf239d051e8c | 117 | Upper.speed(Up_PID.compute() * dir); |
ParinyaT | 1:84167ca00307 | 118 | } |
ParinyaT | 1:84167ca00307 | 119 | //******************************************************/ |
ParinyaT | 1:84167ca00307 | 120 | void Move_Lower() |
ParinyaT | 1:84167ca00307 | 121 | { |
ParinyaT | 1:84167ca00307 | 122 | Read_Encoder(EncoderB); |
ParinyaT | 1:84167ca00307 | 123 | Lower_Position = Get_EnValue(data); |
ParinyaT | 1:84167ca00307 | 124 | |
ParinyaT | 1:84167ca00307 | 125 | Low_PID.setProcessValue(Lower_Position); |
b0ssiz | 6:98871feebea0 | 126 | |
b0ssiz | 6:98871feebea0 | 127 | if(Lower_Position - Lower_SetPoint > 0 ) { |
ParinyaT | 1:84167ca00307 | 128 | dir = 1; |
b0ssiz | 6:98871feebea0 | 129 | } |
b0ssiz | 6:98871feebea0 | 130 | if(Lower_Position - Lower_SetPoint < 0 ) { |
ParinyaT | 1:84167ca00307 | 131 | dir = -1; |
b0ssiz | 6:98871feebea0 | 132 | } |
soulx | 7:bf239d051e8c | 133 | Lower.speed(Low_PID.compute() * dir); |
b0ssiz | 6:98871feebea0 | 134 | } |
ParinyaT | 1:84167ca00307 | 135 | //******************************************************/ |
ParinyaT | 1:84167ca00307 | 136 | |
b0ssiz | 10:3b3d6bc88677 | 137 | void CmdCheck(uint8_t *cmd) |
b0ssiz | 10:3b3d6bc88677 | 138 | { |
b0ssiz | 10:3b3d6bc88677 | 139 | switch(cmd[0]) { |
b0ssiz | 10:3b3d6bc88677 | 140 | case SET_MOTOR_UPPER_ANG : { |
b0ssiz | 10:3b3d6bc88677 | 141 | uint8_t Upper_setpoint_buffer[2]; |
b0ssiz | 10:3b3d6bc88677 | 142 | uint8_t Lower_setpoint_buffer[2]; |
b0ssiz | 10:3b3d6bc88677 | 143 | Upper_setpoint_buffer[0]=cmd[1]; |
b0ssiz | 10:3b3d6bc88677 | 144 | Upper_setpoint_buffer[1]=cmd[2]; |
b0ssiz | 10:3b3d6bc88677 | 145 | Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer); |
b0ssiz | 10:3b3d6bc88677 | 146 | Lower_setpoint_buffer[0]=cmd[5]; |
b0ssiz | 10:3b3d6bc88677 | 147 | Lower_setpoint_buffer[1]=cmd[6]; |
b0ssiz | 10:3b3d6bc88677 | 148 | Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer); |
b0ssiz | 10:3b3d6bc88677 | 149 | //Set Set_point |
b0ssiz | 10:3b3d6bc88677 | 150 | Up_PID.setSetPoint(Upper_SetPoint); |
b0ssiz | 10:3b3d6bc88677 | 151 | Low_PID.setSetPoint(Lower_SetPoint); |
b0ssiz | 10:3b3d6bc88677 | 152 | |
b0ssiz | 10:3b3d6bc88677 | 153 | //Control Motor |
b0ssiz | 10:3b3d6bc88677 | 154 | Move_Upper(); |
b0ssiz | 10:3b3d6bc88677 | 155 | Move_Lower(); |
b0ssiz | 10:3b3d6bc88677 | 156 | } |
b0ssiz | 10:3b3d6bc88677 | 157 | |
b0ssiz | 10:3b3d6bc88677 | 158 | case GET_MOTOR_UPPER_ANG : { |
b0ssiz | 10:3b3d6bc88677 | 159 | float up_angle,low_angle; |
b0ssiz | 10:3b3d6bc88677 | 160 | // |
b0ssiz | 10:3b3d6bc88677 | 161 | com.sendMotorPos(MY_ID,up_angle,low_angle); |
b0ssiz | 10:3b3d6bc88677 | 162 | } |
b0ssiz | 10:3b3d6bc88677 | 163 | case SAVE_DATA_TO_EEPROM : { |
b0ssiz | 10:3b3d6bc88677 | 164 | uint8_t Int_data_buffer[2]; |
b0ssiz | 10:3b3d6bc88677 | 165 | uint8_t Float_data_buffer[2]; |
b0ssiz | 10:3b3d6bc88677 | 166 | uint8_t Address_buffer[2]; |
b0ssiz | 10:3b3d6bc88677 | 167 | float int_buffer; |
b0ssiz | 10:3b3d6bc88677 | 168 | float float_buffer; |
b0ssiz | 10:3b3d6bc88677 | 169 | float data; |
b0ssiz | 10:3b3d6bc88677 | 170 | uint16_t address; |
b0ssiz | 10:3b3d6bc88677 | 171 | Int_data_buffer[0]=cmd[1]; |
b0ssiz | 10:3b3d6bc88677 | 172 | Int_data_buffer[1]=cmd[2]; |
b0ssiz | 10:3b3d6bc88677 | 173 | Float_data_buffer[0]=cmd[3]; |
b0ssiz | 10:3b3d6bc88677 | 174 | Float_data_buffer[1]=cmd[4]; |
b0ssiz | 10:3b3d6bc88677 | 175 | int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Int_data_buffer); |
b0ssiz | 10:3b3d6bc88677 | 176 | float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Float_data_buffer)/FLOAT_CONVERTER; |
b0ssiz | 10:3b3d6bc88677 | 177 | |
b0ssiz | 10:3b3d6bc88677 | 178 | address=Utilities::ConvertUInt8ArrayToInt16(Address_buffer); |
b0ssiz | 10:3b3d6bc88677 | 179 | data=int_buffer+float_buffer; |
b0ssiz | 10:3b3d6bc88677 | 180 | |
b0ssiz | 10:3b3d6bc88677 | 181 | // |
b0ssiz | 10:3b3d6bc88677 | 182 | } |
b0ssiz | 10:3b3d6bc88677 | 183 | } |
b0ssiz | 10:3b3d6bc88677 | 184 | } |
b0ssiz | 10:3b3d6bc88677 | 185 | |
ParinyaT | 1:84167ca00307 | 186 | |
ParinyaT | 0:451c27e4d55e | 187 | int main() |
ParinyaT | 0:451c27e4d55e | 188 | { |
b0ssiz | 10:3b3d6bc88677 | 189 | uint8_t id; |
b0ssiz | 10:3b3d6bc88677 | 190 | uint8_t data_array[10]; |
ParinyaT | 1:84167ca00307 | 191 | SET_UpperPID(); |
ParinyaT | 1:84167ca00307 | 192 | SET_LowerPID(); |
b0ssiz | 10:3b3d6bc88677 | 193 | /* |
b0ssiz | 10:3b3d6bc88677 | 194 | while(1) { |
b0ssiz | 10:3b3d6bc88677 | 195 | //Set Set_point |
b0ssiz | 10:3b3d6bc88677 | 196 | Up_PID.setSetPoint(Upper_SetPoint); |
b0ssiz | 10:3b3d6bc88677 | 197 | Low_PID.setSetPoint(Lower_SetPoint); |
ParinyaT | 1:84167ca00307 | 198 | |
b0ssiz | 10:3b3d6bc88677 | 199 | //Control Motor |
b0ssiz | 10:3b3d6bc88677 | 200 | Move_Upper(); |
b0ssiz | 10:3b3d6bc88677 | 201 | Move_Lower(); |
b0ssiz | 10:3b3d6bc88677 | 202 | } |
b0ssiz | 10:3b3d6bc88677 | 203 | */ |
ParinyaT | 1:84167ca00307 | 204 | |
b0ssiz | 10:3b3d6bc88677 | 205 | com.ReceiveCommand(id,data_array); |
ParinyaT | 0:451c27e4d55e | 206 | } |
b0ssiz | 6:98871feebea0 | 207 | |
ParinyaT | 0:451c27e4d55e | 208 | |
ParinyaT | 0:451c27e4d55e | 209 | |
b0ssiz | 6:98871feebea0 | 210 | |
b0ssiz | 6:98871feebea0 | 211 | |
b0ssiz | 6:98871feebea0 | 212 |