File content as of revision 0:8ffb3e099b90:

#include"mbed.h"
#include"KondoServo.h"
#include "EC.h" //Encoderライブラリをインクルード
#define RESOLUTION 500

//Serial pc(USBTX, USBRX); // tx, rx
PwmOut f(p26);//モーター
PwmOut b(p25);

Ticker ticker;//割込み

Ec1multi EC(p16,p17,RESOLUTION);//エンコーダー
InterruptIn X(p15);

KondoServo servo(p28,p27);//サーボ

DigitalOut out1(p19);//エアシリンダー（つかむ方）
DigitalOut out2(p20);//エアシリンダー（投擲）

DigitalIn button(p14,PullUp);//タッチセンサー

DigitalOut led1(LED1);//LED
DigitalOut led2(LED2);

void cal()
{
    EC.calOmega();
}

int X_count=0;

void Xcount()
{
    X_count++;
}

int main()
{
    X.rise(&Xcount);
    f.period_us(50);
    b.period_us(50);
    out1 = 0;
    out2 = 0;
    double a=0,r=0.4,v=0;//rで半径指定 a*r=v
    int count=0;
    ticker.attach(&cal,0.05);

    int id = 0;
    double SERVO2DEG = 270.0 / (11500 - 3500);
    double first = (6800 - 3500) * SERVO2DEG;
    double grab = (3800 - 3500) * SERVO2DEG;
    double pass = (5000 - 3500) * SERVO2DEG;

    servo.set_degree(id, first);
    printf("start\r\n");
    wait(1);

    servo.set_degree(id, grab);
    wait(1);
    led1 = 1;//掴む
    out1 = 1;
    printf("grab\r\n");
    wait(1);

    while(1) {//投擲アームをセット
        a = EC.getOmega();
        count = EC.getCount();
        v = a*r;
        f = 0.0;//速度一定
        b = 0.2;
        printf("%.3f %.3f %d\r\n",a,v,count);
        
        if(X_count == 1) {
            f = 0.0;
            b = 0.0;
            break;
        }
    }

    servo.set_degree(id, pass);
    printf("set\r\n");

    while(1) {
        a = EC.getOmega();
        count = EC.getCount();
        v = a*r;
        f = 0.0;//速度一定
        b = 0.05;
        printf("%.3f %.3f\r\n",a,v);
        if(button.read() == 0) {
            wait(0.05);
            if(button.read() == 0) {
                f = 0.0;
                b = 0.0;
                wait(0.5);
                break;
            }
        }
    }
    led2 = 1;//投擲アームが掴む
    out2 = 1;
    wait (1);

    led1 = 0;//離す
    out1 = 0;
    printf("ok\r\n");
    wait(1);
    servo.set_degree(id, first);
    wait(1);

    while(1) {//投擲アームを上に
        a = EC.getOmega();
        count = EC.getCount();
        v = a*r;
        f = 0.1;//速度一定
        b = 0.0;
        printf("%.3f %.3f\r\n",a,v);
        if(X_count == 2) {
            EC.reset();
            X_count =0;
            break;
        }
    }

    servo.set_degree(id, grab);//掴むアームをセット
    printf("finish\r\n");
    return 0;
}