main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "define.h" //ステータス用
00004 
00005 Ticker timer;
00006 Timer T;
00007 
00008 QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00009 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00010 QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00011 
00012 DigitalIn sw1(p26);
00013 DigitalIn sw2(p25);
00014 DigitalIn limit1(p15);
00015 DigitalIn limit2(p16);
00016 DigitalIn sensor1(p18);
00017 DigitalIn sensor2(p17);
00018 
00019 DigitalOut fet1(p21);
00020 DigitalOut fet2(p22);
00021 
00022 Serial Saber(p13,p14);
00023 Serial pc(USBTX,USBRX);
00024 
00025 int Button() {
00026     int button_in = sw1.read();
00027     static int pre_button = 1;
00028     static int sw_state = LOW;
00029     
00030     if(button_in && pre_button)sw_state = HIGH;
00031     if(!button_in && !pre_button)sw_state = LOW;
00032     if(button_in && !pre_button)sw_state = FALL;
00033     if(!button_in && pre_button)sw_state = RISE;
00034     
00035     pre_button = button_in;
00036     
00037     return sw_state;
00038 }
00039 
00040 void timer_warikomi()
00041 {
00042     float encount_ang = 0.0;
00043     float encount_rot = 0.0;
00044     float Ksp = 0.005, Ksd = 0.0015;    //速度係数
00045     float Kp=5.0, Ki=0.01, Kd=0.1;             //角度係数
00046     //float Kp_V=1, Kd_V=0;               //角速度係数
00047     float ppr = 1.0;
00048     static float pre_angle = 0.0;
00049     static float pre_angleE = 0.0;
00050     static float pre_spd = 0.0;
00051     static float pre_spdE = 0.0;
00052     static float ref_angle = 0.0;
00053     static float ref_spd = 0.0;
00054     static float angle_I = 0.0;
00055     static int cmd_spd = 0;
00056     static int cmd_ang = 0;
00057     static int mode = 0;
00058     static int lim_cmdA = 20;
00059     static int lim_cmdS = 127;
00060     static int pre_encount = 0;
00061     
00062     int sw_point = Button();
00063         
00064     int encount2 = Enc2.getPulses();
00065     int encount3 = Enc3.getPulses();
00066     
00067     encount_ang = Enc1.getPulses()- pre_encount;
00068     if (encount2 > encount3) encount_rot = encount2;
00069     else encount_rot = encount3;
00070     
00071     float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0;   
00072     float ang_spd =(angle - pre_angle)/INT_TIME;
00073     
00074     float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; 
00075     float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU);
00076     
00077     float angle_P = (ref_angle - angle);
00078     float angle_D=(angle_P - pre_angleE)/INT_TIME;
00079     angle_I += (angle_P + pre_angleE)*INT_TIME/2.0;
00080     
00081     cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki);
00082     
00083     float spd_e = ref_spd - spd;
00084     float spd_D = (spd_e - pre_spdE)/INT_TIME;
00085     cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd);
00086     
00087     if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA;
00088     if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA;
00089     
00090     if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS;
00091     if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS;
00092     
00093     if (sw_point != HIGH) switch (mode) {
00094         
00095         case 0:
00096             ref_angle = 0;
00097             ref_spd = 0.0;
00098             fet2 = OFF;
00099             if (sw_point == RISE) mode = 1;
00100             break;
00101             
00102         case 1:
00103             cmd_ang = 20;
00104             if (limit1.read()) {
00105                 cmd_ang = 0;
00106                 pre_encount = Enc1.getPulses();
00107                 if (sw_point == RISE) mode = 2;
00108             }
00109             break;
00110         
00111         case 2:
00112             ref_angle = -125;
00113             if (sw_point == RISE) mode = 3;
00114             break;
00115             
00116         case 3:
00117             if (!sensor1 && !sensor2) {
00118                 fet1 = ON;
00119                 if (sw_point == RISE) mode = 4;
00120                 }
00121             break;
00122         
00123         case 4:
00124             ref_angle = 0;
00125                  //if(99<=angle||angle<=101){
00126             if (sw_point == RISE) {
00127                 cmd_ang = 0;
00128                 pre_encount = Enc1.getPulses(); //スイッチを離したところを初期値に
00129                 mode = 5;                       //して無理くり止めてます
00130                 }
00131             break;
00132         
00133         case 5:
00134             fet1 = OFF;
00135             ref_spd = 30.0;
00136             if (sw_point == RISE) mode = 6;
00137             break;
00138         
00139         case 6:
00140             fet2 = ON;
00141             if (sw_point == RISE) mode = 0;
00142             break;
00143         }
00144         
00145     if (!sw2.read()) {
00146         cmd_spd = 0;
00147         cmd_ang = 0;
00148         }
00149         
00150     if (cmd_ang >= 0) {
00151         Saber.putc(SB_ADRS_A);
00152         Saber.putc(1);
00153         Saber.putc(cmd_ang);
00154         Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111);
00155         } 
00156     else {
00157         Saber.putc(SB_ADRS_A);
00158         Saber.putc(0);
00159         Saber.putc(abs(cmd_ang));
00160         Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111);
00161         }
00162         
00163     if (cmd_spd >= 0) {
00164         Saber.putc(SB_ADRS_B);
00165         Saber.putc(1);
00166         Saber.putc(cmd_spd);
00167         Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111);
00168         }
00169     else {
00170         Saber.putc(SB_ADRS_B);
00171         Saber.putc(0);
00172         Saber.putc(abs(cmd_spd));
00173         Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111);
00174         }
00175         
00176         pre_spd = spd;
00177         pre_spdE = spd_e;
00178         pre_angle = angle;
00179         pre_angleE = angle_P;
00180 }
00181 
00182 int main() {
00183     Saber.baud(115200);
00184     pc.baud(9600);
00185     timer.attach(timer_warikomi,INT_TIME);
00186     
00187     while(1) {
00188     }
00189 }