File content as of revision 0:a56be39653d0:

#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#include "ps3.h"
#include "User.h"

#include "mbed.h"
#include "math.h"

#define PI 3.141592

int RSX,RSY,LSX,LSY,BSU,BSL;

CAN can1(p30,p29);
DigitalOut cansend_led(LED1);
DigitalOut canread_led(LED2);

PwmOut fullcolor_g(p23);  //green
PwmOut fullcolor_r(p22);  //red
PwmOut fullcolor_b(p21);  //blue

DigitalOut checkled1(p16);
DigitalOut checkled2(p14);
DigitalOut checkled3(p12);
DigitalOut checkled4(p10);

DigitalIn in_right(p20);
DigitalIn in_left(p18);

//Ticker can_ticker;  //タイマー割り込み
char can_ashidata[3]= {0}; //CAN送信用の配列
char can_armdata[2]= {0};

int can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6;
int can_armdata0_01,can_armdata0_23,can_armdata0_4,can_armdata0_5,can_armdata0_6,can_armdata0_7;
int can_armdata1_0,can_armdata1_12,can_armdata1_34;
int old_select_state,now_select_state,old_start_state,now_start_state,state_count;

double alpha1,alpha2;
int theta;

int old_button1,now_button1;
int old_button2,now_button2;
int old_button3,now_button3;
int old_button4,now_button4;
int old_button5,now_button5;
int old_button6,now_button6;
int old_button7,now_button7;
int old_button8,now_button8;
int flag1,flag2,flag3,flag4,flag5,flag6,flag7,flag8;

int t1 = 0, t2 = 0, t3 = 0;
char final_t[1] = {0};

void can_sendread()  //割込み使ってない
{
    if(can1.write(CANMessage(1,can_ashidata,3))) {  //IDを1にして3バイト送信
        cansend_led = 1;
        printf("ashi[0],start&select=%d R1=%d RS=%d LS=%d TOGGLE=%d\n\r"
               ,can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6);
    }

    if(can1.write(CANMessage(2,can_armdata,2))) {  //IDを2にして2バイト送信
        cansend_led = 1;
        printf("arm,CIRCLE=%d CROSS=%d SQUARE=%d TRIANGEL=%d RIGHT=%d DOWN=%d UP=%d LEFT=%d\n\r"
               ,can_armdata0_01,can_armdata0_23,can_armdata0_4,can_armdata0_5,can_armdata0_6,can_armdata0_7,can_armdata1_0,can_armdata1_12);
    }
    
    if(can1.write(CANMessage(8,final_t,1))) {  //IDを8にして1バイト送信
        cansend_led = 1;

    } else {
        cansend_led = 0;
        printf("not send\n\r");
    }


    CANMessage msg;

    if(can1.read(msg)) {
        canread_led = 1;

        if(msg.id == 7) { //from ashi
            checkled1 = (msg.data[0]>>7 | 00000001);  //-1<now_angle<1 off/on(0 or 1)
            checkled2 = (msg.data[0]>>6 | 00000001);  //350<now_angle off/on(0 or 1)
            checkled3 = (msg.data[0]>>5 | 00000001);  //-1<now_x<1 off/on(0 or 1)
            checkled4 = (msg.data[0]>>4 | 00000001);  //-1<now_y<1 off/on(0 or 1)
        }

        if(msg.id == 9) {
            t1 = msg.data[0];
        }

        if(msg.id == 10) {
            t2 = msg.data[0];
        }

        /*if(msg.id == 11){
            t3 = msg.data[0];
        }*/

    } else {
        canread_led = 0;
    }

    if(t2 >= t3) {
        final_t[0] = t2;
    } else if(t2 < t3) {
        final_t[0] = t3;
    }

}

void UserLoopSetting()
{

    in_right.mode(PullUp);
    in_left.mode(PullUp);

    old_select_state=0;
    now_select_state=0;
    old_start_state=0;
    now_start_state=0;
    state_count=0;
    can1.frequency(1000000);
    //can_ticker.attach(&can_send,0.01);
}

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;
    }
    //ここより下にプログラムを書く

//-----モード切替--------------------------------------------------------------------------------------------------------------//

//selectを押してstate_countを増やす（手動方向）-------------
    old_select_state=now_select_state;
    if(((ButtonState >> BUTTONSELECT)&1) == 1)now_select_state  = 1;
    else now_select_state  = 0;

    if(old_select_state==0&&now_select_state==1&&state_count<1)state_count++;
//-----------------------------------------------------

//startを押してstate_countを減らす（自動方向）--------------
    old_start_state=now_start_state;
    if(((ButtonState >> BUTTONSTART)&1) == 1)now_start_state = 1;
    else now_start_state= 0;

    if(old_start_state==0&&now_start_state==1&&state_count>-1)state_count--;
//-----------------------------------------------------

    //printf("select:%d start:%d  ",now_select_state,now_start_state);

//can通信で送る&------------------------------------------

    if(state_count>0) { //manual mode
        fullcolor_g = 1;  //white
        fullcolor_r = 1;
        fullcolor_b = 1;
        can_ashidata0_01 = 2;
        can_armdata1_34 = 2;
    } else if(state_count<0) { //auto mode
        fullcolor_g = 1;  //green
        fullcolor_r = 0;
        fullcolor_b = 0;
        can_ashidata0_01 = 1;
        can_armdata1_34 = 1;
    } else { //wait mode
        fullcolor_g = 0.5;  //orange
        fullcolor_r = 1;
        fullcolor_b = 0;
        can_ashidata0_01 = 0;
        can_armdata1_34 = 0;
    }

//-----ジョイスティック&ボタン入力をCANで送る----------------------------------------------------------------------------------------------//

    //ジョイスティックの入力を角度に直す
    alpha1 = atan( (127.5-LSX) / (127.5-LSY) );
    alpha2 = (alpha1 / PI)*180;

    if(LSX >127.5 && LSY >127.5) {
        theta = alpha2;  //x=127.5 かつ y>127.5 の直線を0°としてθをとる
    } else if(LSX <127.5 && LSY >127.5) {
        theta = 360 + alpha2;
    } else if(LSY <127.5) {
        theta = 180 + alpha2;
    }

    //printf("LSX=%d LSY=%d alpha1=%f alpha2=%f theta=%d\n\r",LSX,LSY,alpha1,alpha2,theta);

    can_ashidata[1] = theta>>8;
    can_ashidata[2] = theta&255;

    if(((ButtonState >> BUTTONR1)&1) == 1) {  //buttonR1 on
        can_ashidata0_2 = 1;
    } else {
        can_ashidata0_2 = 0;
    }

    if(RSX >=100 && RSX <150) {
        can_ashidata0_34 = 1;  //not rotate
    } else if(RSX >=150) {
        can_ashidata0_34 = 2;  //right rotation
    } else if(RSX <100) {
        can_ashidata0_34 = 3;  //left rotation
    }

    if(LSX >=100 && LSX <150 && LSY >=100 && LSY <150) { //left joystick neutral position
        can_ashidata0_5 = 1;
    } else {
        can_ashidata0_5 = 0;
    }

    can_ashidata[0] = (can_ashidata0_01<<6 | can_ashidata0_2<<5 | can_ashidata0_34<<3 | can_ashidata0_5<<2 | can_ashidata0_6<<1);

//-----〇ボタン motorR---------------------------------------------------------
    old_button1=now_button1;
    if(((ButtonState >> BUTTONCIRCLE)&1) == 1) {
        now_button1 = 1;
    } else {
        now_button1 = 0;
    }

    if(old_button1==0 &&now_button1==1 &&flag1==0) {
        can_armdata0_01 = 1;
        flag1 = 1;
    } else if(old_button1==0 &&now_button1==1 &&flag1==1) {
        can_armdata0_01 = 2;
        flag1 = 0;
    }

//-----×ボタン motorL---------------------------------------------------------
    old_button2=now_button2;
    if(((ButtonState >> BUTTONCROSS)&1) == 1) {
        now_button2 = 1;
    } else {
        now_button2 = 0;
    }

    if(old_button2==0 &&now_button2==1 &&flag2==0) {
        can_armdata0_23 = 1;
        flag2 = 1;
    } else if(old_button2==0 &&now_button2==1 &&flag2==1) {
        can_armdata0_23 = 2;
        flag2 = 0;
    }

//-----□ボタン armR---------------------------------------------------------
    old_button3=now_button3;
    if(((ButtonState >> BUTTONSQUARE)&1) == 1) {
        now_button3 = 1;
    } else {
        now_button3 = 0;
    }

    if(old_button3==0 &&now_button3==1 &&flag3==0) {
        can_armdata0_4 = 0;
        flag3 = 1;
    } else if(old_button3==0 &&now_button3==1 &&flag3==1) {
        can_armdata0_4 = 1;
        flag3 = 0;
    }

//-----△ボタン armL---------------------------------------------------------
    old_button4=now_button4;
    if(((ButtonState >> BUTTONTRIANGEL)&1) == 1) {
        now_button4 = 1;
    } else {
        now_button4 = 0;
    }

    if(old_button4==0 &&now_button4==1 &&flag4==0) {
        can_armdata0_5 = 0;
        flag4 = 1;
    } else if(old_button4==0 &&now_button4==1 &&flag4==1) {
        can_armdata0_5 = 1;
        flag4 = 0;
    }

//-----RIGHTボタン sanpou---------------------------------------------------------
    old_button5=now_button5;
    if(((ButtonState >> BUTTONRIGHT)&1) == 1) {
        now_button5 = 1;
    } else {
        now_button5 = 0;
    }

    if(old_button5==0 &&now_button5==1 &&flag5==0) {
        can_armdata0_6 = 0;
        flag5 = 1;
    } else if(old_button5==0 &&now_button5==1 &&flag5==1) {
        can_armdata0_6 = 1;
        flag5 = 0;
    }

//-----DOWNボタン under_sanpou---------------------------------------------------------
    old_button6=now_button6;
    if(((ButtonState >> BUTTONDOWN)&1) == 1) {
        now_button6 = 1;
    } else {
        now_button6 = 0;
    }

    if(old_button6==0 &&now_button6==1 &&flag6==0) {
        can_armdata0_7 = 0;
        flag6 = 1;
    } else if(old_button6==0 &&now_button6==1 &&flag6==1) {
        can_armdata0_7 = 1;
        flag6 = 0;
    }

//-----UPボタン above_sanpou---------------------------------------------------------
    old_button7=now_button7;
    if(((ButtonState >> BUTTONUP)&1) == 1) {
        now_button7 = 1;
    } else {
        now_button7 = 0;
    }

    if(old_button7==0 &&now_button7==1 &&flag7==0) {
        can_armdata1_0 = 0;
        flag7 = 1;
    } else if(old_button7==0 &&now_button7==1 &&flag7==1) {
        can_armdata1_0 = 1;
        flag7 = 0;
    }

//-----LEFTボタン motorC---------------------------------------------------------
    old_button8=now_button8;
    if(((ButtonState >> BUTTONLEFT)&1) == 1) {
        now_button8 = 1;
    } else {
        now_button8 = 0;
    }

    if(old_button8==0 &&now_button8==1 &&flag8==0) {
        can_armdata1_12 = 1;
        flag8 = 1;
    } else if(old_button8==0 &&now_button8==1 &&flag8==1) {
        can_armdata1_12 = 2;
        flag8 = 0;
    }

//-----TOGGLE SWITCH---------------------------------------------------------
    //printf("R=%d L=%d\n\r",in_right.read(),in_left.read());

    if(in_right.read() == 1 && in_left.read() == 0) { //right mode
        can_ashidata0_6 = 0;
    } else { //left mode
        can_ashidata0_6 = 1;
    }

    can_armdata[0] = (can_armdata0_01<<6 | can_armdata0_23<<4 | can_armdata0_4<<3 | can_armdata0_5<<2 | can_armdata0_6<<1 | can_armdata0_7);
    can_armdata[1] = (can_armdata1_0<<7 | can_armdata1_12<<5 | can_armdata1_34<<3);
    //printf("%d,%d,%d",can_ashidata[0],can_armdata[0],can_armdata[1]);



    can_sendread();
}