Secret_Betago
first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
BEAR_Motion
by 
BE@R lab
Diff: main.cpp
- Revision:
- 30:3f8e86fa1413
- Parent:
- 29:5db0a9261161
- Child:
- 31:d6fa5e8e26b3
diff -r 5db0a9261161 -r 3f8e86fa1413 main.cpp
--- a/main.cpp Wed Jan 27 12:52:37 2016 +0000
+++ b/main.cpp Wed Jan 27 21:43:48 2016 +0000
@@ -12,12 +12,16 @@
//--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;
@@ -28,7 +32,7 @@
//-- Communication --
Serial PC(D1,D0);
Bear_Receiver com(Tx,Rx,1000000);
-int16_t MY_ID = 0x01;
+int16_t MY_ID = 0x02;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
uint8_t UpMargin[4];
@@ -156,7 +160,7 @@
}
//******************************************************/
-void CmdCheck(int16_t id,uint8_t *cmd,uint8_t ins)
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
if(id==MY_ID) {
switch (ins) {
@@ -164,10 +168,10 @@
break;
}
case WRITE_DATA: {
- switch (cmd[0]) {
+ switch (command[0]) {
case ID: {
///
- MY_ID = (int16_t)cmd[1];
+ MY_ID = (int16_t)command[1];
break;
}
case MOTOR_UPPER_ANG: {
@@ -175,27 +179,29 @@
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];
+ 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;
-
- IntLowAngle[0]=cmd[5];
- IntLowAngle[1]=cmd[6];
- FloatLowAngle[0]=cmd[7];
- FloatLowAngle[1]=cmd[8];
+ //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;
+ //printf("Low Angle = %f\n",low_angle);
break;
}
case UP_MARGIN: {
int i;
for(i=0; i<4; i++) {
- UpMargin[0+i]=cmd[1+i];
+ UpMargin[i]=command[1+i];
//printf("UPMARGIN[%d]=0x%02x\n\r",i,UpMargin[i]);
}
break;
@@ -203,7 +209,7 @@
case LOW_MARGIN: {
int i;
for(i=0; i<4; i++) {
- LowMargin[0+i]=cmd[1+i];
+ LowMargin[i]=command[1+i];
}
break;
}
@@ -211,130 +217,137 @@
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_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,KI;
- int_buffer[0]=cmd[1];
- int_buffer[1]=cmd[2];
- float_buffer[0]=cmd[3];
- float_buffer[1]=cmd[4];
+ 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;
- KI=Int+Float;
- U_Ti=KI/U_Kc;
+ 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,KD;
- int_buffer[0]=cmd[1];
- int_buffer[1]=cmd[2];
- float_buffer[0]=cmd[3];
- float_buffer[1]=cmd[4];
+ 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;
- KD=Int+Float;
- U_Td=KD/U_Kc;
+ 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]=cmd[1];
- int_buffer[1]=cmd[2];
- float_buffer[0]=cmd[3];
- float_buffer[1]=cmd[4];
+ 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,KI;
- int_buffer[0]=cmd[1];
- int_buffer[1]=cmd[2];
- float_buffer[0]=cmd[3];
- float_buffer[1]=cmd[4];
+ 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;
- KI=Int+Float;
- L_Ti=KI/L_Kc;
+ 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,KD;
- int_buffer[0]=cmd[1];
- int_buffer[1]=cmd[2];
- float_buffer[0]=cmd[3];
- float_buffer[1]=cmd[4];
+ 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;
- KD=Int+Float;
- L_Td=KD/L_Kc;
+ 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]=cmd[1+i];
+ Height[0+i]=command[1+i];
}
break;
}
case WHEELPOS: {
int i;
for(i=0; i<4; i++) {
- Wheelpos[0+i]=cmd[1+i];
+ Wheelpos[0+i]=command[1+i];
}
break;
}
case MAG_DATA: {
int i;
for(i=0; i<24; i++) {
- Mag[0+i]=cmd[1+i];
+ Mag[0+i]=command[1+i];
}
break;
}
case OFFSET: {
int i;
for(i=0; i<8; i++) {
- Offset[0+i]=cmd[1+i];
+ Offset[0+i]=command[1+i];
}
break;
}
case BODY_WIDTH: {
int i;
for(i=0; i<4; i++) {
- Body_width[0+i]=cmd[1+i];
+ Body_width[0+i]=command[1+i];
}
break;
}
case ANGLE_RANGE_UP: {
int i;
for(i=0; i<8; i++) {
- Angle_Range_Up[0+i]=cmd[1+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]=cmd[1+i];
+ Angle_Range_Low[0+i]=command[1+i];
}
break;
}
@@ -342,14 +355,14 @@
case UP_LINK_LENGTH: {
int i;
for(i=0; i<4; i++) {
- UpLinkLength[0+i]=cmd[1+i];
+ UpLinkLength[i]=command[1+i];
}
break;
}
case LOW_LINK_LENGTH: {
int i;
for(i=0; i<4; i++) {
- LowLinkLength[0+i]=cmd[1+i];
+ LowLinkLength[i]=command[1+i];
}
break;
}
@@ -357,19 +370,19 @@
case SAVE_EEPROM_DATA: {
if(id==0x01) {
- if (cmd[1]==HEIGHT) {
+ if (command[1]==HEIGHT) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(Height);
memory.write(ADDRESS_HEIGHT,data_buff);
wait_ms(1);
- } else if(cmd[1]==BODY_WIDTH) {
+ } 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);
- } else if(cmd[1]==OFFSET) {
+ } 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;
@@ -384,7 +397,7 @@
memory.write(ADDRESS_OFFSET+4,z_data_buffer);
wait_ms(1);
- } else if(cmd[1]==MAG_DATA) {
+ } else if(command[1]==MAG_DATA) {
uint8_t x_max_array[4];
uint8_t x_min_array[4];
uint8_t y_max_array[4];
@@ -423,40 +436,46 @@
}
// else {
- if (cmd[1]==ID) {
+ if (command[1]==ID) {
memory.write(ADDRESS_ID,id);
wait_ms(1);
- } else if(cmd[1]==UP_MARGIN) {
+ } 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);
//printf("save OK!!\n\r");
- } else if (cmd[1]==LOW_MARGIN) {
+ } 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);
- } else if (cmd[1]==PID_UPPER_MOTOR) {
+ } 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);
- memory.write(ADDRESS_UPPER_KI,U_Ti);
+ memory.write(ADDRESS_UPPER_KI,U_Ki_gain);
+ //printf("U_Write : %f\r\n",U_Ki_gain);
wait_ms(1);
- memory.write(ADDRESS_UPPER_KD,U_Td);
+ memory.write(ADDRESS_UPPER_KD,U_Kd_gain);
+ //printf("U_Write : %f\r\n",U_Kd_gain);
wait_ms(1);
- } else if (cmd[1]==PID_LOWER_MOTOR) {
+ } 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);
- memory.write(ADDRESS_LOWER_KI,L_Ti);
+ memory.write(ADDRESS_LOWER_KI,L_Ki_gain);
+ //printf("L_Write : %f\r\n",L_Ki_gain);
wait_ms(1);
- memory.write(ADDRESS_LOWER_KD,L_Td);
+ memory.write(ADDRESS_LOWER_KD,L_Kd_gain);
+ //printf("L_Write : %f\r\n",L_Kd_gain);
wait_ms(1);
- } else if (cmd[1]==ANGLE_RANGE_UP) {
+ } 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;
@@ -471,7 +490,7 @@
memory.write(ADDRESS_ANGLE_RANGE_UP+4,min_data_buffer);
wait_ms(1);
- } else if (cmd[1]==ANGLE_RANGE_LOW) {
+ } 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;
@@ -486,19 +505,19 @@
memory.write(ADDRESS_ANGLE_RANGE_LOW+4,min_data_buffer);
wait_ms(1);
- } else if (cmd[1]==UP_LINK_LENGTH) {
+ } 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);
- } else if (cmd[1]==LOW_LINK_LENGTH) {
+ } 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);
- } else if (cmd[1]==WHEELPOS) {
+ } else if (command[1]==WHEELPOS) {
int32_t data_buff;
data_buff = Utilities::ConvertUInt8ArrayToInt32(Wheelpos);
memory.write(ADDRESS_WHEELPOS,data_buff);
@@ -511,18 +530,13 @@
break;
}
case READ_DATA: {
- switch (cmd[0]) {
+ switch (command[0]) {
case MOTOR_UPPER_ANG: {
-
- com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
- uint8_t status;
- status =com.sendMotorPos(MY_ID,Upper_Position,Lower_Position);
+ com.sendMotorPos(MY_ID,up_angle,low_angle);
break;
}
case UP_MARGIN: {
int32_t data_buff;
- com.sendUpMargin(MY_ID,UpMargin);
-
memory.read(ADDRESS_UP_MARGIN,data_buff);
Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin);
com.sendUpMargin(MY_ID,UpMargin);
@@ -537,16 +551,26 @@
}
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);
+ 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);
- memory.read(ADDRESS_LOWER_KD,L_Kd);
- com.sendLowMotorKpKiKd(MY_ID,L_Kc,L_Ki,L_Kd);
+ 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: {