main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "encoder.h"//#include "Encoder.h"//encoder
00004 #include "pid.h"//position_and_velocity_PID
00005 
00006 #define t_pid 0.01 //fixed
00007 #define Kp 0.0
00008 #define Ki 0.0005
00009 #define Kd 0.0
00010 #define mini_duty 0.0
00011 #define max_duty 0.95
00012 #define pc_tx       USBTX
00013 #define pc_rx       USBRX
00014 #define pc_baud     460800
00015 
00016 Encoder encoder_vibrate(D11, D12);
00017 Encoder encoder_outaxis(D13, D14);
00018 DigitalOut m_dir(D2);
00019 PwmOut m_duty(D3);
00020 Serial pc(USBTX,USBRX,460800);
00021 SpeedPid speedPid;
00022 Ticker calc_pid;
00023 Ticker hz;
00024 
00025 void debug();
00026 void duty();
00027 void accelerate();
00028 int num_pidticker=0;
00029 float rpm_target = 6*60;
00030 float sec=0;
00031 bool renew = false;
00032 
00033 /*************
00034 setup_function
00035 *************/
00036 void setup() {
00037     m_duty.period(0.00005);
00038     encoder_vibrate.init(200,0,t_pid,X4_ENCODE);
00039     encoder_outaxis.init(2048,0,t_pid,X4_ENCODE);
00040     speedPid.init(Kp,Ki,Kd,t_pid);
00041     calc_pid.attach(&duty, t_pid);
00042     //hz.attach(&accelerate, 1);
00043 }
00044 
00045 /**************
00046 output_function
00047 **************/
00048 void duty(){
00049     float refe = rpm_target;
00050     encoder_vibrate.calculate();
00051     encoder_outaxis.calculate();
00052     speedPid.cal(refe,encoder_vibrate.getState(RPM));
00053     float pwm = speedPid.getState(OUTPUT);
00054     m_dir = 1;
00055     
00056     if(pwm >= max_duty) m_duty = max_duty;
00057     else if(pwm > mini_duty) m_duty = pwm;
00058     else m_duty = 0;
00059     
00060     num_pidticker++;
00061 }
00062 
00063 void accelerate(){
00064     rpm_target = rpm_target + 150;
00065     if(rpm_target > 1500) renew = true;
00066 }
00067 
00068 int main() {
00069     setup();
00070     while(1) {
00071         debug();
00072         wait(0.05);
00073     }
00074 }
00075 
00076 void debug(){
00077         sec = num_pidticker*t_pid;
00078         pc.printf("%5.2f,%5.2f,%5.2f,%5.2f\r\n",sec,rpm_target,encoder_vibrate.getState(RPM),encoder_outaxis.getState(RPM));
00079 }