Secret_Betago
first
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
BEAR_Motion
by 
BE@R lab
Diff: main.cpp
- Revision:
- 36:1561b6d61095
- Parent:
- 33:65cfa4b72195
- Parent:
- 34:0cf04acfe422
--- a/main.cpp Wed Feb 03 14:45:43 2016 +0000
+++ b/main.cpp Wed Feb 03 14:49:46 2016 +0000
@@ -2,28 +2,34 @@
// Include //
#include "mbed.h"
#include "pinconfig.h"
-#include "PID.h"
+//#include "PID.h"
#include "Motor.h"
#include "eeprom.h"
#include "Receiver.h"
#include "Motion_EEPROM_Address.h"
+
+#include "pidcontrol.h"
+
#define EEPROM_DELAY 2
+
+//#define DEBUG_UP
+//#define DEBUG_LOW
//*****************************************************/
//--PID parameter--
//-Upper-//
-float U_Kc;
-float U_Ki_gain;
-float U_Kd_gain;
-float U_Ti;
-float U_Td;
+float U_Kc=0.2;
+float U_Ki_gain=0.0;
+float U_Kd_gain=0.0;
+float U_Ti=0.0;
+float U_Td=0.0;
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_Kc=0.2;
+float L_Ki_gain=0.0;
+float L_Kd_gain=0.0;
+float L_Ti=0.0;
+float L_Td=0.0;
float L_Ki=L_Kc*L_Ti;
float L_Kd=L_Kc*L_Td;
//*****************************************************/
@@ -35,8 +41,8 @@
int16_t MY_ID = 0x01;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
-uint8_t UpMargin[4];
-uint8_t LowMargin[4];
+float UpMargin;
+float LowMargin;
uint8_t Height[4];
uint8_t Wheelpos[4];
uint8_t Mag[24];
@@ -59,11 +65,20 @@
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);
+int Upper_SetPoint=20;
+int Lower_SetPoint=8;
+//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);
+
+PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
+PID Low_PID(L_Kc, L_Ti, L_Td);
+
//*****************************************************/
+void Start_Up();
+void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);
+
+Timer counterUP;
+Timer counterLOW;
DigitalOut myled(LED1);
@@ -102,17 +117,15 @@
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
- {
+ if ( MY_ID == 0x01 ) { //when it was left side
while(Val != codes[i]) {
i++;
- }
+ }
}
- if ( MY_ID == 0x02 )//when it was right side
- {
+ if ( MY_ID == 0x02 ) { //when it was right side
while(Val != codes[127-i]) {
i++;
- }
+ }
}
return i;
@@ -121,52 +134,198 @@
void SET_UpperPID()
{
Upper.period(0.001);
- Up_PID.setMode(0);
- Up_PID.setInputLimits(0,127);
- Up_PID.setOutputLimits(0,1);
+
+ memory.read(ADDRESS_UP_MARGIN,UpMargin);
+ Up_PID.setMargin(UpMargin);
+
+ memory.read(ADDRESS_UPPER_KP,U_Kc);
+ Up_PID.setKp(U_Kc);
+ memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
+ Up_PID.setKi(U_Ki_gain);
+ memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
+ Up_PID.setKd(U_Kd_gain);
+
+ //Up_PID.setMode(0);
+ //Up_PID.setInputLimits(18,62);
+ //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);
+
+ memory.read(ADDRESS_LOW_MARGIN,LowMargin);
+ Low_PID.setMargin(LowMargin);
+
+ memory.read(ADDRESS_LOWER_KP,L_Kc);
+ Low_PID.setKp(L_Kc);
+ memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
+ Low_PID.setKi(L_Ki_gain);
+
+ memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
+ Low_PID.setKd(L_Kd_gain);
+
+ //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);
+ Upper_Position = (float)Get_EnValue(data);
+#ifdef DEBUG_UP
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Upper_SetPoint);
+ printf("Upper_Position = %f\n\r",Upper_Position);
+#endif
+ Up_PID.setCurrent(Upper_Position);
+ float speed =Up_PID.compute();
+#ifdef DEBUG_UP
+ printf("E_n= %f\n\r",Up_PID.getErrorNow());
+ printf("Kp = %f\n\r",Up_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Upper.speed(speed);
}
//******************************************************/
void Move_Lower()
{
Read_Encoder(EncoderB);
- Lower_Position = Get_EnValue(data);
-
- Low_PID.setProcessValue(Lower_Position);
+ Lower_Position = (float)Get_EnValue(data);
+ Low_PID.setCurrent(Lower_Position);
+#ifdef DEBUG_LOW
+ printf("read_encode = 0x%2x \n\r",data);
+ printf("Setpoint = %d\n\r",Lower_SetPoint);
+ printf("Upper_Position = %f\n\r",Lower_Position);
+#endif
- if(Lower_Position - Lower_SetPoint > 0 ) {
- dir = 1;
- }
- if(Lower_Position - Lower_SetPoint < 0 ) {
- dir = -1;
- }
- Lower.speed(Low_PID.compute() * dir);
+ float speed =Low_PID.compute();
+#ifdef DEBUG_LOW
+ printf("E_n= %f\n\r",Low_PID.getErrorNow());
+ printf("Kp = %f\n\r",Low_PID.getKp());
+ printf("speed = %f\n\n\n\r",speed);
+#endif
+ Lower.speed(speed);
}
//******************************************************/
+
+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 ID:0x%02x\n\r",MY_ID);
+ /*
+ while(1)
+ {
+
+ Upper.speed(-1);
+ wait_ms(400);
+ Upper.speed(1);
+ wait_ms(400);
+ Upper.break();
+
+ Lower.speed(-1.0);
+ wait_ms(401);
+ Lower.speed(1.0);
+ wait_ms(401);
+ Lower.break();
+ }
+ */
+
+ // counterUP.start();
+// counterLOW.start();
+
+ while(1) {
+
+ /*
+ //printf("timer = %d\n\r",counter.read_ms());
+ if(counterUP.read_ms() > 1400) {
+
+ if(Upper_SetPoint < 53) {
+ Upper_SetPoint++;
+ } else
+ Upper_SetPoint =18;
+
+ counterUP.reset();
+
+ }
+
+ if(counterLOW.read_ms() > 700) {
+
+ if(Lower_SetPoint < 100) {
+ Lower_SetPoint++;
+ } else
+ Lower_SetPoint =8;
+
+ counterLOW.reset();
+
+ }
+ */
+ myled =0;
+ //wait_ms(10);
+///////////////////////////////////////////////////// start
+ //Set Set_point
+ Up_PID.setGoal(Upper_SetPoint);
+ Low_PID.setGoal(Lower_SetPoint);
+
+ //Control Motor
+ Move_Upper();
+ Move_Lower();
+///////////////////////////////////////////////////// stop =306us
+ //uint8_t aaa[1]={10};
+ //com.sendBodyWidth(0x01,aaa);
+ Rc();
+ //wait_ms(1);
+ }
+}
+
+
+
+
+
+
+
+
+
void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
if(id==MY_ID) {
@@ -187,27 +346,42 @@
IntUpAngle[0]=command[1];
IntUpAngle[1]=command[2];
- Upper_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
+ Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(IntUpAngle);
//printf("Up Angle = %f\n",up_angle);
IntLowAngle[0]=command[5];
IntLowAngle[1]=command[6];
- Lower_Position=(float)Utilities::ConvertUInt8ArrayToInt16(IntLowAngle);
+ Lower_SetPoint=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]);
- }
+ 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;
+ UpMargin=Int+Float;
+ Up_PID.setMargin(UpMargin);
+ //printf("Kp Upper : %f\r\n",U_Kc);
break;
}
case LOW_MARGIN: {
- int i;
- for(i=0; i<4; i++) {
- LowMargin[i]=command[1+i];
- }
+ 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;
+ LowMargin=Int+Float;
+ Low_PID.setMargin(LowMargin);
+ //printf("Kp Upper : %f\r\n",U_Kc);
break;
}
case KP_UPPER_MOTOR: {
@@ -222,6 +396,7 @@
Float=(float)Utilities::ConvertUInt8ArrayToInt16(float_buffer)/10000;
U_Kc=Int+Float;
//printf("Kp Upper : %f\r\n",U_Kc);
+ Up_PID.setKp(U_Kc);
break;
}
case KI_UPPER_MOTOR: {
@@ -235,7 +410,8 @@
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;
+ //U_Ti=U_Ki_gain;
+ Up_PID.setKi(U_Ki_gain);
//printf("Ki Upper : %f\r\n",U_Ki_gain);
break;
}
@@ -250,7 +426,8 @@
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;
+ Up_PID.setKd(U_Kd_gain);
+ //U_Td=U_Kd_gain/U_Kc;
//printf("Kd Upper : %f\r\n",U_Kd_gain);
break;
}
@@ -438,16 +615,12 @@
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);
+ memory.write(ADDRESS_UP_MARGIN,UpMargin);
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);
+ memory.write(ADDRESS_LOW_MARGIN,LowMargin);
wait_ms(EEPROM_DELAY);
} else if (command[1]==PID_UPPER_MOTOR) {
@@ -533,16 +706,12 @@
break;
}
case UP_MARGIN: {
- int32_t data_buff;
- memory.read(ADDRESS_UP_MARGIN,data_buff);
- Utilities::ConvertInt32ToUInt8Array(data_buff,UpMargin);
+ memory.read(ADDRESS_UP_MARGIN,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);
+ memory.read(ADDRESS_LOW_MARGIN,LowMargin);
com.sendLowMargin(MY_ID,LowMargin);
break;
}
@@ -688,87 +857,3 @@
}
}
-
-/******************************************************/
-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);
- }
-}
-
-
-
-
-
-