FRA221_2015
section B 57340500043 57340500046
Dependencies: Motor PID eeprom mbed
Fork of
Tanginamo
by 
Digital B14
main.cpp
- Committer:
- ParinyaT
- Date:
- 2015-12-09
- Revision:
- 5:4168693bfb80
- Parent:
- 4:6e29193d7f95
File content as of revision 5:4168693bfb80:
//*****************************************************/
// Include //
#include "mbed.h"
#include "pinconfig.h"
#include "PID.h"
#include "Motor.h"
#include "eeprom.h"
//*****************************************************/
// 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 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;
//-- Packet --
int Start1,Start2,ID,Lenght,inst,GOAL,Pos;
int8_t Check,Checksum;
//*****************************************************/
void Read_Encoder()
{
SPI device(Emosi, Emiso, Esck);
device.format(8,0);
device.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k
Encoder = 0;
wait_us(50);
device.write(0x41);
wait_us(50);
device.write(0x09);
wait_us(50);
ENdata = device.write(0x00);
wait_us(50);
Encoder = 1;
}
//*****************************************************/
void 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
};
while(i<=127) {
if(Val == codes[i]) {
Position = i;
break;
} else i++;
}
}
//******************************************************************************************
void eewrite()
{
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(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);
}
}
//*****************************************************/
int main()
{
uint8_t state_menu=0;
uint8_t state_show=0;
uint8_t state_exit =0;
uint8_t data;
// myled = 0;
PC.printf("Wellcome !\n");
while(1) {
if(state_show == 0) {
PC.printf("Menu\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()) {
data = PC.getc();
PC.printf("\n");
state_show =0;
state_exit =0;
switch(data) {
case '1':
do {
if(state_menu == 0) {
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()) {
data = PC.getc();
PC.printf("\n");
state_menu=0;
switch(data) {
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 '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;
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 '3':
do {
if(state_menu == 0) {
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.2,1);
LU_PID.setMode(AUTO_MODE);
//get the target position
//SetPoint = 64;
LU_PID.setSetPoint(SetPoint);
Read_Encoder();
//PC.printf("%d\n",ENdata);
Get_EnValue(ENdata);
//PC.printf("%d\n********************\n",Position);
while(abs(Position-SetPoint)>1) {
LU_PID.setProcessValue(Position);
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(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;
}
}
}
} while(state_exit ==0);
PC.printf("\n\n");
break;
case '2':
do {
if(state_menu == 0) {
PC.printf("Mode SetPoint\n");
PC.printf("Waiting for command\n");
state_menu = 1;
}
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;
} while(state_exit ==0);
PC.printf("\n\n");
break;
case 0x00:
break;
default:
PC.printf("plz select 1 or 2 or 3 only\n");
PC.printf("\n\n");
break;
}
}
}
}