File content as of revision 2:079ba2b551a4:

//ロボコン3年投擲テスト用プログラム
//一連の動作を実行できるように
//チャージ -> シャトル認識 -> つかむ -> 解放(投擲) -> 再装填

//hファイル
#include "mbed.h"
#include "QEI.h"
#include "ultrasonic.h"
#include "VL53L0X.h"

//difine

//pin設定
DigitalOut valve1(p15);         //ｐ21->p15
QEI rollen(p21, p22, NC, 1024); //p29->p21 p30->p22


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

DigitalIn button_hand(p5);
DigitalIn button_zero(p6);
DigitalIn button_charge(p7);

Serial pc(USBTX,USBRX);



//timer初期化
Ticker rollening;

//グローバル変数
int enc_hand = 0;
//int enc_belt = 0;
char state = '0';
int enc_old = 0;    //過去カウント比較用

//グローバル関数
void Dthrow(){
    enc_hand = rollen.getPulses();
    
    //pc.printf("%d\n",enc_hand);
    switch (state){
        case '0': // Rotate only Motor1 by Sabertooth responding to aIn_1 input.
            if(enc_hand < -524){ //角度入力 deg(90)
                led1 = 1;
                valve1 = 1;
                }
            else{
                led1 = 0;
                led2 = 0;
                led3 = 0;
                led4 = 0;
                valve1 = 0;
                }
            
            break;
        case '1': // Rotate only Motor2 by Sabertooth responding to aIn_2 input.
            if(enc_hand < -455){ //角度入力 deg(80)
                led2 = 1;
                valve1 = 1;
                }
            else{
                led1 = 0;
                led2 = 0;
                led3 = 0;
                led4 = 0;
                valve1 = 0;
                }
            break;
        case '2': // Stop both Motor1 and Moter2 by Sabertooth.
            if(enc_hand < -427){ //角度入力 deg(75)
                led3 = 1;
                valve1 = 1;
                }
            else{
                led1 = 0;
                led2 = 0;
                led3 = 0;
                led4 = 0;
                valve1 = 0;
                }
            break;
        /*case '3': // Rotate only Motor1 by Sabertooth responding to aIn_1 input.
            if(enc_hand < - /360*1024){ //角度入力
                led2 = 1;
                valve1 = 1;
                }
            else{
                led2 = 0;
                valve1 = 0;
                }
            break;
        case '4': // Rotate only Motor2 by Sabertooth responding to aIn_2 input.
            if(enc_hand < - /360*1024){ //角度入力
                led2 = 1;
                valve1 = 1;
                }
            else{
                led2 = 0;
                valve1 = 0;
                }
            break;
        case '5': // Stop both Motor1 and Moter2 by Sabertooth.
            if(enc_hand < - /360*1024){ //角度入力
                led2 = 1;
                valve1 = 1;
                }
            else{
                led2 = 0;
                valve1 = 0;
                }
            break;*/
        }
        /*if(enc_hand<enc_old){  
        led1 = 1;
        led3 = 0;
        }
    else{
        led1 = 0;
        led3 = 1;
    }*/
    
    enc_old = enc_hand;
}

void dist(int distance){
    //put code here to execute when the distance has changed
    printf("Distance_1 %d mm\r\n", distance);
    //printf("Distance_2 %d mm\r\n", distance);
}

ultrasonic mu(p6, p7, .1, 1, &dist);

void hand(){
    
    }
    
void zero(){
    enc_hand = 0;
    }
    
void charge(){
    
    }
    
//main関数
int main(){
    button_hand.mode(PullUp);
    button_zero.mode(PullUp);
    button_charge.mode(PullUp);
    button_hand.rise(&hand);
    button_zero.rise(&zero);
    button_charge.rise(&charge);
    mu.startUpdates();//start measuring the distance
    rollening.attach(&Dthrow, 0.01);
    valve1 = 1;
    //メモ：attaach_usでマイクロ秒もいけるらしいですがいけませんでした
    
    while(1){
        mu.checkDistance();
        pc.printf("state:%c\t encoder:%d\n",state,enc_hand); 
        if(pc.readable()) state = pc.getc();
        if(button == 1){
            valve1 = 1
            led4 = 1;
            }
            else if(button == 0){
                valve1 = 0;
                led4 = 0;
                wait(0.5);
                
                }
        wait(0.01);
    }
    
}