File content as of revision 8:38db82c3492a:

//発射、リボルバープログラム
#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,0x0803f800);//l432kc
ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807f800);//f446re

RotaryInc v(PA_14,PA_15,1,1024,4);

Motor motor_revolver(PB_13,PB_14);//リボルバーモーター
Motor motor_shoot(PA_11,PB_1);//発射機構モーター

PwmOut servo1(PB_8); //サーボ
PwmOut servo2(PB_9);

DigitalIn limit(dp16);

int shoot_able;//発射フェーズ 0:待機　1:引き動作　2:リボルバー1/6回転　3:発射　
int turn_able;//回転フェーズ　0:待機　1:1/6回転

double revolver_pwm = 0.1;//リボルバー回転速度
double shoot_pwm = 0.1;//装填モーター速度
int theta = 500;//サーボ回転角度
double interval = 0.1;
int roricon = 0;

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

//リボルバーP制御関数
//double k_p = 0.001;
//double target = 171.0;
/*void revolver(int count){
    if(turn_able == 1){
        roricon =  v.get() - (171 * (count - 1));
            
        while(roricon != 171){
            pwm = k_p * (target - roricon);
            motor_revolver.output(pwm);
            printf("roricon = %lf\n",roricon);
            printf("pwm = %lf\n",pwm);
            wait(interval);
        }      
        motor_revolver.output(0.0);
        turn_able = 0;  
    }
}*/

void revolver(int count){
    if(turn_able == 1){
        while(roricon < 171 * count){
            roricon = v.get();
            motor_revolver.output(revolver_pwm);
            printf("roricon = %d\n",roricon);
            wait(interval);
        }
        motor_revolver.output(0.0);
        turn_able = 0;
    }
}

int main(){
    int count = 1;//何発目か
    
    servo1.period_us(20000);
    servo2.period_us(20000);
    
    slave.addCMD(2,interrupt);
    while(1){
        switch (shoot_able){
            case 0:
            //待機
            while(shoot_able == 0){
                wait(0.1);
                printf("waiting\n");  
            }
            printf("finished waiting\n");  
            break;
            
            case 1:
            //装填動作コードをここに書く！
            while(limit.read() == 0){
                motor_shoot.output(shoot_pwm);
            }
            shoot_able = 2;
            printf("finished pulling\n"); 
            wait(5);
            break;
            
            case 2:
            wait(5);
            //リボルバー回転
            turn_able = 1;
            revolver(count);
            if(turn_able == 0){
                shoot_able = 3;
            }
            printf("finished turnning\n");  
            break;
            
            case 3:
            wait(5);
            //発射動作コードをここに書く！
            servo1.pulsewidth_us(theta);
            servo2.pulsewidth_us(theta);
            shoot_able = 0;
            printf("finished shooting %d\n",count);  
            count ++;
            servo1.pulsewidth_us(0);
            servo2.pulsewidth_us(0);
            break;
        } 
    }
}