File content as of revision 5:869dc702b923:

#include "mbed.h"
#include "QEI.h"
#include "Filter.h"
#define SB_ADRS 132
#define SABER_ADDR 128
#define INT_TIME 0.02
#define RESOLUTION 48
#define MULTIPLU 4.0

Ticker timer;
Timer T;
QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
Serial Saber(p13,p14);
Serial pc(USBTX,USBRX);
Filter velfilter(INT_TIME);

DigitalIn sw2(p25);
DigitalIn sw1(p26);//モード切替
DigitalOut fet1(p22);
DigitalOut fet2(p21);

DigitalIn limit1(p15);
DigitalIn limit2(p16);

DigitalIn SENS1(p18);
DigitalIn SENS2(p17);

DigitalIn G_limit1(p9);
DigitalIn G_limit2(p10);

int cmd,A;
int SA1,B_SA1,LIM1,LIM2;
int S1,S2;

float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd;
float goal_D=0,Kp=5,Ki=0.01,Kd=0.1;
float Ksp2 = 6.5, Ksd2 = 0.4; 
float Ksp3 = 6.5, Ksd3 = 0.4; 

float encount,b_encount;

int mode = 7;
int cmd2 = 0;
int cmd3 = 0;

float spd2=0;
float spd3=0;

float spd_err2=0;
float spd_err3=0;

int tmp1;
int tmp2;

double filtered_ref_spd;

int G_LIM1=0,G_LIM2=0;

int G_cmd;

int modee=2;
int button_in_2=0; 

int Button() {
    
    int button_in = sw1.read();
    
    static int pre_button = 0;
    static int sw_state = 3;
    
    if(button_in && pre_button)sw_state = 0;
    if(!button_in && !pre_button)sw_state = 3;
    if(button_in && !pre_button)sw_state = 1;
    if(!button_in && pre_button)sw_state = 2;
    
    pre_button = button_in;
    
    return sw_state;
}
int Button2() {//スイッチのノイズをとる関数
    
    button_in_2 = sw2.read();
    
    static int pre_button_2 = 0;
    static int sw_state_2 = 3;
    
    if(button_in_2 && pre_button_2)sw_state_2 = 0;
    if(!button_in_2 && !pre_button_2)sw_state_2 = 3;
    if(button_in_2 && !pre_button_2)sw_state_2 = 1;
    if(!button_in_2 && pre_button_2)sw_state_2 = 2;
    
    pre_button_2 = button_in_2;
    
    return sw_state_2;
}


void timer_warikomi()
{

    LIM1=!limit1.read();
    LIM2=!limit2.read();
    S1=SENS1.read();
    S2=SENS2.read();
    encount=Enc.getPulses()-b_encount;

    float ppr = 1.0;
    
    static float pre_spd2 = 0.0;
    static float pre_spd3 = 0.0;
    
    static float pre_err2 = 0.0;
    static float pre_err3 = 0.0;
    
    static float ref_spd = 0.0;
    
    static int lim_cmd2 = 87;
    static int lim_cmd3 = 92;
    
    int sw_point = Button();
    
    angle=(float)(encount)*(360.0/48.0)/4.0;   
    SOKUDO=(angle-pre_angle)/INT_TIME;

    e_D=(goal_D-angle);
    ed_D=(e_D-pre_e_D)/INT_TIME;
    ei_D+=(e_D+pre_e_D)*INT_TIME/2.0;
    
    cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki));
         
    float encount2 = Enc2.getPulses();
    float encount3 = Enc3.getPulses();
    
    float rot_sp2 = encount2/MULTIPLU/ppr;
    spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4);
    float rot_sp3 = encount3/MULTIPLU/ppr; 
    spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4);
    
    spd_err2 = filtered_ref_spd - spd2;
    float spd_d2 = (spd_err2 - pre_err2)/INT_TIME;
    tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2));
    if(tmp1>=127)tmp1=127;
    if(tmp1<=-127)tmp1=-127;
    cmd2 += tmp1;
    
    spd_err3 = filtered_ref_spd - spd3;
    float spd_d3 = (spd_err3 - pre_err3)/INT_TIME;
    tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3));
    if(tmp2>=127)tmp2=127;
    if(tmp2<=-127)tmp2=-127;
    cmd3 += tmp2;
    
    if (cmd2 > lim_cmd2) cmd2 = lim_cmd2;
    if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2;
    
    if (cmd3 > lim_cmd3) cmd3 = lim_cmd3;
    if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;
    
    if(sw_point != 0) switch(mode){
          case 0:
              goal_D=0;
                if(sw_point==2)mode=1;
                break;
          case 1:
              cmd=-15;
                if(sw_point==2)mode=2;
                if(LIM2==1){
                        cmd=0;
                        b_encount=Enc.getPulses();
                        }
                        break;
          case 2:
              goal_D=125;
                 if(sw_point==2)mode=3;
                 if(angle>=120)cmd=0;
                 if(S1==0&&S2==0){
                fet1=1;
                A=1;
                }
                 break;
               
          /*case 3:
                if(sw_point==2)mode=4;
                if(S1==0&&S2==0){
                fet1=1;
                A=1;
                }
                break;*/
                
          case 3:
                goal_D=0;
                    if(sw_point==2){
                    cmd=0;
                    b_encount=Enc.getPulses();
                    mode=4;
                }
                break;
          case 4:
                fet1=0;
                if(sw_point==2)mode=5;
                break;
           
          case 5:
               ref_spd = 26.0;
               if (sw_point == 2) mode = 6;
               break;
        
          case 6:
               fet2 = 0;
               if (sw_point == 2) mode = 7;
               break;
        
          case 7:
               ref_spd = 0.0;
               fet2 = 1;
               if (sw_point == 2) mode = 0;
               if (spd3<=5.0) {
                   cmd2 = 0;
                   cmd3 = 0;
                   }
               break;
          }
          
          G_LIM1=!G_limit1.read();//pullupなので逆
    G_LIM2=!G_limit2.read();//pullupなので逆
    
    int sw_point2 = Button2();//スイッチの関数からリターン
        
    if(sw_point2 != 0) switch(modee){
        case(0):  
        G_cmd=120;
        if(G_LIM1==1){
            G_cmd=0;
                }
        if (sw_point2 == 2) modee = 1;

        break;
        
        case(1):
        G_cmd=-120;
        if (sw_point2 == 2) modee = 2;
        if(G_LIM2==1){
            G_cmd=0;
            }
        break;
        
        case(2)://モータ停止と押し上げ機構の降下
        G_cmd=0;
        if (sw_point2 == 2) modee = 0;
        break;
        }
    
          
          if(filtered_ref_spd>=25.5&&mode==5){
          filtered_ref_spd=26;
    }else if(filtered_ref_spd>=25.5&&mode==6){
          filtered_ref_spd=26;
    }else if(filtered_ref_spd<=5.0&&mode==7){
          filtered_ref_spd=0;
    }else if(mode==5||mode==6||mode==7){
          filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
    }
    
    if(cmd>20) cmd=20;
    if(cmd<-15)cmd=-15; 
    
    int F=1,FF=0;//向き
    
    if(cmd>=0) {
        Saber.putc(SABER_ADDR);
        Saber.putc(F); 
        Saber.putc(abs(cmd));
        Saber.putc((SABER_ADDR + F + abs(cmd)) & 0b01111111); 
    } else {
        Saber.putc(SABER_ADDR);
        Saber.putc(FF); 
        Saber.putc(abs(cmd));
        Saber.putc((SABER_ADDR + FF + abs(cmd)) & 0b01111111); 
    }
    
    if (cmd2 > 0) {
        Saber.putc(SB_ADRS);
        Saber.putc(4);
        Saber.putc(cmd2);
        Saber.putc((SB_ADRS + 4 + cmd2) & 0b01111111);
        }
    else {
        Saber.putc(SB_ADRS);
        Saber.putc(5);
        Saber.putc(abs(cmd2));
        Saber.putc((SB_ADRS + 5 + abs(cmd2)) & 0b01111111);
        }
        
    if (cmd3 > 0) {
        Saber.putc(SB_ADRS);
        Saber.putc(0);
        Saber.putc(cmd3);
        Saber.putc((SB_ADRS + 0 + cmd3) & 0b01111111);
        }
    else {
        Saber.putc(SB_ADRS);
        Saber.putc(1);
        Saber.putc(abs(cmd3));
        Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
        }
    if (G_cmd > 0) {
        Saber.putc(SABER_ADDR);
        Saber.putc(4);
        Saber.putc(G_cmd);
        Saber.putc((SABER_ADDR + 4 + G_cmd) & 0b01111111);
        }
    else {
        Saber.putc(SABER_ADDR);
        Saber.putc(5);
        Saber.putc(abs(G_cmd));
        Saber.putc((SABER_ADDR + 5 + abs(G_cmd)) & 0b01111111);
        }
        
    pre_spd2 = rot_sp2;
    pre_err2 = spd_err2;
    pre_spd3 = rot_sp3;
    pre_err3 = spd_err3;
        
        
    pre_e_D=e_D;
    pre_angle=angle;
    pre_e_V=e_V;
    B_SA1=SA1;
    }


int main() {
    Saber.baud(19200);
    pc.baud(9600);
    fet1=0;
    fet2=1;
    
    G_limit1.mode( PullUp );    //  内蔵プルアップを使う
    G_limit2.mode( PullUp ); 
    
    velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
    
    timer.attach(timer_warikomi,INT_TIME);
    
    while(1) {
        //pc.printf("%d\n",mode);
        pc.printf("spd2%f\t spd3 %f\n",spd2,spd3);
    }
}