File content as of revision 21:0f12ee2322e4:

#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "User.h"
#include "mbed.h"
#include "math.h"
#include "mbed.h"
#include"KondoServo.h"
#include "EC.h" //Encoderライブラリをインクルード
#define RESOLUTION 500

int RSX,RSY,LSX,LSY,BSU,BSL;
//Digitalout cs(p8);
int move=5,shoot=0,ball=0,box=0;
CAN controller(p30,p29);//CANpin_name

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
char can[1]= {0};

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

PwmOut f(p26);//投F
PwmOut b(p25);//投B

Ticker ticker;

Ec1multi EC(p16,p17,RESOLUTION);//投E
InterruptIn X(p15);

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

int id = 0;
int id2 = 1;
double SERVO2DEG = 270.0 / (11500 - 3500);
double first = (6800 - 3500) * SERVO2DEG;
double grab = (3600 - 3500) * SERVO2DEG;
double pass = (5000 - 3500) * SERVO2DEG;
double grab2 = (3550 - 3500) * SERVO2DEG;

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

void shot();
void catch_ball();
void show_angle();

int X_count=0;
void Xcount()
{
    X_count++;
}

void UserLoopSetting()
{
}
void UserLoop(char n,const u8* data)
{
    u16 ButtonState;
    if(n==0) { //有線Ps3USB.cpp
        RSX = ((ps3report*)data)->RightStickX;
        RSY = ((ps3report*)data)->RightStickY;
        LSX = ((ps3report*)data)->LeftStickX;
        LSY = ((ps3report*)data)->LeftStickY;
        BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)data)->ButtonState;
    } else {//無線TestShell.cpp
        RSX = ((ps3report*)(data + 1))->RightStickX;
        RSY = ((ps3report*)(data + 1))->RightStickY;
        LSX = ((ps3report*)(data + 1))->LeftStickX;
        LSY = ((ps3report*)(data + 1))->LeftStickY;
        BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
        BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
        //ボタンの処理
        ButtonState =  ((ps3report*)(data + 1))->ButtonState;
    }
    //ここより下にプログラムを書く
    controller.frequency(1000000);
    led1=0;
    led2=0;
    led3=0;
    led4=0;

    //move
    if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1
        move = 9;
    } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1
        move = 3;
    } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1
        move = 7;
    } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1
        move = 1;
    } else if((ButtonState >> BUTTONUP)&1 == 1) { //up
        move = 8;
        led1 = 1;
    } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down
        move = 2;
        led2 =1;
    } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right
        move = 6;
        led3 = 1;
    } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left
        move = 4;
        led4 = 1;
    } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1
        move = 10;
    } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
        move = 11;
    } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
        move = 1;
    } else {
        move = 5;
    }
    //move
    //shoot
    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
        shoot = 1;
        led1 = 1;
    } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
        shoot = 4;
        led2 = 1;
    } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
        shoot = 3;
        led3 = 1;
    } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
        shoot = 2;
        led4 = 1;
    } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
        shoot = 5;
        led4 = 1;
    }
    show_angle();
    //CAN通信用プログラム
    if(shoot==1)shot();
    if(shoot>=2)catch_ball();
    can[0] = move;
    controller.write(CANMessage(1,can,1));
    printf("%d\r\n",can[0]);
}

void show_angle()
{
    X.rise(&Xcount);
    f.period_us(50);
    b.period_us(50);
    double a=0,r=0.4,v=0;
    int i=0,count;//rで半径指定 a*r=v
    //ticker.attach(&cal,0.05);
    if(X_count ==1) {
        printf("set ok");
        EC.reset();
    }
    //角度リセット
    a=EC.getOmega();
    count=EC.getCount();
    i =count%500;
    printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
    f=0;
    b=0;
    led1=0;
    led2=1;
    led3=1;
    led4=0;

}
void shot()
{
    X.rise(&Xcount);
    f.period_us(50);
    b.period_us(50);
    double a=0,r=0.4,v=0;
    int i=0,count;//rで半径指定 a*r=v
    ticker.attach(&cal,0.05);
    while(1) {
        printf("set");
        if(X_count ==1) {
            EC.reset();
            X_count =0;
        }
        break;
    }
    //角度リセット
    while(1) {
        printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
        a=EC.getOmega();
        count=EC.getCount();
        i =count%500;
        v=a*r;
        f=0.45;//速度一定
        b=0.0;
        led1=1;
        led2=0;
        led3=0;
        led4=1;
        if(X_count>3) {
            if(i < -380 && i > -390) {//離す角度を決める
                out2=1; //ボールを離す
                printf("shot\r\n");
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                break;
            }
        }
    }
    printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
    X.rise(&Xcount);
    a=EC.getOmega();
    count=EC.getCount();
    i =count%500;
    f=0;
    b=0;
    led1=0;
    led2=1;
    led3=1;
    led4=0;

}

void catch_ball()
{
    switch (shoot) {
        case 2:
            if(ball ==0) {
                servo.set_degree(id, first);
                wait(0.5);
                shoot =0;
                ball = 1;
            } else if(ball ==1) {
                servo.set_degree(id, grab);
                wait(0.5);
                shoot = 0;
                ball =0;
            }
            break;
        case 3:
            servo.set_degree(id2, grab2);
            break;
        case 4:
            if (out1==0) {
                out1=1;
                shoot=0;
            } else if (out1 == 1) {
                out1=0;
                shoot=0;
            }
            break;
        case 5:
            if (out3==0) {
                out3=1;
            } else if (out3 == 1) {
                out3=0;
            }
    }
}

void catch_box()
{

}