File content as of revision 0:c2f05c36e357:

#include "mbed.h"
#include "define.h"
#include "Sabertooth_Serial.h"
#include "PID.h"
#include "QEI.h"
#include "Filter.h"
#include "string"

Ticker timer;
Timer T;

SaberSerial MD_S( 19200, p13, p14);
RawSerial Master( p9, p10, 115200);
RawSerial pc( USBTX, USBRX, 115200);

PID pid1(INT_TIME);
PID pid2(INT_TIME);

Filter LPF1(INT_TIME);
Filter LPF2(INT_TIME);

QEI Enc1(p18,p15,NC,RESO,&T,QEI::X4_ENCODING);
QEI Enc2(p17,p16,NC,RESO,&T,QEI::X4_ENCODING);
//_____________________________↓機能設定

DigitalIn sw1(p29);
DigitalIn sw2(p30);
//DigitalIn pe1(p19);
//DigitalIn pe2(p20);

DigitalOut fet1(p25);//CB回収
DigitalOut fet2(p26);//つっかえ
DigitalOut fet3(p27);//押出 上
DigitalOut fet4(p28);//押出 下

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
//_____________________________↓変数宣言

string Master_str = "";
char global_mode = 0b00000000;
int gl_enc1 = 0, gl_enc2 = 0;
double ref_spd1 = 0, ref_spd2 = 0, load_co = 0, recv_co = 0;
double f_spd1 = 0, f_spd2 = 0;
int gl_cmd1 = 0, gl_cmd2 = 0;
int gl_move = 0, gl_channel = 0;

void Master_send(char mode){
    Master.printf("%c\n",mode);
}

//______________________________↓受信割込み

void Master_recv(){
    recv_co = 1;
    char Master_c = Master.getc();
    static char local_mode = 0b00000000;
    
    if(Master_c == '\n'){
        local_mode = Master_str[0];
        if(local_mode == EMERGENCY) {
            global_mode = EMERGENCY;
        }
        else {
            global_mode = local_mode;
        }
        Master_str = "";
    }
    else Master_str += Master_c;
}
//________________________________________↓速度制限・加減速制限

double limit_MD(double cmd,double max_cmd){
    if (cmd > max_cmd) cmd = max_cmd;
    else if (cmd < -max_cmd) cmd = -max_cmd;
    return cmd;
}

//_____________________________↓主要処理(周期割込み)

void timer_warikomi(){
    char mode = global_mode;
    int sw1_def = sw1.read(), sw2_def = sw2.read();
    int enc1 = Enc1.getPulses(), enc2 = Enc2.getPulses();
    static char send_mode = 0b00000000;
    static bool points1 = 0, points2 = 0, points3 = 0;
    static bool positions = 0;
    static bool pre_sw1 = 1, pre_sw2 = 1, move = 0, channel = 0;
    static int counter = 0, point_co = 0, p1_co = 0 ,p3_co = 0, p2_co = 0;
    static int pre_enc1 = 0, pre_enc2 = 0;
    static double cmd1 = 0, cmd2 = 0, up_cmd = 0;
    
    
    if(sw1_def == 0 && pre_sw1 == 1) move = !move;
    if(sw2_def == 0 && pre_sw2 == 1) channel = !channel;
    
    pre_sw1 = sw1_def;
    pre_sw2 = sw2_def;
    
    if(!recv_co);
    if(mode == EMERGENCY){
        f_spd1 = 0;
        f_spd2 = 0;
        up_cmd = 0;
        if(counter >= EMER_COUNT){
            led1 = !led1;
            led2 = !led2;
            led3 = !led3;
            led4 = !led4;
            counter = 0;
        }else counter++;
    }else if(mode & AUTOMATION){
        up_cmd = 0;
        led3 = 1;
        led4 = 1;
        points1   = (mode & POINTS_1);//20
        points2   = (mode & POINTS_2);//40
        points3   = (mode & POINTS_3);//10
        positions = (mode & POSITIONS);
        
        if(!points1 && !points2 && !points3){
            f_spd1 = 0;
            f_spd2 = 0;
        }
        if (points2){
            led4 = 1;
            f_spd1 = REF_SPD2_1;
            f_spd2 = REF_SPD2_2;
            send_mode |= POINTS_2;
            if(positions && (enc1 - pre_enc1) > (REF_SPD2_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
                if(p1_co < 3){
                    point_co++;
                    if(point_co >= 20){
                        fet3 = F3_ON;
                        p1_co++;
                        point_co = 0;
                    }else fet3 = F3_OFF;
                }else{
                    send_mode |= POSITIONS;
                    fet3 = F3_OFF;
                }
            }else{
                send_mode &= ~POINTS_1;
                send_mode &= ~POSITIONS;
            }
        }if(points1){
            led4 = 1;
            f_spd1 = REF_SPD1_1;
            f_spd2 = REF_SPD1_2;
            send_mode |= POINTS_1;
            if(positions && (enc1 - pre_enc1) > (REF_SPD1_1 - 30) && (enc1 - pre_enc1) < (REF_SPD1_1 + 30)){
                fet3 = F3_ON;
                if(p2_co > 8) send_mode |= POSITIONS;
                else p2_co++;
            }else{
                fet3 = F3_OFF;
                send_mode &= ~POINTS_3;
                send_mode &= ~POSITIONS;
            }
        }if(points3){
            led4 = 1;
            fet2 = F2_OFF;
            fet3 = F3_OFF;
            f_spd1 = REF_SPD3_1;
            f_spd2 = REF_SPD3_2;
            send_mode |= POINTS_3;
            if(positions && (enc1 - pre_enc1) > (REF_SPD3_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
                if(p3_co < 6){
                    if (point_co >= 20){
                        fet4 = F4_ON;
                        point_co = 0;
                        p3_co++;
                    }else {        
                        fet4 = F4_OFF;
                        send_mode &= ~POINTS_2;
                        send_mode &= ~POSITIONS;
                    }
                }else{
                    if(p3_co > 15){
                        fet4 = F4_OFF;
                        send_mode |= POSITIONS;
                        f_spd1 = 0;
                        f_spd2 = 0;
                    }else p3_co++;
                }
            }
        }else if(positions){
            led4 = 1;
            send_mode |= POSITIONS;
        }else if(!positions){
            send_mode &= ~POSITIONS;
        }else;
        
        Master_send(send_mode);
    }else{
        /*led1 = 0;
        led2 = 0;
        led3 = 0;
        led4 = 0;*/
        if(mode & CB_COLLECT){
            up_cmd = 50;
            fet1 = F1_ON;
        }else{
            fet1 = F1_OFF;
            up_cmd = 0;
        }
        if(move){
            led1 = 1;
            led2 = 0;
            f_spd1 = REF_SPD3_1;
            f_spd2 = REF_SPD3_2;
            if(channel) fet4 = F4_ON;
            else fet4 = F4_OFF;
        }else{
            led1 = 0;
            led2 = 1;
            f_spd1 = 0;
            f_spd2 = 0;
            if(channel) fet4 = F4_ON;
            else fet4 = F4_OFF;
        }
    }
    gl_enc1 = enc1 - pre_enc1;
    gl_enc2 = enc2 - pre_enc2;
    
    ref_spd1 = LPF1.LowPassFilter(f_spd1);
    ref_spd2 = LPF2.LowPassFilter(f_spd2);
    
    cmd1 += pid1.con(ref_spd1 - gl_enc1);
    cmd1 = limit_MD( cmd1, 120);
    if(f_spd1 == 0 && abs(cmd1) < 20) cmd1 = 0;
    if(cmd1 >= 0) MD_S.Serial(SB_ADRS1, 1, (int)(abs(cmd1)));
    else MD_S.Serial(SB_ADRS1, 0, (int)(abs(cmd1)));
        
    cmd2 += pid2.con(ref_spd2 - gl_enc2);
    cmd2 = limit_MD( cmd2, 120);
    if(f_spd2 == 0 && abs(cmd2) < 20) cmd2 = 0;
    if(cmd2 >= 0) MD_S.Serial(SB_ADRS1, 5, (int)(abs(cmd2)));
    else MD_S.Serial(SB_ADRS1, 6, (int)(abs(cmd2)));
    
    pre_enc1 = enc1;
    pre_enc2 = enc2;
    
    gl_cmd1 = cmd1;
    gl_cmd2 = cmd2;
    gl_move = move;
    gl_channel = channel;
    
    MD_S.Serial(SB_ADRS2, 5, up_cmd);
}
//__________________________________________

int main() {
    fet1 = F1_OFF;
    fet2 = F2_OFF;
    fet3 = F3_OFF;
    fet4 = F4_ON;
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    
    pid1.set(P1,NU,D1);
    pid2.set(P2,NU,D1);
    
    LPF1.setLowPassPara(2.0, 0);
    LPF2.setLowPassPara(2.0, 0);
    
    timer.attach(&timer_warikomi,INT_TIME);
    Master.attach(&Master_recv ,RawSerial::RxIrq);
    while(true) {
        /*static int buff1 = 0, buff2 = 0;
        buff1 = Enc1.getPulses();
        buff2 = Enc2.getPulses();*/
        pc.printf("ERR1:%d ERR2:%d CMD1:%d CMD2:%d MODE:%d CAHN:%d\n", gl_enc1, gl_enc2, gl_cmd1, gl_cmd2, int(global_mode), gl_channel);
        wait(0.1);
    }
}