main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "define.h" //ステータス用
00004 
00005 Ticker timer;
00006 Timer T;
00007 
00008 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00009 QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);
00010 DigitalIn sw1(p26);
00011 DigitalIn sw2(p25);
00012 DigitalOut fet1(p21);
00013 DigitalOut fet2(p22);
00014 Serial Saber(p13,p14);
00015 Serial pc(USBTX,USBRX);
00016 
00017 int Button() {
00018     
00019     int button_in = sw1.read();
00020     static int pre_button = 0;
00021     static int sw_state = LOW;
00022     
00023     if(button_in && pre_button)sw_state = HIGH;
00024     if(!button_in && !pre_button)sw_state = LOW;
00025     if(button_in && !pre_button)sw_state = FALL;
00026     if(!button_in && pre_button)sw_state = RISE;
00027     
00028     pre_button = button_in;
00029     
00030     return sw_state;
00031 }
00032 
00033 void timer_warikomi()
00034 {
00035     float encount_rot = 0.0;
00036     float Ksp = 0.005, Ksd = 0.0015;    
00037     float ppr = 1.0;
00038     static float pre_spd = 0.0;
00039     static float pre_err = 0.0;
00040     static float ref_spd = 0.0;
00041     static int cmd = 0;
00042     static int mode = 2;
00043     static int lim_cmd = 127;
00044     
00045     int sw_point = Button();
00046     
00047     if(sw_point != HIGH) switch(mode){
00048         case(0):    
00049         ref_spd = 30.0;
00050         if (sw_point == RISE) mode = 1;
00051         break;
00052         
00053         case(1):
00054         fet2 = ON;
00055         if (sw_point == RISE) mode = 2;
00056         break;
00057         
00058         case(2):
00059         ref_spd = 0.0;
00060         fet2 = OFF;
00061         if (sw_point == RISE) mode = 0;
00062         break;
00063         }
00064         
00065     int encount2 = Enc2.getPulses();
00066     int encount3 = Enc3.getPulses();
00067     
00068     if (encount2 > encount3) encount_rot = encount2;
00069     else encount_rot = encount3;
00070     
00071     float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; 
00072     float spd = (rot_sp - pre_spd)/INT_TIME/(48*4);
00073     
00074     float spd_err = ref_spd - spd;
00075     float spd_d = (spd_err - pre_err)/INT_TIME;
00076     cmd += spd_err * Ksp + spd_d * Ksd;
00077     
00078     if (cmd > lim_cmd) cmd = lim_cmd;
00079     if (-cmd < -lim_cmd) cmd = -lim_cmd;
00080     
00081     if (cmd > 0) {
00082         Saber.putc(SB_ADRS);
00083         Saber.putc(1);
00084         Saber.putc(cmd);
00085         Saber.putc((SB_ADRS + 1 + cmd) & 0b01111111);
00086         }
00087     else {
00088         Saber.putc(SB_ADRS);
00089         Saber.putc(0);
00090         Saber.putc(abs(cmd));
00091         Saber.putc((SB_ADRS + 0 + abs(cmd)) & 0b01111111);
00092         }
00093         
00094         pre_spd = spd;
00095         pre_err = spd_err;
00096 }
00097 
00098 int main() {
00099     Saber.baud(115200);
00100     pc.baud(9600);
00101     timer.attach(timer_warikomi,INT_TIME);
00102     
00103     while(1) {
00104     }
00105 }