first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
main.cpp
- Committer:
- ParinyaT
- Date:
- 2016-01-21
- Revision:
- 12:6b3b997dd7c2
- Parent:
- 11:3dd92d1d542c
- Child:
- 13:49cb002ad8fd
File content as of revision 12:6b3b997dd7c2:
//*****************************************************/ // Include // #include "mbed.h" #include "pinconfig.h" #include "PID.h" #include "Motor.h" #include "eeprom.h" #include "Receiver.h" //*****************************************************/ //--PID parameter-- //-Upper-// float U_Kc; float U_Ti; float U_Td; //-lower-// float L_Kc; float L_Ti; float L_Td; //*****************************************************/ // Global // Ticker Recieve; //-- Communication -- Serial PC(D1,D0); Bear_Receiver com(Tx,Rx,115200); int8_t MY_ID = 0x01; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); //-- encoder -- int Upper_Position; int Lower_Position; int data; SPI ENC(Emosi, Emiso, Esck); DigitalOut EncA(EncoderA); DigitalOut EncB(EncoderB); //-- Motor -- int dir; Motor Upper(PWM_LU,A_LU,B_LU); Motor Lower(PWM_LL,A_LL,B_LL); //-- PID -- int Upper_SetPoint; int Lower_SetPoint; PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate PID Low_PID(L_Kc, L_Ti, L_Td, 0.001); //*****************************************************/ void Read_Encoder(PinName Encoder) { ENC.format(8,0); ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k if(Encoder == EncoderA) { EncA = 0; } else { EncB = 0; } ENC.write(0x41); ENC.write(0x09); data = ENC.write(0x00); if(Encoder == EncoderA) { EncA = 1; } else { EncB = 1; } } //*****************************************************/ int Get_EnValue(int Val) { int i = 0; static unsigned char codes[] = { 127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175, 191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215, 223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235, 239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245, 247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250, 251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125, 253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190, 254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95 }; while(Val != codes[i]) { i++; } return i; } //*****************************************************/ void SET_UpperPID() { Upper.period(0.001); Up_PID.setMode(0); Up_PID.setInputLimits(0,127); Up_PID.setOutputLimits(0,1); } //******************************************************/ void SET_LowerPID() { Lower.period(0.001); Low_PID.setMode(0); Low_PID.setInputLimits(0,127); // set range Low_PID.setOutputLimits(0,1); } //******************************************************/ void Move_Upper() { Read_Encoder(EncoderA); Upper_Position = Get_EnValue(data); Up_PID.setProcessValue(Upper_Position); if(Upper_Position - Upper_SetPoint > 0 ) { dir = 1; } if(Upper_Position - Upper_SetPoint < 0 ) { dir = -1; } Upper.speed(Up_PID.compute() * dir); } //******************************************************/ void Move_Lower() { Read_Encoder(EncoderB); Lower_Position = Get_EnValue(data); Low_PID.setProcessValue(Lower_Position); if(Lower_Position - Lower_SetPoint > 0 ) { dir = 1; } if(Lower_Position - Lower_SetPoint < 0 ) { dir = -1; } Lower.speed(Low_PID.compute() * dir); } //******************************************************/ void CmdCheck(uint8_t *cmd,uint8_t ins) { switch (ins) { case PING:{ } case READ_DATA:{ switch (cmd[0]){ case ID:{ MY_ID = cmd[1]; } case MOTOR_UPPER_ANG:{ uint8_t Upper_setpoint_buffer[2]; Upper_setpoint_buffer[0]=cmd[1]; Upper_setpoint_buffer[1]=cmd[2]; Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer); } case MOTOR_LOWER_ANG:{ uint8_t Lower_setpoint_buffer[2]; Lower_setpoint_buffer[0]=cmd[1]; Lower_setpoint_buffer[1]=cmd[2]; Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer); } case MARGIN:{ uint8_t Margin_buffer[4]; Margin_buffer[0]=cmd[1]; Margin_buffer[1]=cmd[2]; Margin_buffer[2]=cmd[3]; Margin_buffer[3]=cmd[4]; Margin=Utilities::ConvertUInt8ArrayToInt16(Margin_buffer); } case KP_UPPER_MOTOR:{ uint8_t KP_buffer[2]; KP_buffer[0]=cmd[1]; KP_buffer[1]=cmd[2]; U_Kc=Utilities::ConvertUInt8ArrayToInt16(KP_buffer); } case KI_UPPER_MOTOR:{ uint8_t KI_buffer[2]; float KI; KI_buffer[0]=cmd[1]; KI_buffer[1]=cmd[2]; KI=Utilities::ConvertUInt8ArrayToInt16(KI_buffer); U_Ti=KI/U_Kc; } case KD_UPPER_MOTOR:{ } } } /******************************************************/ void Start_Up ()// load parameter from eeprom before start { // wait for reciever memory.read(0x00,MY_ID); memory.read(0x04,Upper_SetPoint); memory.read(0x08,Lower_SetPoint); memory.read(0x10,U_Kc); memory.read(0x14,U_Ti); memory.read(0x18,U_Td); memory.read(0x1c,L_Kc); memory.read(0x20,L_Ti); memory.read(0x24,L_Td); } /*******************************************************/ void Rc () { uint8_t data_array[10]; int8_t ins; com.ReceiveCommand(&MY_ID,data_array,&ins); CmdCheck(data_array,ins); } /*******************************************************/ int main() { Recieve.attach(&Rc,0.2); Start_Up(); SET_UpperPID(); SET_LowerPID(); while(1) { //Set Set_point Up_PID.setSetPoint(Upper_SetPoint); Low_PID.setSetPoint(Lower_SetPoint); //Control Motor Move_Upper(); Move_Lower(); } //com.ReceiveCommand(id,data_array); }