File content as of revision 5:1506bf89f48e:

#include "mbed.h"
#include "scrp_slave.hpp"
#include "air.hpp"
#include "Servo.h"
ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807ffff);
Serial pc(SERIAL_TX,SERIAL_RX);
DigitalIn limitA(PB_6,PullUp);
DigitalIn limitB(PB_4,PullUp);

DigitalIn limitA2(PC_2,PullUp);
DigitalIn limitB2(PC_3,PullUp);

Air catch_no(PA_9);
Air catch_no2(PB_8);
//Air catch_no3(PA_6);

DigitalIn limitC(PB_5,PullUp);
Air hassya(PA_8);
Servo myservo(PA_11);
Timer timer;
int tim=-2000;
bool limit_switch = true;
int date=0;
int date2=0;
int air_date=0;
int air_date2=0;
int limit_air=0;
int a=0;


void driveMotorS2(double pwm){
    PwmOut up_pin_A(PB_13);
    PwmOut up_pin_B(PB_14);
    up_pin_A.period_us(2048);
    up_pin_B.period_us(2048);
    if (pwm==0) {
        up_pin_A = 0;
        up_pin_B = 0;
    } else if (0 < pwm) {
        up_pin_A = pwm;
        up_pin_B = 0;
    } else {
        up_pin_A = 0;
        up_pin_B= -pwm;
    }
}
//　↓サーボ用
void Servo(bool data){
    if(data==0){
    myservo = 0;
    }else{
    myservo = 0.5;
    }
}
//↓発射用
bool Hassya(int rx_data,int &tx_data){
    if(rx_data==1 && a==1){
        tim=timer.read_ms();
        a=0;        
    }
    if(timer.read_ms()-tim<500){
            hassya.move(0);
            driveMotorS2(0);
            Servo(1);
    }else if(timer.read_ms()-tim<1000){
            hassya.move(1);
            driveMotorS2(0);
            Servo(0);
    }else if((timer.read_ms()-tim>1000 && !limitC==0) || a==1){
            hassya.move(0);
            driveMotorS2(0);
            Servo(0);
            a=1;
    }else if(timer.read_ms()-tim>1000 && a==0){
            hassya.move(1);
            driveMotorS2(-0.1);
            Servo(0);
    }else{
            hassya.move(0);
            driveMotorS2(0);
            Servo(0);
    }
    return true;
}

//↓モーター
void driveMotorS(double pwm){
    PwmOut up_pin_A1(PA_0);
    PwmOut up_pin_B1(PA_1);
    up_pin_A1.period_us(2048);
    up_pin_B1.period_us(2048);
    if (pwm==0) {
        up_pin_A1 = 0;
        up_pin_B1 = 0;
    } else if (0 < pwm) {
        up_pin_A1 = pwm;
        up_pin_B1 = 0;
    } else {
        up_pin_A1 = 0;
        up_pin_B1= -pwm;
    }
}

void driveMotorS3(double pwm){
    PwmOut up_pin_A3(PC_9);
    PwmOut up_pin_B3(PC_8);
    up_pin_A3.period_us(2048);
    up_pin_B3.period_us(2048);
    if (pwm==0) {
        up_pin_A3 = 0;
        up_pin_B3 = 0;
    } else if (0 < pwm) {
        up_pin_A3 = pwm;
        up_pin_B3 = 0;
    } else {
        up_pin_A3 = 0;
        up_pin_B3= -pwm;
    }
}


//↓昇降
//速さはテキトー
bool Up(int rx_data,int &tx_data){
    
    if(rx_data==1){
    date=1;
    }
    if(rx_data==-1){
    date=-1;
    }
    if(date==1 && !limitA){
        driveMotorS(0.4);
    }else if(date==-1 && !limitB){
        driveMotorS(-0.4);
    }else{
        driveMotorS(0);
    }
    return true;
}

//↓昇降
//速さはテキトー
bool Up2(int rx_data,int &tx_data){
    
    if(rx_data==1){
    date2=1;
    }
    if(rx_data==-1){
    date2=-1;
    }
    if(limitA2){
    air_date2=0;
    }
    
    if(date2==1 && !limitA2){
        driveMotorS3(0.6);
        limit_air=0;
    }else if(tx_data==-1 &&!limitB2){
        driveMotorS3(-0.6);
        limit_air=0;
    }else{
        driveMotorS3(0);
    }
    return true;
}

//↓回収用
bool CATCH_NO(int rx_data,int &tx_data){
    if(tx_data==1){
        air_date+=1;    
    }else if(tx_data==2&&(air_date%2)==0){
        air_date=1;    
    }
    catch_no.move(air_date%2);
    return true;
}

bool CATCH_NO2(int rx_data,int &tx_data){
    air_date2+=tx_data;
    catch_no2.move(air_date2%2);
    return true;
    
}

    
bool limit2(int id,int ppp){
    if(ppp==1){
        limit_switch=false;
        driveMotorS(0);
        driveMotorS2(0);
    }
    return true;
}

bool  limit(int a,int &b){
    return limit2(0,a);
}



int main(){
    timer.start();
    slave.addCMD(1,limit);
    slave.addCMD(22,Hassya);
    slave.addCMD(20,CATCH_NO);
    slave.addCMD(21,Up);
    slave.addCMD(23,CATCH_NO2);
    slave.addCMD(24,Up2);
    while(limit_switch){
        
     
    }
    driveMotorS(0);
    driveMotorS2(0);
}