File content as of revision 0:56c3d27ab161:

#include "mbed.h"
#include "QEI.h"
#include "define.h" //ステータス用

Ticker timer;
Timer T;

QEI Enc1(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
QEI Enc3(p6,p5,NC,RESOLUTION,&T,QEI::X4_ENCODING);

DigitalIn sw1(p26);
DigitalIn sw2(p25);
DigitalIn limit1(p15);
DigitalIn limit2(p16);
DigitalIn sensor1(p18);
DigitalIn sensor2(p17);

DigitalOut fet1(p21);
DigitalOut fet2(p22);

Serial Saber(p13,p14);
Serial pc(USBTX,USBRX);

int Button() {
    int button_in = sw1.read();
    static int pre_button = 1;
    static int sw_state = LOW;
    
    if(button_in && pre_button)sw_state = HIGH;
    if(!button_in && !pre_button)sw_state = LOW;
    if(button_in && !pre_button)sw_state = FALL;
    if(!button_in && pre_button)sw_state = RISE;
    
    pre_button = button_in;
    
    return sw_state;
}

void timer_warikomi()
{
    float encount_ang = 0.0;
    float encount_rot = 0.0;
    float Ksp = 0.005, Ksd = 0.0015;    //速度係数
    float Kp=5.0, Ki=0.01, Kd=0.1;             //角度係数
    //float Kp_V=1, Kd_V=0;               //角速度係数
    float ppr = 1.0;
    static float pre_angle = 0.0;
    static float pre_angleE = 0.0;
    static float pre_spd = 0.0;
    static float pre_spdE = 0.0;
    static float ref_angle = 0.0;
    static float ref_spd = 0.0;
    static float angle_I = 0.0;
    static int cmd_spd = 0;
    static int cmd_ang = 0;
    static int mode = 0;
    static int lim_cmdA = 20;
    static int lim_cmdS = 127;
    static int pre_encount = 0;
    
    int sw_point = Button();
        
    int encount2 = Enc2.getPulses();
    int encount3 = Enc3.getPulses();
    
    encount_ang = Enc1.getPulses()- pre_encount;
    if (encount2 > encount3) encount_rot = encount2;
    else encount_rot = encount3;
    
    float angle = encount_ang * GEAR_RATE * (360.0/48.0) / 4.0;   
    float ang_spd =(angle - pre_angle)/INT_TIME;
    
    float rot_sp = (float)encount_rot/MULTIPLU/ppr*PULL_RATE; 
    float spd = (rot_sp - pre_spd)/INT_TIME(RESOLUTION*MULTIPLU);
    
    float angle_P = (ref_angle - angle);
    float angle_D=(angle_P - pre_angleE)/INT_TIME;
    angle_I += (angle_P + pre_angleE)*INT_TIME/2.0;
    
    cmd_ang = (int)(angle_P * Kp + angle_D * Kd + angle_I * Ki);
    
    float spd_e = ref_spd - spd;
    float spd_D = (spd_e - pre_spdE)/INT_TIME;
    cmd_spd += (int)(spd_e * Ksp + spd_D * Ksd);
    
    if (cmd_ang > lim_cmdA) cmd_ang = lim_cmdA;
    if (-cmd_ang < -lim_cmdA) cmd_ang = -lim_cmdA;
    
    if (cmd_spd > lim_cmdS) cmd_spd = lim_cmdS;
    if (-cmd_spd < -lim_cmdS) cmd_spd = -lim_cmdS;
    
    if (sw_point != HIGH) switch (mode) {
        
        case 0:
            ref_angle = 0;
            ref_spd = 0.0;
            fet2 = OFF;
            if (sw_point == RISE) mode = 1;
            break;
            
        case 1:
            cmd_ang = 20;
            if (limit1.read()) {
                cmd_ang = 0;
                pre_encount = Enc1.getPulses();
                if (sw_point == RISE) mode = 2;
            }
            break;
        
        case 2:
            ref_angle = -125;
            if (sw_point == RISE) mode = 3;
            break;
            
        case 3:
            if (!sensor1 && !sensor2) {
                fet1 = ON;
                if (sw_point == RISE) mode = 4;
                }
            break;
        
        case 4:
            ref_angle = 0;
                 //if(99<=angle||angle<=101){
            if (sw_point == RISE) {
                cmd_ang = 0;
                pre_encount = Enc1.getPulses(); //スイッチを離したところを初期値に
                mode = 5;                       //して無理くり止めてます
                }
            break;
        
        case 5:
            fet1 = OFF;
            ref_spd = 30.0;
            if (sw_point == RISE) mode = 6;
            break;
        
        case 6:
            fet2 = ON;
            if (sw_point == RISE) mode = 0;
            break;
        }
        
    if (!sw2.read()) {
        cmd_spd = 0;
        cmd_ang = 0;
        }
        
    if (cmd_ang >= 0) {
        Saber.putc(SB_ADRS_A);
        Saber.putc(1);
        Saber.putc(cmd_ang);
        Saber.putc((SB_ADRS_A + 1 + cmd_ang) & 0b01111111);
        } 
    else {
        Saber.putc(SB_ADRS_A);
        Saber.putc(0);
        Saber.putc(abs(cmd_ang));
        Saber.putc((SB_ADRS_A + 0 + abs(cmd_ang)) & 0b01111111);
        }
        
    if (cmd_spd >= 0) {
        Saber.putc(SB_ADRS_B);
        Saber.putc(1);
        Saber.putc(cmd_spd);
        Saber.putc((SB_ADRS_B + 1 + cmd_spd) & 0b01111111);
        }
    else {
        Saber.putc(SB_ADRS_B);
        Saber.putc(0);
        Saber.putc(abs(cmd_spd));
        Saber.putc((SB_ADRS_B + 0 + abs(cmd_spd)) & 0b01111111);
        }
        
        pre_spd = spd;
        pre_spdE = spd_e;
        pre_angle = angle;
        pre_angleE = angle_P;
}

int main() {
    Saber.baud(115200);
    pc.baud(9600);
    timer.attach(timer_warikomi,INT_TIME);
    
    while(1) {
    }
}