File content as of revision 15:7c26b01dcefd:

//発射、リボルバープログラム
#include "mbed.h"
#include "rotary_inc.hpp"
#include "scrp_slave.hpp"
#include "Motor.hpp"

Motor motor_revolver(PB_4,PB_5);//リボルバーモーター
Motor motor_shoot(PB_1,PA_11);//発射機構モーター
PwmOut servo(PB_8);//ロック解除用サーボ

//ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,0x0803f800);//l432kc
ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807f800);

//RotaryInc v(PA_14,PA_15,1,1024,4);//リボルバーロリコン
RotaryInc v(PA_14,PA_15,1,1024,2);

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

int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
bool turn_able;//回転可否　0:待機　1:回転

double revolver_pwm = -0.03;//リボルバー回転速度
double load_pwm = -0.3;//装填モーター速度
double back_pwm = 0.3;//復帰モーター速度

double turn = 0.0;

int theta_0 = 1290;//0度
int theta_45 = 1875;//45度
int roricon = 0;//ロリコン

bool interrupt(int rx_data,int &tx_data){
    if(shoot_phase == 0){//フェーズ0以外は、発射できないように
        shoot_phase = rx_data;//1を受け取って装填動作開始 
    }
    return true;  
}

void revolver_back(int count){
    if(turn_able == 1){
        while(roricon < 341 * (count - 1) || limit_2.read() == 0){
            if(count != 1){
                if(roricon < 341 * (count - 1)){
                    roricon = v.get();
                    motor_revolver.output(revolver_pwm);
                    printf("roricon = %d\n",roricon);    
                }else{
                    motor_revolver.output(0);
                    turn_able = 0;
                    printf("finished turnning\n");  
                }    
            }
            
            if(limit_2.read() == 0){
                motor_shoot.output(back_pwm);
                printf("backing\n");
            }
            
            wait(0.01);
        }
    }
}

int main(){
    int count = 1;//何発目か
    shoot_phase = 0;
    
    servo.period_us(20000);
    limit_1.mode(PullUp);
    limit_2.mode(PullUp);
    
    slave.addCMD(2,interrupt);
    
    while(1){
        switch (shoot_phase){
            
            case 0://待機
            while(shoot_phase == 0){
                wait(0.1);
                printf("waiting\n");  
            }
            shoot_phase = 1;
            printf("finished waiting\n");  
            break;
            
            case 1://装填動作
            while(limit_1.read() == 0){
                motor_shoot.output(load_pwm);
                printf("loading\n");
                wait(0.1);
                if(limit_2.read() == 1){
                    break;
                }
            } 
            motor_shoot.output(0);
            shoot_phase = 2;
            printf("finished loading\n");
            wait(5); 
            break;
            
            case 2://リボルバー回転,モーター復帰
            turn_able = 1;
            revolver_back(count);
            printf("finished backing\n"); 
            shoot_phase = 3;
            break;
            
            case 3://発射動作
            wait(5);
            servo.pulsewidth_us(theta_45);
            printf("finished shooting %d shot\n",count);  
            count ++;
            wait(5);
            servo.pulsewidth_us(theta_0);
            motor_shoot.output(load_pwm);
            wait(0.1);
            motor_shoot.output(0);
            shoot_phase = 0;
            printf("finished init\n");  
            break;            
        }
    }
}