โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
main.cpp
- Committer:
- b0ssiz
- Date:
- 2016-01-26
- Revision:
- 23:999dd41eef14
- Parent:
- 22:449f31da2d3d
- Parent:
- 21:1c04c4afe3b7
- Child:
- 25:c66a0d029e1f
File content as of revision 23:999dd41eef14:
//*****************************************************/ // Include // #include "mbed.h" #include "pinconfig.h" #include "PID.h" #include "Motor.h" #include "eeprom.h" #include "Receiver.h" #include "Motion_EEPROM_Address.h" //*****************************************************/ //--PID parameter-- //-Upper-// float U_Kc; float U_Ti; float U_Td; float U_Ki=U_Kc*U_Ti; float U_Kd=U_Kc*U_Td; //-lower-// float L_Kc; float L_Ti; float L_Td; float L_Ki=L_Kc*L_Ti; float L_Kd=L_Kc*L_Td; //*****************************************************/ // Global // Ticker Recieve; //-- Communication -- Serial PC(D1,D0); Bear_Receiver com(Tx,Rx,1000000); int16_t MY_ID = 0x01; //-- Memorry -- EEPROM memory(PB_4,PA_8,0); uint8_t UpMargin[4]; uint8_t LowMargin[4]; uint8_t Height[4]; uint8_t Wheelpos[4]; uint8_t Mag[24]; uint8_t Offset[8]={1,2,3,4,5,6,7,8}; uint8_t Body_width[4]; uint8_t Angle_Range_Up[8]; uint8_t Angle_Range_Low[8]; uint8_t UpLinkLength[4]; uint8_t LowLinkLength[4]; //-- encoder -- float up_angle,low_angle; 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); //*****************************************************/ DigitalOut myled(LED1); 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(int16_t id,uint8_t *cmd,uint8_t ins) { if(id==MY_ID) { switch (ins) { case PING: { break; } case WRITE_DATA: { switch (cmd[0]) { case ID: { /// MY_ID = (int16_t)cmd[1]; break; } case MOTOR_UPPER_ANG: { uint8_t IntUpAngle[2],FloatUpAngle[2]; uint8_t IntLowAngle[2],FloatLowAngle[2]; float int_buffer,float_buffer; IntUpAngle[0]=cmd[1]; IntUpAngle[1]=cmd[2]; FloatUpAngle[0]=cmd[3]; FloatUpAngle[1]=cmd[4]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; up_angle=int_buffer+float_buffer; IntLowAngle[0]=cmd[5]; IntLowAngle[1]=cmd[6]; FloatLowAngle[0]=cmd[7]; FloatLowAngle[1]=cmd[8]; int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER; low_angle=int_buffer+float_buffer; break; } case UP_MARGIN: { int i; for(i=0; i<4; i++) { UpMargin[0+i]=cmd[1+i]; } break; } case LOW_MARGIN: { int i; for(i=0; i<4; i++) { LowMargin[0+i]=cmd[1+i]; } break; } case KP_UPPER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; U_Kc=Int+Float; break; } case KI_UPPER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float,KI; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; KI=Int+Float; U_Ti=KI/U_Kc; break; } case KD_UPPER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float,KD; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; KD=Int+Float; U_Td=KD/U_Kc; break; } case KP_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; L_Kc=Int+Float; break; } case KI_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float,KI; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; KI=Int+Float; L_Ti=KI/L_Kc; break; } case KD_LOWER_MOTOR: { uint8_t int_buffer[2]; uint8_t float_buffer[2]; float Int,Float,KD; int_buffer[0]=cmd[1]; int_buffer[1]=cmd[2]; float_buffer[0]=cmd[3]; float_buffer[1]=cmd[4]; Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; KD=Int+Float; L_Td=KD/L_Kc; break; } case HEIGHT: { int i; for(i=0; i<4; i++) { Height[0+i]=cmd[1+i]; } break; } case WHEELPOS: { int i; for(i=0; i<4; i++) { Wheelpos[0+i]=cmd[1+i]; } break; } case MAG_DATA: { int i; for(i=0; i<24; i++) { Mag[0+i]=cmd[1+i]; } break; } case OFFSET: { int i; for(i=0; i<8; i++) { Offset[0+i]=cmd[1+i]; } break; } case BODY_WIDTH: { int i; for(i=0; i<4; i++) { Body_width[0+i]=cmd[1+i]; } break; } case ANGLE_RANGE_UP: { int i; for(i=0; i<8; i++) { Angle_Range_Up[0+i]=cmd[1+i]; } break; } case ANGLE_RANGE_LOW: { int i; for(i=0; i<8; i++) { Angle_Range_Low[0+i]=cmd[1+i]; } break; } case UP_LINK_LENGTH: { int i; for(i=0; i<4; i++) { UpLinkLength[0+i]=cmd[1+i]; } break; } case LOW_LINK_LENGTH: { int i; for(i=0; i<4; i++) { LowLinkLength[0+i]=cmd[1+i]; } break; } // unfinish yet!!!!!!!!!!!!!!!!! case SAVE_EEPROM_DATA: { if(id==0x01) { if (cmd[1]==HEIGHT) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Height); memory.write(ADDRESS_HEIGHT,data_buff); } else if(cmd[1]==BODY_WIDTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width); memory.write(ADDRESS_BODY_WIDTH,data_buff); } else if(cmd[1]==OFFSET) { uint8_t y_offset_array[4]; uint8_t z_offset_array[4]; int32_t y_data_buffer,z_data_buffer; for(int i=0; i<4; i++) { y_offset_array[i]=Offset[i]; z_offset_array[i]=Offset[i+4]; } y_data_buffer = Utilities::ConvertUInt8ArrayToInt32(y_offset_array); z_data_buffer = Utilities::ConvertUInt8ArrayToInt32(z_offset_array); memory.write(ADDRESS_OFFSET,y_data_buffer); memory.write(ADDRESS_OFFSET+4,z_data_buffer); } else if(cmd[1]==MAG_DATA) { uint8_t x_max_array[4]; uint8_t x_min_array[4]; uint8_t y_max_array[4]; uint8_t y_min_array[4]; uint8_t z_max_array[4]; uint8_t z_min_array[4]; int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer; for(int i=0; i<4; i++) { x_max_array[i]=Mag[i]; x_min_array[i]=Mag[i+4]; y_max_array[i]=Mag[i+8]; y_min_array[i]=Mag[i+12]; z_max_array[i]=Mag[i+16]; z_min_array[i]=Mag[i+20]; } x_max_buffer = Utilities::ConvertUInt8ArrayToInt32(x_max_array); x_min_buffer = Utilities::ConvertUInt8ArrayToInt32(x_min_array); y_max_buffer = Utilities::ConvertUInt8ArrayToInt32(y_max_array); y_min_buffer = Utilities::ConvertUInt8ArrayToInt32(y_min_array); z_max_buffer = Utilities::ConvertUInt8ArrayToInt32(z_max_array); z_min_buffer = Utilities::ConvertUInt8ArrayToInt32(z_min_array); memory.write(ADDRESS_MAG_DATA,x_max_buffer); memory.write(ADDRESS_MAG_DATA+4,x_min_buffer); memory.write(ADDRESS_MAG_DATA+8,y_max_buffer); memory.write(ADDRESS_MAG_DATA+12,y_min_buffer); memory.write(ADDRESS_MAG_DATA+16,z_max_buffer); memory.write(ADDRESS_MAG_DATA+20,z_min_buffer); } } else { if (cmd[1]==ID) { memory.write(ADDRESS_ID,id); } else if(cmd[1]==UP_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); memory.write(ADDRESS_UP_MARGIN,data_buff); } else if (cmd[1]==LOW_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); memory.write(ADDRESS_LOW_MARGIN,data_buff); } else if (cmd[1]==PID_UPPER_MOTOR) { memory.write(ADDRESS_UPPER_KP,U_Kc); memory.write(ADDRESS_UPPER_KI,U_Ti); memory.write(ADDRESS_UPPER_KD,U_Td); } else if (cmd[1]==PID_LOWER_MOTOR) { memory.write(ADDRESS_LOWER_KP,L_Kc); memory.write(ADDRESS_LOWER_KI,L_Ti); memory.write(ADDRESS_LOWER_KD,L_Td); } else if (cmd[1]==ANGLE_RANGE_UP) { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; for(int i=0; i<4; i++) { max_array[i]=Angle_Range_Up[i]; min_array[i]=Angle_Range_Up[i+4]; } max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); } else if (cmd[1]==ANGLE_RANGE_LOW) { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; for(int i=0; i<4; i++) { max_array[i]=Angle_Range_Low[i]; min_array[i]=Angle_Range_Low[i+4]; } max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); } else if (cmd[1]==UP_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); } else if (cmd[1]==LOW_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); } else if (cmd[1]==WHEELPOS) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); memory.write(ADDRESS_WHEELPOS,data_buff); } } break; } // break; } case READ_DATA: { switch (cmd[0]) { case MOTOR_UPPER_ANG: { com.sendMotorPos(MY_ID,Upper_Position,Lower_Position); break; } case UP_MARGIN: { int32_t data_buff; memory.read(ADDRESS_UP_MARGIN,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin); com.sendUpMargin(MY_ID,UpMargin); break; } case LOW_MARGIN: { int32_t data_buff; memory.read(ADDRESS_LOW_MARGIN,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,LowMargin); com.sendLowMargin(MY_ID,LowMargin); break; } case PID_UPPER_MOTOR: { memory.read(ADDRESS_UPPER_KP,U_Kc); memory.read(ADDRESS_UPPER_KI,U_Ki); memory.read(ADDRESS_UPPER_KD,U_Kd); com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki,U_Kd); break; } case PID_LOWER_MOTOR: { memory.read(ADDRESS_LOWER_KP,L_Kc); memory.read(ADDRESS_LOWER_KI,L_Ki); memory.read(ADDRESS_LOWER_KD,L_Kd); com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd); break; } case HEIGHT: { int32_t data_buff; memory.read(ADDRESS_HEIGHT,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,Height); com.sendHeight(MY_ID,Height); break; } case WHEELPOS: { int32_t data_buff; memory.read(ADDRESS_WHEELPOS,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,Wheelpos); com.sendWheelPos(MY_ID,Wheelpos); break; } case MAG_DATA: { uint8_t x_max_array[4]; uint8_t x_min_array[4]; uint8_t y_max_array[4]; uint8_t y_min_array[4]; uint8_t z_max_array[4]; uint8_t z_min_array[4]; int32_t x_max_buffer,x_min_buffer,y_max_buffer,y_min_buffer,z_max_buffer,z_min_buffer; memory.read(ADDRESS_MAG_DATA,x_max_buffer); memory.read(ADDRESS_MAG_DATA+4,x_min_buffer); memory.read(ADDRESS_MAG_DATA+8,y_max_buffer); memory.read(ADDRESS_MAG_DATA+12,y_min_buffer); memory.read(ADDRESS_MAG_DATA+16,z_max_buffer); memory.read(ADDRESS_MAG_DATA+20,z_min_buffer); Utilities::ConvertInt32ToUInt8Array(x_max_buffer,x_max_array); Utilities::ConvertInt32ToUInt8Array(x_min_buffer,x_min_array); Utilities::ConvertInt32ToUInt8Array(y_max_buffer,y_max_array); Utilities::ConvertInt32ToUInt8Array(y_min_buffer,y_min_array); Utilities::ConvertInt32ToUInt8Array(z_max_buffer,z_max_array); Utilities::ConvertInt32ToUInt8Array(z_min_buffer,z_min_array); for(int i=0; i<4; i++) { Mag[i]=x_max_array[i]; Mag[i+4]=x_min_array[i]; Mag[i+8]=y_max_array[i]; Mag[i+12]=y_min_array[i]; Mag[i+16]=z_max_array[i]; Mag[i+20]=z_min_array[i]; } com.sendMagData(MY_ID,Mag); break; } case OFFSET: { uint8_t y_offset_array[4]; uint8_t z_offset_array[4]; int32_t y_data_buffer,z_data_buffer; memory.read(ADDRESS_OFFSET,y_data_buffer); memory.read(ADDRESS_OFFSET+4,z_data_buffer); Utilities::ConvertInt32ToUInt8Array(y_data_buffer,y_offset_array); Utilities::ConvertInt32ToUInt8Array(z_data_buffer,z_offset_array); for(int i=0; i<4; i++) { Offset[i]=y_offset_array[i]; Offset[i+4]=z_offset_array[i]; } com.sendOffset(MY_ID,Offset); break; } case BODY_WIDTH: { int32_t data_buff; memory.read(ADDRESS_BODY_WIDTH,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,Body_width); com.sendBodyWidth(MY_ID,Body_width); break; } case ANGLE_RANGE_UP: { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; memory.read(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); memory.read(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array); Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array); for(int i=0; i<4; i++) { Angle_Range_Up[i]=max_array[i]; Angle_Range_Up[i+4]=min_array[i]; } com.sendUpAngleRange(MY_ID,Angle_Range_Up); break; } case ANGLE_RANGE_LOW: { uint8_t max_array[4]; uint8_t min_array[4]; int32_t max_data_buffer,min_data_buffer; memory.read(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); memory.read(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); Utilities::ConvertInt32ToUInt8Array(max_data_buffer,max_array); Utilities::ConvertInt32ToUInt8Array(min_data_buffer,min_array); for(int i=0; i<4; i++) { Angle_Range_Low[i]=max_array[i]; Angle_Range_Low[i+4]=min_array[i]; } com.sendLowAngleRange(MY_ID,Angle_Range_Low); break; } case UP_LINK_LENGTH: { int32_t data_buff; memory.read(ADDRESS_UP_LINK_LENGTH,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,UpLinkLength); com.sendUpLinkLength(MY_ID,UpLinkLength); break; } case LOW_LINK_LENGTH: { int32_t data_buff; memory.read(ADDRESS_LOW_LINK_LENGTH,data_buff); Utilities::ConvertInt32ToUInt8Array(data_buff,LowLinkLength); com.sendLowLinkLength(MY_ID,LowLinkLength); break; } // break; } } } } } } /******************************************************/ void Start_Up() { // wait for reciever memory.read(ADDRESS_ID,MY_ID); memory.read(ADDRESS_UPPER_KP,U_Kc); memory.read(ADDRESS_UPPER_KI,U_Ti); memory.read(ADDRESS_UPPER_KD,U_Td); memory.read(ADDRESS_LOWER_KP,L_Kc); memory.read(ADDRESS_LOWER_KI,L_Ti); memory.read(ADDRESS_LOWER_KD,L_Td); } /*******************************************************/ inline void Rc() { myled =1; uint8_t data_array[30]; uint8_t id; uint8_t ins; uint8_t status; status = com.ReceiveCommand(&id,data_array,&ins); if(status == ANDANTE_ERRBIT_NONE) { CmdCheck((int16_t)id,data_array,ins); } } /*******************************************************/ int main() { //Recieve.attach(&Rc,0.025); //Start_Up(); SET_UpperPID(); SET_LowerPID(); while(1) { myled =0; //wait_ms(10); ///////////////////////////////////////////////////// start //Set Set_point Up_PID.setSetPoint(Upper_SetPoint); Low_PID.setSetPoint(Lower_SetPoint); //Control Motor Move_Upper(); Move_Lower(); ///////////////////////////////////////////////////// stop =306us //uint8_t aaa[1]={10}; //com.sendBodyWidth(0x01,aaa); Rc(); //wait_ms(1); } }