BE@R lab
โปรแกรมของบอร์ด Motion
Dependencies: BEAR_Reciever Motor eeprom iSerial mbed
Fork of
DogPID
by 
Digital B14
Revision 36:1561b6d61095, committed 2016-02-03
- Comitter:
- soulx
- Date:
- Wed Feb 03 14:49:46 2016 +0000
- Parent:
- 35:7fa769563e61
- Parent:
- 34:0cf04acfe422
- Commit message:
- ???code
Changed in this revision
diff -r 7fa769563e61 -r 1561b6d61095 PID.lib --- a/PID.lib Wed Feb 03 14:45:43 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/teams/BEaR-lab/code/PID/#807a17d0a172
diff -r 7fa769563e61 -r 1561b6d61095 main.cpp
--- 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);
- }
-}
-
-
-
-
-
-
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);
+ }
+}
+
+
+
+
+
+
diff -r 7fa769563e61 -r 1561b6d61095 pidcontrol.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.cpp Wed Feb 03 14:49:46 2016 +0000
@@ -0,0 +1,168 @@
+#include "pidcontrol.h"
+
+PID::PID()
+{
+ Kp=1.0f;
+ Ki=0.0f;
+ Kd=0.0f;
+ il=65535.0;
+ margin = 0.0f;
+
+}
+
+PID::PID(float p,float i,float d)
+{
+ Kp=p;
+ Ki=i;
+ Kd=d;
+ il=65535.0;
+ margin =0.0f;
+}
+
+void PID::setGoal(float ref)
+{
+ setpoint = ref;
+}
+
+void PID::setCurrent(float sensor)
+{
+ input = sensor;
+}
+
+float PID::compute()
+{
+
+ e_n = setpoint - input;
+
+ if((e_i < il) && (e_i > -il))
+ {
+ e_i += e_n;
+ }
+ else
+ {
+#ifdef PID_DEBUG
+ printf("il overflow\n\r");
+#endif
+ e_i =il;
+ }
+
+
+ output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));
+
+ if(output > 0)
+ {
+ if(output < margin)
+ {
+ output = 0.0;
+ }
+ }
+ else
+ {
+ if(output > -margin)
+ {
+ output = 0.0;
+ }
+ }
+
+ return output;
+}
+
+void PID::setMargin(float gap)
+{
+ margin =gap;
+}
+
+float PID::getMargin()
+{
+ return margin;
+}
+
+
+void PID::setIntegalLimit(float limit)
+{
+ il = limit;
+}
+float PID::getIntegalLimit()
+{
+ return il;
+}
+
+
+float PID::getErrorNow()
+{
+ return e_n;
+}
+
+float PID::getErrorLast()
+{
+ return e_n_1;
+}
+
+float PID::getErrorDiff()
+{
+ return e_n - e_n_1;
+}
+
+float PID::getErrorIntegal()
+{
+ return e_i;
+}
+
+void PID::setKp(float p)
+{
+ if(p < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Kp = 0.0\n\r");
+#endif
+ Kp=0.0;
+ }
+ else
+ {
+ Kp=p;
+ }
+}
+
+void PID::setKi(float i)
+{
+ if(i < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Ki = 0.0\n\r");
+#endif
+ Ki=0.0;
+ }
+ else
+ {
+ Ki=i;
+ }
+}
+void PID::setKd(float d)
+{
+ if(d < 0.0f)
+ {
+#ifdef PID_DEBUG
+ printf("Kd = 0.0\n\r");
+#endif
+ Kd=0.0;
+ }
+ else
+ {
+ Kd=d;
+ }
+}
+
+float PID::getKp()
+{
+ return Kp;
+}
+
+float PID::getKi()
+{
+ return Ki;
+}
+
+float PID::getKd()
+{
+ return Kd;
+}
\ No newline at end of file
diff -r 7fa769563e61 -r 1561b6d61095 pidcontrol.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pidcontrol.h Wed Feb 03 14:49:46 2016 +0000
@@ -0,0 +1,49 @@
+#ifndef _PIDCONTROL_H_
+#define _PIDCONTROL_H_
+
+#include "mbed.h"
+
+class PID{
+ public:
+ PID();
+ PID(float p,float i,float d);
+ void setGoal(float ref);
+ //float getGoal();
+ void setCurrent(float sensor);
+ float compute();
+
+ void setMargin(float gap);
+ float getMargin();
+ void setIntegalLimit(float limit);
+ float getIntegalLimit();
+
+ float getErrorNow();
+ float getErrorLast();
+ float getErrorDiff();
+ float getErrorIntegal();
+
+ void setKp(float);
+ void setKi(float);
+ void setKd(float);
+
+ float getKp();
+ float getKi();
+ float getKd();
+
+ private:
+ float e_n; //error now
+ float e_n_1; //error last time
+ float e_i; //error integal
+ float il; //integal limit
+ float margin; //output margin
+
+ float Kp,Ki,Kd;
+
+ float setpoint;
+ float input;
+ float output;
+};
+
+
+
+#endif
\ No newline at end of file