โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of DogPID by
Diff: main.cpp.orig
- Revision:
- 36:1561b6d61095
diff -r 7fa769563e61 -r 1561b6d61095 main.cpp.orig --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp.orig Wed Feb 03 14:49:46 2016 +0000 @@ -0,0 +1,774 @@ +//*****************************************************/ +// Include // +#include "mbed.h" +#include "pinconfig.h" +#include "PID.h" +#include "Motor.h" +#include "eeprom.h" +#include "Receiver.h" +#include "Motion_EEPROM_Address.h" +#define EEPROM_DELAY 2 +//*****************************************************/ +//--PID parameter-- +//-Upper-// +float U_Kc; +float U_Ki_gain; +float U_Kd_gain; +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_Ki_gain; +float L_Kd_gain; +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; +float Upper_Position; +float 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 + }; + if ( MY_ID == 0x01 )//when it was left side + { + while(Val != codes[i]) { + i++; + } + } + if ( MY_ID == 0x02 )//when it was right side + { + while(Val != codes[127-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 *command,uint8_t ins) +{ + if(id==MY_ID) { + switch (ins) { + case PING: { + break; + } + case WRITE_DATA: { + switch (command[0]) { + case ID: { + /// + MY_ID = (int16_t)command[1]; + break; + } + case MOTOR_UPPER_ANG: { + uint8_t IntUpAngle[2]; + uint8_t IntLowAngle[2]; + + IntUpAngle[0]=command[1]; + IntUpAngle[1]=command[2]; + Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle); + //printf("Up Angle = %f\n",up_angle); + IntLowAngle[0]=command[5]; + IntLowAngle[1]=command[6]; + Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle); + //printf("Low Angle = %f\n",low_angle); + break; + } + case UP_MARGIN: { + int i; + for(i=0; i<4; i++) { + UpMargin[i]=command[1+i]; + //printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]); + } + break; + } + case LOW_MARGIN: { + int i; + for(i=0; i<4; i++) { + LowMargin[i]=command[1+i]; + } + break; + } + case KP_UPPER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + U_Kc=Int+Float; + //printf("Kp Upper : %f\r\n",U_Kc); + break; + } + case KI_UPPER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + U_Ki_gain=Int+Float; + U_Ti=U_Ki_gain/U_Kc; + //printf("Ki Upper : %f\r\n",U_Ki_gain); + break; + } + case KD_UPPER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + U_Kd_gain=Int+Float; + U_Td=U_Kd_gain/U_Kc; + //printf("Kd Upper : %f\r\n",U_Kd_gain); + break; + } + case KP_LOWER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + L_Kc=Int+Float; + //printf("Kp Lower : %f\r\n",L_Kc); + break; + } + case KI_LOWER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + L_Ki_gain=Int+Float; + L_Ti=L_Ki_gain/L_Kc; + //printf("Ki Lower : %f\r\n",L_Ki_gain); + break; + } + case KD_LOWER_MOTOR: { + uint8_t int_buffer[2]; + uint8_t float_buffer[2]; + float Int,Float; + int_buffer[0]=command[1]; + int_buffer[1]=command[2]; + float_buffer[0]=command[3]; + float_buffer[1]=command[4]; + Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer); + Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000; + L_Kd_gain=Int+Float; + L_Td=L_Kd_gain/L_Kc; + //printf("Kd Lower : %f\r\n",L_Kd_gain); + break; + } + case HEIGHT: { + int i; + for(i=0; i<4; i++) { + Height[0+i]=command[1+i]; + } + break; + } + case WHEELPOS: { + int i; + for(i=0; i<4; i++) { + Wheelpos[0+i]=command[1+i]; + } + break; + } + case MAG_DATA: { + int i; + for(i=0; i<24; i++) { + Mag[0+i]=command[1+i]; + } + break; + } + case OFFSET: { + int i; + for(i=0; i<8; i++) { + Offset[0+i]=command[1+i]; + } + break; + } + case BODY_WIDTH: { + int i; + for(i=0; i<4; i++) { + Body_width[0+i]=command[1+i]; + } + break; + } + case ANGLE_RANGE_UP: { + int i; + for(i=0; i<8; i++) { + Angle_Range_Up[i]=command[1+i]; + //printf("%d Angle = 0x%02x\r\n",i,Angle_Range_Up[i]); + } + break; + } + case ANGLE_RANGE_LOW: { + int i; + for(i=0; i<8; i++) { + Angle_Range_Low[0+i]=command[1+i]; + } + break; + } + + case UP_LINK_LENGTH: { + int i; + for(i=0; i<4; i++) { + UpLinkLength[i]=command[1+i]; + } + break; + } + case LOW_LINK_LENGTH: { + int i; + for(i=0; i<4; i++) { + LowLinkLength[i]=command[1+i]; + } + break; + } + // unfinish yet!!!!!!!!!!!!!!!!! + case SAVE_EEPROM_DATA: { + if(id==0x01) { + + if (command[1]==HEIGHT) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(Height); + memory.write(ADDRESS_HEIGHT,data_buff); + 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(EEPROM_DELAY); + + } else if(command[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); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_OFFSET+4,z_data_buffer); + wait_ms(EEPROM_DELAY); + + } else if(command[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); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_MAG_DATA+4,x_min_buffer); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_MAG_DATA+8,y_max_buffer); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_MAG_DATA+12,y_min_buffer); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_MAG_DATA+16,z_max_buffer); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_MAG_DATA+20,z_min_buffer); + wait_ms(EEPROM_DELAY); + + } + + } + // else { + if (command[1]==ID) { + memory.write(ADDRESS_ID,id); + 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(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(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(EEPROM_DELAY); + memory.write(ADDRESS_UPPER_KI,U_Ki_gain); + //printf("U_Write : %f\r\n",U_Ki_gain); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_UPPER_KD,U_Kd_gain); + //printf("U_Write : %f\r\n",U_Kd_gain); + 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(EEPROM_DELAY); + memory.write(ADDRESS_LOWER_KI,L_Ki_gain); + //printf("L_Write : %f\r\n",L_Ki_gain); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_LOWER_KD,L_Kd_gain); + //printf("L_Write : %f\r\n",L_Kd_gain); + wait_ms(EEPROM_DELAY); + + } else if (command[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); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer); + wait_ms(EEPROM_DELAY); + + } else if (command[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); + wait_ms(EEPROM_DELAY); + memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer); + 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(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(EEPROM_DELAY); + + } else if (command[1]==WHEELPOS) { + int32_t data_buff; + data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos); + memory.write(ADDRESS_WHEELPOS,data_buff); + wait_ms(EEPROM_DELAY); + } + break; + } + break; + } + break; + } + case READ_DATA: { + switch (command[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_gain); + memory.read(ADDRESS_UPPER_KD,U_Kd_gain); + com.sendUpMotorKpKiKd(MY_ID,U_Kc,U_Ki_gain,U_Kd_gain); + /* + printf("After read Kp : %f\r\n",U_Kc); + printf("After read Ki : %f\r\n",U_Ki_gain); + printf("After read Kd : %f\r\n",U_Kd_gain); + */ + break; + } + case PID_LOWER_MOTOR: { + memory.read(ADDRESS_LOWER_KP,L_Kc); + memory.read(ADDRESS_LOWER_KI,L_Ki_gain); + memory.read(ADDRESS_LOWER_KD,L_Kd_gain); + com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki_gain,L_Kd_gain); + /* + printf("After read L_Kp : %f\r\n",L_Kc); + printf("After read L_Ki : %f\r\n",L_Ki_gain); + printf("After read L_Kd : %f\r\n",L_Kd_gain); + */ + 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); + +} +/*******************************************************/ +void Rc() +{ + myled =1; + uint8_t data_array[30]; + uint8_t id=0; + uint8_t ins=0; + uint8_t status=0xFF; + + + + status = com.ReceiveCommand(&id,data_array,&ins); + //PC.printf("status = 0x%02x\n\r",status); + if(status == ANDANTE_ERRBIT_NONE) { + CmdCheck((int16_t)id,data_array,ins); + } + +} +/*******************************************************/ +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(); + + SET_UpperPID(); + SET_LowerPID(); + + printf("BEAR MOTION\n\r"); + while(1) { + myled =0; + //wait_ms(10); +///////////////////////////////////////////////////// start + //Set Set_point + Up_PID.setSetPoint(Upper_SetPoint); + Low_PID.setSetPoint(Lower_SetPoint); + + Read_Encoder(EncoderB); + Lower_Position = Get_EnValue(data); + PC.printf("position = %2f\n",Lower_Position); + //Control Motor + //Move_Upper(); + //Move_Lower(); +///////////////////////////////////////////////////// stop =306us + //uint8_t aaa[1]={10}; + //com.sendBodyWidth(0x01,aaa); + Rc(); + //wait_ms(1); + } +} + + + + + +