main.cpp

00001 #include "mbed.h"//LPC1768でパルス取得
00002 #include "QEI.h"
00003 #include "math.h"
00004 #define Z_A 16
00005 #define Z_B 35
00006 #define ZZ 2000
00007 #define REAL 54
00008 #define LIMIT 0.5
00009 Serial pc(USBTX,USBRX,115200);
00010 
00011 DigitalOut dir(p21);
00012 PwmOut pwm(p22);
00013 Ticker Diff;
00014 Timer t;
00015 QEI enc(p27, p28, NC, 500, &t, QEI::X4_ENCODING);//エンコーダのライブラリ
00016 DigitalIn SW(p20);
00017 DigitalOut RLED(p19);
00018 DigitalOut YLED(p17);
00019 DigitalOut GLED(p15);
00020 double angle,radspeed,pre_dist,dist,vel;
00021 double ref_vel = 0.0;// 目標速度(Reference Velocity) mm/s
00022 double ref_dist = 0.20;// 目標位置(Reference Ddist) m
00023 
00024 double Kpv = 0.5;//速度制御の比例ゲイン
00025 double Kiv = 0.0;//速度制御の積分ゲイン
00026 double Kdv = 0.0;//速度制御の微分ゲイン
00027 
00028 double Kpd = 5.0;//位置制御の比例ゲイン
00029 double Kid = 1.0;//位置制御の積分ゲイン
00030 double Kdd = 0.0;//位置制御の微分ゲイン
00031 double Int_dist_error = 0;
00032 double Int_vel_error = 0;
00033 int encount;
00034 double cmd = 0;
00035 int k;
00036 
00037 void diff(){
00038 
00039     encount = enc.getPulses();
00040     dist=(double)encount*REAL*Z_A/(Z_B*ZZ*1000);//[m]
00041     vel=(dist-pre_dist)/0.01;
00042     pre_dist=dist;
00043 
00044 
00045     double dist_error = ref_dist - dist;
00046     static double pre_dist_error = dist_error;
00047     
00048     Int_dist_error += (dist_error+pre_dist_error)*0.01/2.0;
00049     ref_vel = dist_error*Kpd +Int_dist_error*Kid + (dist_error - pre_dist_error)/0.01 * Kdd;
00050     
00051     double vel_error = ref_vel-vel;
00052     static double pre_vel_error = vel_error;
00053     
00054     Int_vel_error += (vel_error+pre_vel_error)*0.01/2.0;
00055     cmd += vel_error*Kpv +Int_vel_error*Kiv + (vel_error - pre_vel_error)/0.01 * Kdv;
00056     pre_dist_error = dist_error;    
00057     
00058     if(cmd > LIMIT) cmd = LIMIT;
00059     else if(cmd < -LIMIT) cmd = -LIMIT;
00060 
00061     if(cmd >= 0) {
00062         dir.write(1);//モータ回転方向指定
00063         k=1;
00064     } else {
00065         dir.write(0);
00066         k=0;
00067     }
00068 
00069     pwm.write(fabs(cmd));
00070     GLED=1.0;
00071    
00072 }
00073 
00074 int main()
00075 {
00076     pwm.period_us(50);
00077     int sw;
00078     do {
00079         sw=SW.read();
00080         dir=0.0;
00081         pwm.write(0.3);
00082 
00083     } while(sw!=1);
00084 
00085     pwm.write(0.0);
00086 
00087     wait(3.0);
00088     enc.qei_reset();
00089     Diff.attach(&diff,0.01);
00090     while(1) {
00091  pc.printf("dist:%lf ref_vel:%lf\r\n",dist,ref_vel);
00092     }
00093 }