Diff: User.cpp

Revision:

0:a56be39653d0

Child:

1:530908de68c6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/User.cpp	Wed Feb 27 04:58:52 2019 +0000
@@ -0,0 +1,380 @@
+#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();
+}
\ No newline at end of file