FRA221_2015
section B 57340500043 57340500046
Dependencies: Motor PID eeprom mbed
Fork of
Tanginamo
by 
Digital B14
Diff: main.cpp
- Revision:
- 5:4168693bfb80
- Parent:
- 4:6e29193d7f95
diff -r 6e29193d7f95 -r 4168693bfb80 main.cpp
--- a/main.cpp Mon Dec 07 21:32:46 2015 +0000
+++ b/main.cpp Wed Dec 09 00:01:34 2015 +0000
@@ -7,39 +7,32 @@
#include "eeprom.h"
//*****************************************************/
-// Defines //
-#define Rate 0.01
-#define Kc -2.6
-#define Ti 0.0
-#define Td 0.0
-
+// PID parameter //
+float Rate = 0.001;
+float Kc = 0.4;
+float Ti = 0.15;
+float Td = 0.0;
+float Ki;
+float Kd;
//*****************************************************/
// Global //
//-- pc monitor --
Serial PC(D1,D0);
//-- encoder --
int Position;
-int data;
-
-
-//*******************************************************
-
-
-//write encoder
-EEPROM memory(I2C_SDA,I2C_SCL,0);
-int8_t out,write;
-int mod;
-int FromSerial;
-
-
-//*******************************************************
+int ENdata;
SPI device(Emosi, Emiso, Esck);
DigitalOut Encoder(EncoderA);
//-- Motor --
+int dir;
Motor LeftUpper(PWM_LU,A_LU,B_LU);
+//-- Memory --
+EEPROM memory(PB_4,PA_8,0);
//-- PID --
int SetPoint;
-PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate
+//-- Packet --
+int Start1,Start2,ID,Lenght,inst,GOAL,Pos;
+int8_t Check,Checksum;
//*****************************************************/
void Read_Encoder()
{
@@ -53,7 +46,7 @@
wait_us(50);
device.write(0x09);
wait_us(50);
- data = device.write(0x00);
+ ENdata = device.write(0x00);
wait_us(50);
Encoder = 1;
@@ -83,37 +76,37 @@
//******************************************************************************************
void eewrite()
{
- write = Position;
- memory.write(0xFF,write);
- PC.printf("%x\n",write);
- //PC.printf("write %x Complete",write);
-
+ memory.write(0x11,Kc);
+ wait_ms(10);
+ memory.write(0x22,Ki);
+ wait_ms(10);
+ memory.write(0x33,Kd);
+
wait_ms(10);
}
//******************************************************************************************
void eeread()
{
- memory.read(0xFF,out);
- PC.printf("%x\n",out);
- //PC.printf("Read %x Complete",out);
+ memory.read(0x11,Kc);
+ wait_ms(10);
+ memory.read(0x22,Ki);
+ wait_ms(10);
+ memory.read(0x33,Kd);
wait_ms(10);
}
//******************************************************************************************
void StartWriteRead(int mod)
{
-
- if(mod==1)
- {
- eewrite();
- wait_ms(10);
- }
- else if(mod==0)
- {
- eeread();
- wait_ms(10);
- }
-
+
+ if(mod==1) {
+ eewrite();
+ wait_ms(10);
+ } else if(mod==0) {
+ eeread();
+ wait_ms(10);
+ }
+
}
//*****************************************************/
int main()
@@ -129,9 +122,9 @@
while(1) {
if(state_show == 0) {
PC.printf("Menu\n");
- PC.printf("1.Mode Write Or Read \n");
- PC.printf("2.Mode POP\n");
- PC.printf("3.Mode Set Encode\n");
+ PC.printf("1.Set PID Parameter \n");
+ PC.printf("2.Set Position \n");
+ PC.printf("3.PID Test \n");
state_show =1;
}
if(PC.readable()) {
@@ -143,11 +136,15 @@
switch(data) {
case '1':
do {
+
if(state_menu == 0) {
- PC.printf("Mode Write Or Read\n");
- PC.printf("a.Mode Write \n");
- PC.printf("d.Mode Read \n");
- PC.printf("s.Stop \n");
+ PC.printf("Set PID Parameter\n");
+ PC.printf("Kp : %f\n", Kc);
+ PC.printf("Ki : %f\n", Kc*Ti);
+ PC.printf("Kd : %f\n", Kc*Td);
+ PC.printf("1.New setting\n");
+ PC.printf("2.Load setting\n");
+ PC.printf("3.Exit\n");
state_menu = 1;
}
if(PC.readable()) {
@@ -157,116 +154,129 @@
switch(data) {
- case 'a':
- StartWriteRead(1);
- break;
-
- case 's':
- state_exit = 1;
-
+ case '1':
+ PC.printf("Set Kp : ");
+ PC.scanf("%f", &Kc);
+ PC.printf("\nSet Ki : ");
+ PC.scanf("%f", &Ki);
+ PC.printf("\nSet Kd : ");
+ PC.scanf("%f", &Kd);
+
+ Ti = Ki/Kc;
+ Td = Kd/Kc;
+
+ eewrite();
+
break;
- case 'd':
- StartWriteRead(0);
+ case '2':
+ eeread();
+
+ /*PC.printf("Set Kp : %f\n",Kc);
+ PC.printf("Set Ki : %f\n",Ki);
+ PC.printf("Set Kd : %f\n",Kd);*/
+
+ Ti = Ki/Kc;
+ Td = Kd/Kc;
+
break;
- default:
- PC.printf("plz select a or d\n");
- PC.printf("\n\n");
+ case '3':
+ state_exit = 1;
break;
+ //default:
+ //PC.printf("plz select 1,2,3\n");
+ //PC.printf("\n\n");
+ //break;
+
}
}
-
-
-
} while(state_exit ==0);
PC.printf("\n\n");
break;
- case '2':
+ case '3':
do {
if(state_menu == 0) {
- PC.printf("Mode POP\n\n");
+ PC.printf("Mode POP\n");
+ PC.printf("press x to exit\n\n");
+ PID LU_PID(Kc, Ti, Td, Rate);//Kp,Ki,Kd,Rate
//***start mode
- {
+ wait (2);
+ while(1){
+
LeftUpper.period(0.00005);
LU_PID.setInputLimits(0,127);
- LU_PID.setOutputLimits(0,0.9);
+ LU_PID.setOutputLimits(0.2,1);
LU_PID.setMode(AUTO_MODE);
//get the target position
- SetPoint = 63;
+ //SetPoint = 64;
LU_PID.setSetPoint(SetPoint);
Read_Encoder();
- PC.printf("%d\n",data);
- Get_EnValue(data);
- PC.printf("%d\n********************\n",Position);
+ //PC.printf("%d\n",ENdata);
+ Get_EnValue(ENdata);
+ //PC.printf("%d\n********************\n",Position);
- while(Position != SetPoint) {
+ while(abs(Position-SetPoint)>1) {
LU_PID.setProcessValue(Position);
- LeftUpper.speed(LU_PID.compute());
+ if( Position - SetPoint > 0 ) {
+ dir = 1;
+ }if(Position - SetPoint < 0 ) {
+ dir = -1;
+ }
+
+
+ LeftUpper.speed(LU_PID.compute() * dir);
+ wait_ms(17);
Read_Encoder();
- Get_EnValue(data);
+ Get_EnValue(ENdata);
+ //PC.printf("%d\n********************\n",Position);
+ LeftUpper.brake(0);
+
+ }LeftUpper.brake(1);
+ if(PC.readable()) {
+ data = PC.getc();
+ if (data == 'x') {
+ state_exit = 1;
+ break;
+ }else state_menu = 0;
}
- }
- PC.printf("press x to exit or else to continue\n");
- state_menu = 1;
+
}
- if(PC.readable()) {
- data = PC.getc();
- if (data == 'x') state_exit = 1;
- else state_menu = 0;
- }
-
+
+ }
+ } while(state_exit ==0);
+ PC.printf("\n\n");
+ break;
- } while(state_exit ==0);
- PC.printf("\n\n");
- break;
-
- case '3':
- do{
+ case '2':
+ do {
if(state_menu == 0) {
PC.printf("Mode SetPoint\n");
- PC.printf("z.Mode SetPoint \n");
- PC.printf("x.Stop \n");
+ PC.printf("Waiting for command\n");
state_menu = 1;
}
- if(PC.readable()) {
- data = PC.getc();
- PC.printf("\n");
- state_menu=0;
-
- switch(data) {
-
- case 'z':
- PC.printf("Mode SetPoint\n");
- PC.printf("In Put Your Point 0-128 and spacebar\n");
- PC.scanf(" %d",&SetPoint);
- PC.printf("%d",SetPoint);
- while(SetPoint>128)
- {
- PC.printf("Your Number ERROR Plz Input 0-128 and spacebar\n");
- PC.scanf("%d",&SetPoint);
- }
- PC.printf("SETPOINT = %d",SetPoint);
- break;
-
- case 'x':
- state_exit = 1;
- break;
-
- default:
- PC.printf("plz select z or x\n");
- PC.printf("\n\n");
- break;
-
+
+ PC.scanf("%x %x %x %x %x %x %x %x",&Start1,&Start2,&ID,&Lenght,&inst,&GOAL,&Pos,&Checksum);
+ PC.printf("%2x %2x %2x %2x %2x %2x %2x %2x\n",Start1,Start2,ID,Lenght,inst,GOAL,Pos,Checksum);
+ Check = ~(ID+Lenght+inst+GOAL+Pos);
+
+ if(inst == 03){
+ if(GOAL == 01){
+ if(Checksum == Check) {
+ SetPoint=Pos;
+ printf("Position was set to %d\n",Pos);
+ }else printf("Worng Command\n");
}
}
+ state_menu = 0;
+ break;