File content as of revision 24:674b011da45b:

#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  = 1;//ballbreak;
int id2 = 0;//box
double SERVO2DEG = 270.0 / (11500 - 3500);
double first_ball = (5000 - 3500) * SERVO2DEG;
double first_box = (5000 - 3500) * SERVO2DEG;  //X haven't be decided
double grab_ball = (3500 - 3500) * SERVO2DEG;
double pass = (8000 - 3500) * SERVO2DEG;
double grab_box = (3500 - 3500) * SERVO2DEG;

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

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

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

void UserLoopSetting()
{
    f.period_us(50);
    b.period_us(50);
    touchSensor.mode(PullUp);
}
void UserLoop(char n,const u8* data)
{
    static int i=0;
    if(i==0) {
        wait(2);
        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;

    //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 ) {//対応するボタンを書く(今回SELLECTボタン
        move = 12;
    } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
        move = 13;
    } else {
        move = 5;
    }
    //move
    //shoot
    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
        shoot = 1;
        printf("shoot 1\r\n");
        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;
    }
    show_angle();
    //CAN通信用プログラム
    if(shoot==1)shot();
    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()
{
    out1=1;
    int breaknum=0;
    X.rise(&Xcount);
    double a=0,r=0.4,v=0;
    int i=0,count;//rで半径指定 a*r=v
    ticker.attach(&cal,0.05);
    while(1) {
        printf("%d\r\n",touchSensor.read());
        f=0;
        b=0.05;
        if(touchSensor==0) {//受け渡し
            f=0;
            b=0;
            printf("%d",EC.getCount());
            EC.reset();
            printf("%d",EC.getCount());
            wait(2);
            out1=0;
            wait(1);
            out2=0;
            wait(1);
            printf("passed\r\n");
            wait(1);
            while(1) {
                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);
                    breaknum=1;
                    break;
                }

            }
            if(X_count ==1) {
                EC.reset();
                X_count =0;
            }
        }
        if(breaknum==1)break;
    }
    EC.reset();
    wait(1);
    X_count=0;
    double m=0;
    //角度リセット
    while(1) {
        //printf("%.3f %d %d\r\n",v,X_count,i);
        //a=EC.getOmega();
        count=EC.getCount();
        i =count%500;
        //v=a*r;
        if(m<1.0) {
            m=m+0.05;
        }
        if(m>1) {
            m=1;
        }
        f=0.45*m;//速度一定
        b=0.0;
        if(X_count>2) {
            if(i > 345 && i < 355) {//離す角度を決める
                out1=1; //ボールを離す
                f=0;
                b=0;
                wait(1);
                printf("shot\r\n");
                led1=1;
                led2=1;
                led3=1;
                led4=1;
                break;
            }
        }
    }
    printf("%.3f %.3f %d\r\n",a,v,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;
    shoot=0;
}
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;
            } else if (out1 == 1) {
                out1=0;
                ball=0;
            }
            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;
            } else if (out3 == 1) {
                out3=0;
                box = 0;
            }
            break;
    }
}