User.cpp

00001 #include "Utils.h"
00002 #include "USBHost.h"
00003 #include "hci.h"
00004 #include "ps3.h"
00005 #include "User.h"
00006 
00007 #include "mbed.h"
00008 #include "math.h"
00009 
00010 #define PI 3.141592
00011 
00012 int RSX,RSY,LSX,LSY,BSU,BSL;
00013 
00014 CAN can1(p30,p29);
00015 DigitalOut cansend_led(LED1);
00016 DigitalOut canread_led(LED2);
00017 
00018 PwmOut fullcolor_g(p23);  //green
00019 PwmOut fullcolor_r(p22);  //red
00020 PwmOut fullcolor_b(p21);  //blue
00021 
00022 DigitalOut checkled1(p16);
00023 DigitalOut checkled2(p14);
00024 DigitalOut checkled3(p12);
00025 DigitalOut checkled4(p10);
00026 
00027 DigitalIn in_right(p20);
00028 DigitalIn in_left(p18);
00029 
00030 //Ticker can_ticker;  //タイマー割り込み
00031 char can_ashidata[4]= {0}; //CAN送信用の配列
00032 char can_armdata[2]= {0};
00033 
00034 int can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6;
00035 int can_armdata0_01=0,can_armdata0_23=0,can_armdata0_4=0,can_armdata0_5=0,can_armdata0_6=0,can_armdata0_7=0;
00036 int can_armdata1_0=0,can_armdata1_12=0,can_armdata1_34=0;
00037 int old_select_state,now_select_state,old_start_state,now_start_state,state_count;
00038 
00039 double alpha1,alpha2;
00040 int theta;
00041 
00042 int old_button1,now_button1;
00043 int old_button2,now_button2;
00044 int old_button3,now_button3;
00045 int old_button4,now_button4;
00046 int old_button5,now_button5;
00047 int old_button6,now_button6;
00048 int old_button7,now_button7;
00049 int old_button8,now_button8;
00050 int flag1,flag2,flag3,flag4,flag5,flag6,flag7,flag8;
00051 
00052 int t1=0,t2=0,t3=0;
00053 
00054 void can_sendread()  //割込み使ってない
00055 {
00056     if(can1.write(CANMessage(1,can_ashidata,4))) {  //IDを1にして3バイト送信
00057         printf("ashi[0],start&select=%d R1=%d RS=%d LS=%d TOGGLE=%d can_ashidata[3]=%d\n\r"
00058                ,can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6,can_ashidata[3]);
00059     }
00060 
00061     if(can1.write(CANMessage(2,can_armdata,2))) {  //IDを2にして2バイト送信
00062         cansend_led = 1;
00063         printf("arm,CIRCLE=%d CROSS=%d SQUARE=%d TRIANGEL=%d RIGHT=%d DOWN=%d UP=%d LEFT=%d\n\r"
00064                ,can_armdata0_01,can_armdata0_23,can_armdata0_4,can_armdata0_5,can_armdata0_6,can_armdata0_7,can_armdata1_0,can_armdata1_12);
00065 
00066     } else {
00067         cansend_led = 0;
00068         printf("not send\n\r");
00069     }
00070 
00071     CANMessage msg;
00072 
00073     if(can1.read(msg)) {
00074 
00075         if(msg.id == 4) {  //from ashi
00076             checkled1 = (msg.data[0]>>7)%2;  //-1<now_angle<1 off/on(0 or 1)
00077             checkled2 = (msg.data[0]>>6)%2;  //350<now_angle off/on(0 or 1)
00078             checkled3 = (msg.data[0]>>5)%2;  //-1<now_x<1 off/on(0 or 1)
00079             checkled4 = (msg.data[0]>>4)%2;  //-1<now_y<1 off/on(0 or 1)
00080             t1 = msg.data[1];                //value of t
00081             printf("[0]=%d t1=%d\n\r",msg.data[0],t1);
00082         }
00083 
00084         if(msg.id == 5) {  //from arm
00085             t2 = msg.data[0];
00086             canread_led = 1;
00087             printf("t2 = %d\n\r",t2);
00088         }
00089         
00090     } else {
00091         canread_led = 0;
00092         printf("not read\n\r");
00093     }
00094 
00095     if(t1 >= t2) {
00096         can_ashidata[3] = t1;  //from ashi
00097     } else if(t1 < t2) {
00098         can_ashidata[3] = t2;  //from arm
00099     }
00100 
00101 }
00102 
00103 void UserLoopSetting()
00104 {
00105     in_right.mode(PullUp);
00106     in_left.mode(PullUp);
00107 
00108     old_select_state=0;
00109     now_select_state=0;
00110     old_start_state=0;
00111     now_start_state=0;
00112     state_count=0;
00113     can1.frequency(1000000);
00114     //can_ticker.attach(&can_send,0.01);
00115 }
00116 
00117 void UserLoop(char n,const u8* data)
00118 {
00119     u16 ButtonState;
00120     if(n==0) { //有線Ps3USB.cpp
00121         RSX = ((ps3report*)data)->RightStickX;
00122         RSY = ((ps3report*)data)->RightStickY;
00123         LSX = ((ps3report*)data)->LeftStickX;
00124         LSY = ((ps3report*)data)->LeftStickY;
00125         BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
00126         BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
00127         //ボタンの処理
00128         ButtonState =  ((ps3report*)data)->ButtonState;
00129     } else {//無線TestShell.cpp
00130         RSX = ((ps3report*)(data + 1))->RightStickX;
00131         RSY = ((ps3report*)(data + 1))->RightStickY;
00132         LSX = ((ps3report*)(data + 1))->LeftStickX;
00133         LSY = ((ps3report*)(data + 1))->LeftStickY;
00134         BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
00135         BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
00136         //ボタンの処理
00137         ButtonState =  ((ps3report*)(data + 1))->ButtonState;
00138     }
00139     //ここより下にプログラムを書く
00140 
00141 //-----モード切替--------------------------------------------------------------------------------------------------------------//
00142 
00143 //selectを押してstate_countを増やす（手動方向）-------------
00144     old_select_state=now_select_state;
00145     if(((ButtonState >> BUTTONSELECT)&1) == 1)now_select_state  = 1;
00146     else now_select_state  = 0;
00147 
00148     if(old_select_state==0&&now_select_state==1&&state_count<1)state_count++;
00149 //-----------------------------------------------------
00150 
00151 //startを押してstate_countを減らす（自動方向）--------------
00152     old_start_state=now_start_state;
00153     if(((ButtonState >> BUTTONSTART)&1) == 1)now_start_state = 1;
00154     else now_start_state= 0;
00155 
00156     if(old_start_state==0&&now_start_state==1&&state_count>-1)state_count--;
00157 //-----------------------------------------------------
00158 
00159     //printf("select:%d start:%d  ",now_select_state,now_start_state);
00160 
00161 //can通信で送る&------------------------------------------
00162 
00163     if(state_count>0) { //manual mode
00164         fullcolor_g = 1;  //white
00165         fullcolor_r = 1;
00166         fullcolor_b = 1;
00167         can_ashidata0_01 = 2;
00168         can_armdata1_34 = 2;
00169     } else if(state_count<0) { //auto mode
00170         fullcolor_g = 1;  //green
00171         fullcolor_r = 0;
00172         fullcolor_b = 0;
00173         can_ashidata0_01 = 1;
00174         can_armdata1_34 = 1;
00175     } else { //wait mode
00176         fullcolor_g = 0.5;  //orange
00177         fullcolor_r = 1;
00178         fullcolor_b = 0;
00179         can_ashidata0_01 = 0;
00180         can_armdata1_34 = 0;
00181     }
00182 
00183 //-----ジョイスティック&ボタン入力をCANで送る----------------------------------------------------------------------------------------------//
00184 
00185     //ジョイスティックの入力を角度に直す
00186     alpha1 = atan( (127.5-LSX) / (127.5-LSY) );
00187     alpha2 = (alpha1 / PI)*180;
00188 
00189     if(LSX >127.5 && LSY >127.5) {
00190         theta = alpha2;  //x=127.5 かつ y>127.5 の直線を0°としてθをとる
00191     } else if(LSX <127.5 && LSY >127.5) {
00192         theta = 360 + alpha2;
00193     } else if(LSY <127.5) {
00194         theta = 180 + alpha2;
00195     }
00196 
00197     //printf("LSX=%d LSY=%d alpha1=%f alpha2=%f theta=%d\n\r",LSX,LSY,alpha1,alpha2,theta);
00198 
00199     can_ashidata[1] = theta>>8;
00200     can_ashidata[2] = theta&255;
00201 
00202     if(((ButtonState >> BUTTONR1)&1) == 1) {  //buttonR1 on
00203         can_ashidata0_2 = 1;
00204     } else {
00205         can_ashidata0_2 = 0;
00206     }
00207 
00208     if(RSX >=100 && RSX <150) {
00209         can_ashidata0_34 = 1;  //not rotate
00210     } else if(RSX >=150) {
00211         can_ashidata0_34 = 2;  //right rotation
00212     } else if(RSX <100) {
00213         can_ashidata0_34 = 3;  //left rotation
00214     }
00215 
00216     if(LSX >=100 && LSX <150 && LSY >=100 && LSY <150) { //left joystick neutral position
00217         can_ashidata0_5 = 1;
00218     } else {
00219         can_ashidata0_5 = 0;
00220     }
00221 
00222     can_ashidata[0] = (can_ashidata0_01<<6 | can_ashidata0_2<<5 | can_ashidata0_34<<3 | can_ashidata0_5<<2 | can_ashidata0_6<<1);
00223 
00224 //-----〇ボタン motorR---------------------------------------------------------
00225     old_button1=now_button1;
00226     if(((ButtonState >> BUTTONCIRCLE)&1) == 1) {
00227         now_button1 = 1;
00228     } else {
00229         now_button1 = 0;
00230     }
00231 
00232     if(old_button1==0 &&now_button1==1 &&flag1==0) {
00233         can_armdata0_01 = 1;
00234         flag1 = 1;
00235     } else if(old_button1==0 &&now_button1==1 &&flag1==1) {
00236         can_armdata0_01 = 2;
00237         flag1 = 0;
00238     }
00239 
00240 //-----×ボタン motorL---------------------------------------------------------
00241     old_button2=now_button2;
00242     if(((ButtonState >> BUTTONCROSS)&1) == 1) {
00243         now_button2 = 1;
00244     } else {
00245         now_button2 = 0;
00246     }
00247 
00248     if(old_button2==0 &&now_button2==1 &&flag2==0) {
00249         can_armdata0_23 = 1;
00250         flag2 = 1;
00251     } else if(old_button2==0 &&now_button2==1 &&flag2==1) {
00252         can_armdata0_23 = 2;
00253         flag2 = 0;
00254     }
00255 
00256 //-----□ボタン armR---------------------------------------------------------
00257     old_button3=now_button3;
00258     if(((ButtonState >> BUTTONSQUARE)&1) == 1) {
00259         now_button3 = 1;
00260     } else {
00261         now_button3 = 0;
00262     }
00263 
00264     if(old_button3==0 &&now_button3==1 &&flag3==0) {
00265         can_armdata0_4 = 1;
00266         flag3 = 1;
00267     } else if(old_button3==0 &&now_button3==1 &&flag3==1) {
00268         can_armdata0_4 = 0;
00269         flag3 = 0;
00270     }
00271 
00272 //-----△ボタン armL---------------------------------------------------------
00273     old_button4=now_button4;
00274     if(((ButtonState >> BUTTONTRIANGEL)&1) == 1) {
00275         now_button4 = 1;
00276     } else {
00277         now_button4 = 0;
00278     }
00279 
00280     if(old_button4==0 &&now_button4==1 &&flag4==0) {
00281         can_armdata0_5 = 1;
00282         flag4 = 1;
00283     } else if(old_button4==0 &&now_button4==1 &&flag4==1) {
00284         can_armdata0_5 = 0;
00285         flag4 = 0;
00286     }
00287 
00288 //-----RIGHTボタン sanpou---------------------------------------------------------
00289     old_button5=now_button5;
00290     if(((ButtonState >> BUTTONRIGHT)&1) == 1) {
00291         now_button5 = 1;
00292     } else {
00293         now_button5 = 0;
00294     }
00295 
00296     if(old_button5==0 &&now_button5==1 &&flag5==0) {
00297         can_armdata0_6 = 1;
00298         flag5 = 1;
00299     } else if(old_button5==0 &&now_button5==1 &&flag5==1) {
00300         can_armdata0_6 = 0;
00301         flag5 = 0;
00302     }
00303 
00304 //-----DOWNボタン under_sanpou---------------------------------------------------------
00305     old_button6=now_button6;
00306     if(((ButtonState >> BUTTONDOWN)&1) == 1) {
00307         now_button6 = 1;
00308     } else {
00309         now_button6 = 0;
00310     }
00311 
00312     if(old_button6==0 &&now_button6==1 &&flag6==0) {
00313         can_armdata0_7 = 1;
00314         flag6 = 1;
00315     } else if(old_button6==0 &&now_button6==1 &&flag6==1) {
00316         can_armdata0_7 = 0;
00317         flag6 = 0;
00318     }
00319 
00320 //-----UPボタン above_sanpou---------------------------------------------------------
00321     old_button7=now_button7;
00322     if(((ButtonState >> BUTTONUP)&1) == 1) {
00323         now_button7 = 1;
00324     } else {
00325         now_button7 = 0;
00326     }
00327 
00328     if(old_button7==0 &&now_button7==1 &&flag7==0) {
00329         can_armdata1_0 = 1;
00330         flag7 = 1;
00331     } else if(old_button7==0 &&now_button7==1 &&flag7==1) {
00332         can_armdata1_0 = 0;
00333         flag7 = 0;
00334     }
00335 
00336 //-----LEFTボタン motorC---------------------------------------------------------
00337     old_button8=now_button8;
00338     if(((ButtonState >> BUTTONLEFT)&1) == 1) {
00339         now_button8 = 1;
00340     } else {
00341         now_button8 = 0;
00342     }
00343 
00344     if(old_button8==0 &&now_button8==1 &&flag8==0) {
00345         can_armdata1_12 = 1;
00346         flag8 = 1;
00347     } else if(old_button8==0 &&now_button8==1 &&flag8==1) {
00348         can_armdata1_12 = 2;
00349         flag8 = 0;
00350     }
00351 
00352 //-----TOGGLE SWITCH---------------------------------------------------------
00353     //printf("R=%d L=%d\n\r",in_right.read(),in_left.read());
00354 
00355     if(in_right.read() == 1 && in_left.read() == 0) { //right mode
00356         can_ashidata0_6 = 0;
00357     } else { //left mode
00358         can_ashidata0_6 = 1;
00359     }
00360 
00361     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);
00362     can_armdata[1] = (can_armdata1_0<<7 | can_armdata1_12<<5 | can_armdata1_34<<3);
00363     //printf("datasend %d,%d,%d",can_ashidata[0],can_armdata[0],can_armdata[1]);
00364 
00365 
00366 
00367     can_sendread();
00368 }