first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of BEAR_Motion by
Diff: main.cpp
- Revision:
- 31:d6fa5e8e26b3
- Parent:
- 30:3f8e86fa1413
- Child:
- 32:1f81f3e83889
--- a/main.cpp Wed Jan 27 21:43:48 2016 +0000 +++ b/main.cpp Mon Feb 01 19:02:56 2016 +0000 @@ -7,7 +7,7 @@ #include "eeprom.h" #include "Receiver.h" #include "Motion_EEPROM_Address.h" - +#define EEPROM_DELAY 2 //*****************************************************/ //--PID parameter-- //-Upper-// @@ -48,8 +48,8 @@ uint8_t LowLinkLength[4]; //-- encoder -- float up_angle,low_angle; -int Upper_Position; -int Lower_Position; +float Upper_Position; +float Lower_Position; int data; SPI ENC(Emosi, Emiso, Esck); DigitalOut EncA(EncoderA); @@ -130,7 +130,7 @@ void Move_Upper() { Read_Encoder(EncoderA); - Upper_Position = Get_EnValue(data); + Upper_Position = (int)Get_EnValue(data); Up_PID.setProcessValue(Upper_Position); @@ -146,7 +146,7 @@ void Move_Lower() { Read_Encoder(EncoderB); - Lower_Position = Get_EnValue(data); + Lower_Position = (int)Get_EnValue(data); Low_PID.setProcessValue(Lower_Position); @@ -177,24 +177,14 @@ case MOTOR_UPPER_ANG: { uint8_t IntUpAngle[2],FloatUpAngle[2]; uint8_t IntLowAngle[2],FloatLowAngle[2]; - float int_buffer,float_buffer; IntUpAngle[0]=command[1]; IntUpAngle[1]=command[2]; - FloatUpAngle[0]=command[3]; - FloatUpAngle[1]=command[4]; - int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); - float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatUpAngle)/FLOAT_CONVERTER; - up_angle=int_buffer+float_buffer; + Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); //printf("Up Angle = %f\n",up_angle); - IntLowAngle[0]=command[5]; IntLowAngle[1]=command[6]; - FloatLowAngle[0]=command[7]; - FloatLowAngle[1]=command[8]; - int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); - float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(FloatLowAngle)/FLOAT_CONVERTER; - low_angle=int_buffer+float_buffer; + Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); //printf("Low Angle = %f\n",low_angle); break; } @@ -374,13 +364,13 @@ int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Height); memory.write(ADDRESS_HEIGHT,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if(command[1]==BODY_WIDTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Body_width); memory.write(ADDRESS_BODY_WIDTH,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if(command[1]==OFFSET) { uint8_t y_offset_array[4]; @@ -393,9 +383,9 @@ y_data_buffer = Utilities::ConvertUInt8ArrayToInt32(y_offset_array); z_data_buffer = Utilities::ConvertUInt8ArrayToInt32(z_offset_array); memory.write(ADDRESS_OFFSET,y_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_OFFSET+4,z_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if(command[1]==MAG_DATA) { uint8_t x_max_array[4]; @@ -420,17 +410,17 @@ z_max_buffer = Utilities::ConvertUInt8ArrayToInt32(z_max_array); z_min_buffer = Utilities::ConvertUInt8ArrayToInt32(z_min_array); memory.write(ADDRESS_MAG_DATA,x_max_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_MAG_DATA+4,x_min_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_MAG_DATA+8,y_max_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_MAG_DATA+12,y_min_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_MAG_DATA+16,z_max_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_MAG_DATA+20,z_min_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); } @@ -438,42 +428,42 @@ // else { if (command[1]==ID) { memory.write(ADDRESS_ID,id); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if(command[1]==UP_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpMargin); memory.write(ADDRESS_UP_MARGIN,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); //printf("save OK!!\n\r"); } else if (command[1]==LOW_MARGIN) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowMargin); memory.write(ADDRESS_LOW_MARGIN,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==PID_UPPER_MOTOR) { memory.write(ADDRESS_UPPER_KP,U_Kc); //printf("U_Write : %f\r\n",U_Kc); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_UPPER_KI,U_Ki_gain); //printf("U_Write : %f\r\n",U_Ki_gain); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_UPPER_KD,U_Kd_gain); //printf("U_Write : %f\r\n",U_Kd_gain); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==PID_LOWER_MOTOR) { memory.write(ADDRESS_LOWER_KP,L_Kc); //printf("L_Write : %f\r\n",L_Kc); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_LOWER_KI,L_Ki_gain); //printf("L_Write : %f\r\n",L_Ki_gain); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_LOWER_KD,L_Kd_gain); //printf("L_Write : %f\r\n",L_Kd_gain); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==ANGLE_RANGE_UP) { uint8_t max_array[4]; @@ -486,9 +476,9 @@ max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); memory.write(ADDRESS_ANGLE_RANGE_UP,max_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==ANGLE_RANGE_LOW) { uint8_t max_array[4]; @@ -501,27 +491,27 @@ max_data_buffer = Utilities::ConvertUInt8ArrayToInt32(max_array); min_data_buffer = Utilities::ConvertUInt8ArrayToInt32(min_array); memory.write(ADDRESS_ANGLE_RANGE_LOW,max_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==UP_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(UpLinkLength); memory.write(ADDRESS_UP_LINK_LENGTH,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==LOW_LINK_LENGTH) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(LowLinkLength); memory.write(ADDRESS_LOW_LINK_LENGTH,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } else if (command[1]==WHEELPOS) { int32_t data_buff; data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); memory.write(ADDRESS_WHEELPOS,data_buff); - wait_ms(1); + wait_ms(EEPROM_DELAY); } break; } @@ -532,7 +522,7 @@ case READ_DATA: { switch (command[0]) { case MOTOR_UPPER_ANG: { - com.sendMotorPos(MY_ID,up_angle,low_angle); + com.sendMotorPos(MY_ID,Upper_Position,Lower_Position); break; } case UP_MARGIN: { @@ -727,6 +717,19 @@ int main() { PC.baud(115200); + /* + while(1) + { + Read_Encoder(EncoderA); + Upper_Position = Get_EnValue(data); + Read_Encoder(EncoderB); + Lower_Position = Get_EnValue(data); + PC.printf("Upper Position : %f\n",Upper_Position); + PC.printf("Lower_Position : %f\n",Lower_Position); + wait(0.5); + } + */ + //Recieve.attach(&Rc,0.025); //Start_Up();