File content as of revision 6:7afdc6a81566:

#include "mbed.h"
#include "QEI.h"
#include "Filter.h"
#include "string"
#include "define.h"
#include "RoboClaw.h"

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);*/
RoboClaw MD(115200,p9,p10);
Serial Saber(p13,p14);
Serial pc(USBTX,USBRX);
RawSerial Master(p28,p27,115200);//(tx,rx,baud);
Filter velfilter(INT_TIME);

DigitalOut fet1(p22);//shagai把持
DigitalOut fet2(p21);//shagai押出
DigitalOut fet3(p23);//shagaiハンド昇降

DigitalIn limit1(p15);//shagaiハンドlimit
DigitalIn limit2(p16);
//SENSのどっちか減るかも
DigitalIn SENS1(p18);//shagai検出
DigitalIn SENS2(p17);

DigitalIn G_limit1(p9);//gerege limit
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;

char mode = 0x00;
/*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_qpps;

int G_LIM1=0,G_LIM2=0;

int G_cmd;

int limit_MD(int qpps,int max_qpps){
    if (qpps > max_qpps) qpps = max_qpps;
    else if (-qpps < -max_qpps) qpps = -max_qpps;
    return qpps;
}

void robo_serial(int adrs, int qpps1, int qpps2){
    MD.SpeedM1(adrs,qpps1);
    MD.SpeedM2(adrs,qpps2);
}

void Saber_Serial (int adrs, int rot, int cmd){
    Saber.putc(adrs);
    Saber.putc(rot); 
    Saber.putc(abs(cmd));
    Saber.putc((adrs + rot + abs(cmd)) & 0b01111111);
}

/*void Slave_tx(char tx_mode){                                //処理の終了を送る
    Master.printf("%c",tx_mode);
    Master.printf("%c",tx_mode);
    Master.printf("\n");
}*/

void Slave_rx() {                                           //処理内容を受け取る
    static string rx_mode = "";
    char rx_c = Master.getc();
    
    if (rx_c == '\n') {
        if (rx_mode.size() == 2){
        if (rx_mode[0] == rx_mode[1]){
        mode = rx_mode[1];       //モード切替
        }}
        rx_mode = "";
        }
    
    else {
        if (rx_mode.size() > 2) rx_mode = "";
        rx_mode += rx_c;
        }
        //pc.printf("%x\n",mode);
}

void timer_warikomi()
{
    static int qpps1 = 0;
    static int qpps2 = 0;
    static int ref_qpps1 = 0;
    static int ref_qpps2 = 0;

    LIM1=!limit1.read();
    LIM2=!limit2.read();
    S1=SENS1.read();
    S2=SENS2.read();
    static char slave_mode = 0x00;
    static int spd_count = 0;
    /*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;
    
    static int count = 0;
    static int count3 = 0;
    
    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;*/
    
    
    
    G_LIM1=!G_limit1.read();//pullupなので逆
    G_LIM2=!G_limit2.read();//pullupなので逆
    
    switch (mode) {
        case 0x99:
                break;
                
        case 0x10://初期化処理
                ref_qpps1 = 0.0;
                ref_qpps2 = 0.0;
                
                break;
                
        case 0x20://ハンド降下
                //goal_D=125;
                if(S1==0&&S2==0){
                fet1=1;
                A=1;
                }
                break;
                 
        case 0x30://ハンド上昇
                
                break;
                
        case 0x40://geregeハンド
                G_cmd=120;
                if(G_LIM1){
                    G_cmd=0;
                }
                break;
                
        case 0x50://geregeハンド
                G_cmd=-120;
                if(G_LIM2){
                    G_cmd=0;
                slave_mode = 0x55;
                }
                break;
               
        case 0x60://shagai発射
                ref_qpps1 = REF_QPPS1;
                ref_qpps2 = REF_QPPS2;
                if (qpps1 == REF_QPPS1 && qpps2 == REF_QPPS2) {
                    spd_count++;
                }
                break;
        
        case 0x90://全停止
                ref_qpps1 = 0.0;
                ref_qpps2 = 0.0;
                //cmd = 0;
                G_cmd = 0;
                break;
                
        default://何もしない
                break;
        }
        
    //Slave_tx(slave_mode);
    //二次遅れ系 
    if(qpps1>=QPPS_HIGH_RANGE1 && ref_qpps1 == REF_QPPS1) qpps1=ref_qpps1;
    else if(qpps1<=QPPS_HIGH_STOP && ref_qpps1 == 0) qpps1=0;
    else qpps1 = velfilter.SecondOrderLag((double)ref_qpps1);
    if(qpps2>=QPPS_HIGH_RANGE2 && ref_qpps2 == REF_QPPS2) qpps2=ref_qpps2;
    else if(qpps2<=QPPS_HIGH_STOP && ref_qpps2 == 0) qpps2=0;
    else qpps2 = velfilter.SecondOrderLag((double)ref_qpps2);
    //ローラー強制停止
    /*if (qpps <= 5.0 && ref_qpps == 0) {
        cmd3 = 0;
    }*/
    //自動押し出し
    if (spd_count >= 40){
        fet2 = 0;
        if (slave_mode != 0x60) spd_count = 0;
    }
    /*
    if(angle>=124 && goal_D >= 120){
        cmd=0;
        goal_D = angle;
    }
    
    if(angle <= 2 && goal_D <= 10) {
        cmd = 0;
        goal_D = angle;
    }
    
    if(cmd>20) cmd=20;
    if(cmd<-15)cmd=-15;
    
    if(cmd>=0) Saber_Serial (SABER_ADDR, 1, cmd);
    else Saber_Serial (SABER_ADDR, 0, cmd);
    */
    qpps1 = limit_MD( qpps1, MAX_QPPS1);
    qpps2 = limit_MD( qpps2, MAX_QPPS2);
    
    robo_serial(ROBO_ADRS, qpps1, qpps2);
    
    if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd);
    else Saber_Serial (SABER_ADDR, 5, G_cmd);
    /*
    if (cmd3 > 0) Saber_Serial (SB_ADRS, 0, cmd3);
    else Saber_Serial (SB_ADRS, 1, cmd3);
    
    if (cmd2 > 0) Saber_Serial (SB_ADRS, 4, cmd2);
    else Saber_Serial (SB_ADRS, 5, cmd2);*/
        
    /*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;
    fet3=0;
    
    G_limit1.mode( PullUp );    //  内蔵プルアップを使う
    G_limit2.mode( PullUp ); 
    
    velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
    
    Master.attach(&Slave_rx, RawSerial::RxIrq);
    timer.attach(timer_warikomi,INT_TIME);
    
    while(1) {
        pc.printf("%lf\n", filtered_ref_qpps);
    }
}