File content as of revision 25:430aa10d6f13:

#include "mbed.h"
#include "rotary_inc.hpp"
#include "scrp_slave.hpp"
#include "Motor.hpp"

//-----インスタンス--------------------------

ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//ID=3

Motor motor_revolver(PA_7,PA_8);//リボルバーモーター
Motor motor_shoot(PA_1,PA_3);//発射機構モーター
PwmOut servo(PB_6);//ロック解除用サーボ

RotaryInc v(PA_0,PA_4,1,1024,2);//リボルバーロリコン

DigitalIn limit_1(PB_0);//リミットスイッチ１
DigitalIn limit_2(PB_1);//リミットスイッチ２

DigitalOut led[4] = {
    DigitalOut(PB_7),
    DigitalOut(PB_5),
    DigitalOut(PB_4),
    DigitalOut(PB_3)
};

//-----変数---------------------------------

int shoot_phase = 10;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　10:停止　　100:位置合わせ開始
bool turn_able = 0;//回転可否
bool back_able = 0;//復帰可否　

bool shoot_start = 0;//発射開始　0:待機　1:開始済み
bool emergency = 0;//0:停止　1:停止
bool back = 0;

int roricon = 0;//ロリコン

//-----定数---------------------------------
const double revolver_pwm = -0.1;//リボルバー回転速度
const double load_pwm = -0.5;//装填モーター速度
const double back_pwm = 0.5;//復帰モーター速度

const int theta_0 = 1290;//0度
const int theta_45 = 1875;//45度

//-----関数---------------------------------

//位置合わせ開始を受信
bool get_move(int rx_data,int &tx_data){
    if(shoot_phase == 0){//フェーズ0以外は、発射できないように
        if(rx_data == 1){
            shoot_phase = 100;
        }     
    }
    return true;  
}

//発射開始を受信
bool get_shoot(int rx_data,int &tx_data){
    if(shoot_phase == 0){//フェーズ0以外は、発射できないように
        if(rx_data == 1){
            shoot_phase = 1;
        }    
    }
    return true;  
}

//非常停止を受信
bool get_stop(int rx_data,int &tx_data){
    if(rx_data == 0){
        if(shoot_phase == 10){
            shoot_phase = 0;
        }
    }
    if(rx_data == 1){
        while(rx_data == 1){
            emergency = 1;
            shoot_phase = 10;
        }
    }
    return true;
}

//初期化を受信
bool init(int rx_data,int &tx_data){
    if(rx_data == 1){
        while(limit_1.read() == 0){
            motor_shoot.output(load_pwm);
            printf("loading\n");
            wait(0.1);
        }   
        motor_shoot.output(0);
        shoot_phase = 0;
        return true; 
    }  
}

//リボルバー、ラック復帰
void revolver_back(int count){
    while(turn_able == 1 || back_able == 1){
        //リボルバー
        //一射目
        if(count == 1){
            motor_revolver.output(0);
            turn_able = 0;
            back_able = 0;
            printf("no turn and back\n");  
        }
        if(count > 1){
            if(roricon < 341 * (count - 1)){
                if(turn_able == 1){
                    roricon = v.get();
                    motor_revolver.output(revolver_pwm);
                    printf("roricon = %d\n",roricon); 
                }    
            }
            else{
                motor_revolver.output(0);
                turn_able = 0;   
            } 
        }     
        //復帰   
        if(count > 1){
            if(limit_2.read() == 0){
                if(back_able == 1){
                    motor_shoot.output(back_pwm);
                    printf("backing\n");    
                }
            }
            if(limit_2.read() == 1){
                motor_shoot.output(0);
                back_able = 0;
                printf("finished backing\n");
            }   
        }
        //ループ抜け出し   
        if(turn_able == 0 && back_able == 0){
            break;
        }
        wait(0.01);
    }
}

//------------------------------------------

int main(){
    int count = 1;//何発目か
    
    servo.period_us(20000);
    limit_1.mode(PullUp);
    limit_2.mode(PullUp);
    
    slave.addCMD(2,get_shoot);
    slave.addCMD(6,get_move);
    slave.addCMD(51,get_stop);
    
    slave.addCMD(10,init);
    
    while(1){
//----------自動発射------------------------------
        switch (shoot_phase){
            case 0://待機
                led[0].write(1);
                led[1].write(1);
                led[2].write(1);
                led[3].write(1);
                while(shoot_phase == 0){
                    wait(0.1);
                    printf("waiting\n");  
                }
                printf("finished waiting\n"); 
            break;
                
            case 1://装填動作
                led[0].write(0);
                led[1].write(0);
                led[2].write(0);
                led[3].write(0);
                if(count < 7){
                    while(limit_1.read() == 0){
                        if(count == 1){
                            break;
                        }
                        motor_shoot.output(load_pwm);
                        printf("loading\n");
                        wait(0.1);
                    } 
                    motor_shoot.output(0);
                    shoot_phase = 2;
                    printf("finished loading\n");
                    wait(0.1);       
                }else{
                    emergency = 1;
                    shoot_phase = 10;
                } 
            break;
                
            case 2://リボルバー回転,モーター復帰
                turn_able = 1;
                back_able = 1;
                revolver_back(count);
                shoot_phase = 3;
                printf("finished backing\n"); 
                wait(0.5);
            break;
                
            case 3://発射動作
                servo.pulsewidth_us(theta_45);
                printf("%d shot\n",count);  
                wait(1);
                servo.pulsewidth_us(theta_0);
                if(count != 6){
                    motor_shoot.output(load_pwm);
                    wait(0.15);
                    motor_shoot.output(0);  
                }
                shoot_phase = 0;
                printf("finished init\n");  
                count ++;
            break;   
                
            case 10://非常停止
                while(emergency == 1){
                    motor_shoot.output(0);
                    motor_revolver.output(0);
                    printf("emergency\n"); 
                    wait(0.1);
                }
            break;   
            
            case 100://位置合わせ開始、装填
                led[0].write(0);
                led[1].write(0);
                led[2].write(0);
                led[3].write(0);
                while(limit_2.read() == 0){
                    motor_shoot.output(back_pwm);
                    printf("loading\n");
                    wait(0.1);
                } 
                motor_shoot.output(0);  
                shoot_phase = 0;
            break;     
        }   
    }
}