File content as of revision 27:7a8cc28eeb42:

#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,ball_count=0,box_count=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);

DigitalIn touchSensor(p14);//タッチセンサー

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

int id  = 0;//ballbreak;
int id2 = 1;//box
double SERVO2DEG = 270.0 / (11500 - 3500);
double first_ball = (4500 - 3500) * SERVO2DEG;
double first_box = (6000 - 3500) * SERVO2DEG;  //X haven't be decided
double grab_ball = (3850 - 3500) * SERVO2DEG;
double pass = (7080 - 3500) * SERVO2DEG;
double grab_box = (3500 - 3500) * SERVO2DEG;

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

void ballShot(int data);
void catch_ball();
void catch_box();
void show_angle();
int shot_but;
int X_count=0;
void Xcount()
{
    X_count++;
}

void UserLoopSetting()
{
    f.period_us(50);
    b.period_us(50);
    touchSensor.mode(PullUp);
    X.rise(&Xcount);
    out1=1;
}
void UserLoop(char n,const u8* data)
{
    static int i=0;
    if(i==0) {
        wait(1);
        i++;
    }
    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;
    //reset

    if((ButtonState >> BUTTONUP)&1 == 1 &&(ButtonState >> BUTTONTRIANGEL)&1 == 1 ) {//R2
        NVIC_SystemReset();
        move =14;
        printf("reset\r\n");
    }

    //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 >> BUTTONSTART)&1 == 1 ) {//対応するボタンを書く(今回startボタン
        move = 12;
    } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
        move = 13;
    } else {
        move = 5;
    }
    //move
    //shoot
    if((ButtonState >> BUTTONCIRCLE)&1 == 1 && (ButtonState >> BUTTONLEFT)&1 == 1 ) {//O & left shot reset
        shoot =0;
        shot_but = 1;
        printf("shoot 0\r\n");
        led2 = 1;
    } else if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
        shoot = shoot +1;
        shot_but = 1;
        if(shoot > 3) {
            shoot =0;
        }
        printf("shoot %d\r\n",shoot);
        wait(0.5);
        led1 = 1;
    }  else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
        ball = 1;
        printf("ball 1\r\n");
        led2 = 1;
    } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
        box = 1;
        printf("box 1\r\n");
        led3 = 1;
    } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
        ball = 2;
        printf("ball 2\r\n");
        led4 = 1;
    } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
        box = 2;
        printf("box 2\r\n");
        led4 = 1;
    }

    //CAN通信用プログラム
    if(shot_but!=0)ballShot(shoot);
    catch_ball();
    catch_box();
    can[0] = move;
    controller.write(CANMessage(1,can,1));
    printf("%d\r\n",can[0]);
}


void ballShot(int data)
{
    static int breaknum=0;
    X.rise(&Xcount);
    int i=0,count;//rで半径指定 a*r=v
    ticker.attach(&cal,0.05);
    switch(data) {
        case 1:
            if(breaknum==0) {
                out2=1;
                out1=1;
                printf("%d\r\n",touchSensor.read());
                printf("%d\r\n",EC.getCount());
                f=0;
                b=0.07;
                if(touchSensor==0) {//受け渡し
                    f=0;
                    b=0;
                    printf("%d",EC.getCount());
                    EC.reset();
                    printf("%d",EC.getCount());
                    breaknum=1;
                    shot_but = 0;
                }
            }
            break;

        case 2:
            if(breaknum ==1) {
                out1=1;
                wait(1);
                out2=0;
                wait(0.5);
                breaknum++;
            }
            f=0.1;
            b=0;
            printf("moving to remove servo\r\n");
            if(EC.getCount()>200) {
                printf("servo move\r\n");
                f=0;
                b=0;
                servo.set_degree(id, first_ball);
                EC.reset();
                break;
            }
            shot_but= 0;
            break;
        case 3:
            X_count=0;
            double m=0;
            while(1) {
                count=EC.getCount();
                i =count%500;
                if(m<1.0) {
                    m=m+0.05;
                }
                if(m>1) {
                    m=1;
                }
                f=0.35*m;//速度一定
                b=0.0;
                if(X_count>2) {
                    if(i > 345 && i < 355) {//離す角度を決める
                        out1=1; //ボールを離す
                        f=0;
                        b=0;
                        printf("shot\r\n");
                        led1=1;
                        led2=1;
                        led3=1;
                        led4=1;
                        break;
                    }
                }
            }
            X.rise(&Xcount);
            count=EC.getCount();
            i =count%500;
            f=0;
            b=0;
            led1=0;
            led2=1;
            led3=1;
            led4=0;
            shoot=0;
            breaknum=0;
            shot_but=0;
            break;
    }
}

void catch_ball()
{
    switch (ball) {
        case 1: //ball servo
            if(ball_count ==0) {
                printf("pass\r\n");
                servo.set_degree(id,pass);
                wait(0.5);
                ball_count =1;
                ball = 0;
            } else if(ball_count ==1) {
                printf("glab\r\n");
                servo.set_degree(id, first_ball);
                wait(0.5);
                ball_count = 2;
                ball =0;
            } else if(ball_count ==2) {
                printf("first");
                servo.set_degree(id, grab_ball);
                wait(0.5);
                ball_count = 0;
                ball =0;
            }
            break;
        case 2://ball air
            if (out1==0) {
                out1=1;
                ball=0;
                wait(0.5);
            } else if (out1 == 1) {
                out1=0;
                ball=0;
                wait(0.5);
            }
            break;
    }
}

void catch_box()
{
    switch(box) {
        case 1:// box servo
            if(box_count ==0) {
                printf("Box first");
                servo.set_degree(id2,first_box);
                wait(0.5);
                box_count =1;
                box = 0;
            } else if(box_count ==1) {
                printf("Box grab");
                servo.set_degree(id2,grab_box);
                wait(0.5);
                box_count = 0;
                box =0;
            }
            break;
        case 2: //box air
            if (out3==0) {
                out3=1;
                box = 0;
                wait(0.5);
            } else if (out3 == 1) {
                out3=0;
                box = 0;
                wait(0.5);
            }
            break;
    }
}