春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_PS3_3_5

3/5

Dependencies:   mbed

User.cpp@0:a56be39653d0, 2019-02-27 (annotated)

Committer:
la00noix
Date:
Wed Feb 27 04:58:52 2019 +0000
Revision:
0:a56be39653d0
Child:
1:530908de68c6
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:a56be39653d0 1 #include "Utils.h"
la00noix 0:a56be39653d0 2 #include "USBHost.h"
la00noix 0:a56be39653d0 3 #include "hci.h"
la00noix 0:a56be39653d0 4 #include "ps3.h"
la00noix 0:a56be39653d0 5 #include "User.h"
la00noix 0:a56be39653d0 6
la00noix 0:a56be39653d0 7 #include "mbed.h"
la00noix 0:a56be39653d0 8 #include "math.h"
la00noix 0:a56be39653d0 9
la00noix 0:a56be39653d0 10 #define PI 3.141592
la00noix 0:a56be39653d0 11
la00noix 0:a56be39653d0 12 int RSX,RSY,LSX,LSY,BSU,BSL;
la00noix 0:a56be39653d0 13
la00noix 0:a56be39653d0 14 CAN can1(p30,p29);
la00noix 0:a56be39653d0 15 DigitalOut cansend_led(LED1);
la00noix 0:a56be39653d0 16 DigitalOut canread_led(LED2);
la00noix 0:a56be39653d0 17
la00noix 0:a56be39653d0 18 PwmOut fullcolor_g(p23); //green
la00noix 0:a56be39653d0 19 PwmOut fullcolor_r(p22); //red
la00noix 0:a56be39653d0 20 PwmOut fullcolor_b(p21); //blue
la00noix 0:a56be39653d0 21
la00noix 0:a56be39653d0 22 DigitalOut checkled1(p16);
la00noix 0:a56be39653d0 23 DigitalOut checkled2(p14);
la00noix 0:a56be39653d0 24 DigitalOut checkled3(p12);
la00noix 0:a56be39653d0 25 DigitalOut checkled4(p10);
la00noix 0:a56be39653d0 26
la00noix 0:a56be39653d0 27 DigitalIn in_right(p20);
la00noix 0:a56be39653d0 28 DigitalIn in_left(p18);
la00noix 0:a56be39653d0 29
la00noix 0:a56be39653d0 30 //Ticker can_ticker; //タイマー割り込み
la00noix 0:a56be39653d0 31 char can_ashidata[3]= {0}; //CAN送信用の配列
la00noix 0:a56be39653d0 32 char can_armdata[2]= {0};
la00noix 0:a56be39653d0 33
la00noix 0:a56be39653d0 34 int can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6;
la00noix 0:a56be39653d0 35 int can_armdata0_01,can_armdata0_23,can_armdata0_4,can_armdata0_5,can_armdata0_6,can_armdata0_7;
la00noix 0:a56be39653d0 36 int can_armdata1_0,can_armdata1_12,can_armdata1_34;
la00noix 0:a56be39653d0 37 int old_select_state,now_select_state,old_start_state,now_start_state,state_count;
la00noix 0:a56be39653d0 38
la00noix 0:a56be39653d0 39 double alpha1,alpha2;
la00noix 0:a56be39653d0 40 int theta;
la00noix 0:a56be39653d0 41
la00noix 0:a56be39653d0 42 int old_button1,now_button1;
la00noix 0:a56be39653d0 43 int old_button2,now_button2;
la00noix 0:a56be39653d0 44 int old_button3,now_button3;
la00noix 0:a56be39653d0 45 int old_button4,now_button4;
la00noix 0:a56be39653d0 46 int old_button5,now_button5;
la00noix 0:a56be39653d0 47 int old_button6,now_button6;
la00noix 0:a56be39653d0 48 int old_button7,now_button7;
la00noix 0:a56be39653d0 49 int old_button8,now_button8;
la00noix 0:a56be39653d0 50 int flag1,flag2,flag3,flag4,flag5,flag6,flag7,flag8;
la00noix 0:a56be39653d0 51
la00noix 0:a56be39653d0 52 int t1 = 0, t2 = 0, t3 = 0;
la00noix 0:a56be39653d0 53 char final_t[1] = {0};
la00noix 0:a56be39653d0 54
la00noix 0:a56be39653d0 55 void can_sendread() //割込み使ってない
la00noix 0:a56be39653d0 56 {
la00noix 0:a56be39653d0 57 if(can1.write(CANMessage(1,can_ashidata,3))) { //IDを1にして3バイト送信
la00noix 0:a56be39653d0 58 cansend_led = 1;
la00noix 0:a56be39653d0 59 printf("ashi[0],start&select=%d R1=%d RS=%d LS=%d TOGGLE=%d\n\r"
la00noix 0:a56be39653d0 60 ,can_ashidata0_01,can_ashidata0_2,can_ashidata0_34,can_ashidata0_5,can_ashidata0_6);
la00noix 0:a56be39653d0 61 }
la00noix 0:a56be39653d0 62
la00noix 0:a56be39653d0 63 if(can1.write(CANMessage(2,can_armdata,2))) { //IDを2にして2バイト送信
la00noix 0:a56be39653d0 64 cansend_led = 1;
la00noix 0:a56be39653d0 65 printf("arm,CIRCLE=%d CROSS=%d SQUARE=%d TRIANGEL=%d RIGHT=%d DOWN=%d UP=%d LEFT=%d\n\r"
la00noix 0:a56be39653d0 66 ,can_armdata0_01,can_armdata0_23,can_armdata0_4,can_armdata0_5,can_armdata0_6,can_armdata0_7,can_armdata1_0,can_armdata1_12);
la00noix 0:a56be39653d0 67 }
la00noix 0:a56be39653d0 68
la00noix 0:a56be39653d0 69 if(can1.write(CANMessage(8,final_t,1))) { //IDを8にして1バイト送信
la00noix 0:a56be39653d0 70 cansend_led = 1;
la00noix 0:a56be39653d0 71
la00noix 0:a56be39653d0 72 } else {
la00noix 0:a56be39653d0 73 cansend_led = 0;
la00noix 0:a56be39653d0 74 printf("not send\n\r");
la00noix 0:a56be39653d0 75 }
la00noix 0:a56be39653d0 76
la00noix 0:a56be39653d0 77
la00noix 0:a56be39653d0 78 CANMessage msg;
la00noix 0:a56be39653d0 79
la00noix 0:a56be39653d0 80 if(can1.read(msg)) {
la00noix 0:a56be39653d0 81 canread_led = 1;
la00noix 0:a56be39653d0 82
la00noix 0:a56be39653d0 83 if(msg.id == 7) { //from ashi
la00noix 0:a56be39653d0 84 checkled1 = (msg.data[0]>>7 | 00000001); //-1<now_angle<1 off/on(0 or 1)
la00noix 0:a56be39653d0 85 checkled2 = (msg.data[0]>>6 | 00000001); //350<now_angle off/on(0 or 1)
la00noix 0:a56be39653d0 86 checkled3 = (msg.data[0]>>5 | 00000001); //-1<now_x<1 off/on(0 or 1)
la00noix 0:a56be39653d0 87 checkled4 = (msg.data[0]>>4 | 00000001); //-1<now_y<1 off/on(0 or 1)
la00noix 0:a56be39653d0 88 }
la00noix 0:a56be39653d0 89
la00noix 0:a56be39653d0 90 if(msg.id == 9) {
la00noix 0:a56be39653d0 91 t1 = msg.data[0];
la00noix 0:a56be39653d0 92 }
la00noix 0:a56be39653d0 93
la00noix 0:a56be39653d0 94 if(msg.id == 10) {
la00noix 0:a56be39653d0 95 t2 = msg.data[0];
la00noix 0:a56be39653d0 96 }
la00noix 0:a56be39653d0 97
la00noix 0:a56be39653d0 98 /*if(msg.id == 11){
la00noix 0:a56be39653d0 99 t3 = msg.data[0];
la00noix 0:a56be39653d0 100 }*/
la00noix 0:a56be39653d0 101
la00noix 0:a56be39653d0 102 } else {
la00noix 0:a56be39653d0 103 canread_led = 0;
la00noix 0:a56be39653d0 104 }
la00noix 0:a56be39653d0 105
la00noix 0:a56be39653d0 106 if(t2 >= t3) {
la00noix 0:a56be39653d0 107 final_t[0] = t2;
la00noix 0:a56be39653d0 108 } else if(t2 < t3) {
la00noix 0:a56be39653d0 109 final_t[0] = t3;
la00noix 0:a56be39653d0 110 }
la00noix 0:a56be39653d0 111
la00noix 0:a56be39653d0 112 }
la00noix 0:a56be39653d0 113
la00noix 0:a56be39653d0 114 void UserLoopSetting()
la00noix 0:a56be39653d0 115 {
la00noix 0:a56be39653d0 116
la00noix 0:a56be39653d0 117 in_right.mode(PullUp);
la00noix 0:a56be39653d0 118 in_left.mode(PullUp);
la00noix 0:a56be39653d0 119
la00noix 0:a56be39653d0 120 old_select_state=0;
la00noix 0:a56be39653d0 121 now_select_state=0;
la00noix 0:a56be39653d0 122 old_start_state=0;
la00noix 0:a56be39653d0 123 now_start_state=0;
la00noix 0:a56be39653d0 124 state_count=0;
la00noix 0:a56be39653d0 125 can1.frequency(1000000);
la00noix 0:a56be39653d0 126 //can_ticker.attach(&can_send,0.01);
la00noix 0:a56be39653d0 127 }
la00noix 0:a56be39653d0 128
la00noix 0:a56be39653d0 129 void UserLoop(char n,const u8* data)
la00noix 0:a56be39653d0 130 {
la00noix 0:a56be39653d0 131 u16 ButtonState;
la00noix 0:a56be39653d0 132 if(n==0) { //有線Ps3USB.cpp
la00noix 0:a56be39653d0 133 RSX = ((ps3report*)data)->RightStickX;
la00noix 0:a56be39653d0 134 RSY = ((ps3report*)data)->RightStickY;
la00noix 0:a56be39653d0 135 LSX = ((ps3report*)data)->LeftStickX;
la00noix 0:a56be39653d0 136 LSY = ((ps3report*)data)->LeftStickY;
la00noix 0:a56be39653d0 137 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
la00noix 0:a56be39653d0 138 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
la00noix 0:a56be39653d0 139 //ボタンの処理
la00noix 0:a56be39653d0 140 ButtonState = ((ps3report*)data)->ButtonState;
la00noix 0:a56be39653d0 141 } else {//無線TestShell.cpp
la00noix 0:a56be39653d0 142 RSX = ((ps3report*)(data + 1))->RightStickX;
la00noix 0:a56be39653d0 143 RSY = ((ps3report*)(data + 1))->RightStickY;
la00noix 0:a56be39653d0 144 LSX = ((ps3report*)(data + 1))->LeftStickX;
la00noix 0:a56be39653d0 145 LSY = ((ps3report*)(data + 1))->LeftStickY;
la00noix 0:a56be39653d0 146 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
la00noix 0:a56be39653d0 147 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
la00noix 0:a56be39653d0 148 //ボタンの処理
la00noix 0:a56be39653d0 149 ButtonState = ((ps3report*)(data + 1))->ButtonState;
la00noix 0:a56be39653d0 150 }
la00noix 0:a56be39653d0 151 //ここより下にプログラムを書く
la00noix 0:a56be39653d0 152
la00noix 0:a56be39653d0 153 //-----モード切替--------------------------------------------------------------------------------------------------------------//
la00noix 0:a56be39653d0 154
la00noix 0:a56be39653d0 155 //selectを押してstate_countを増やす（手動方向）-------------
la00noix 0:a56be39653d0 156 old_select_state=now_select_state;
la00noix 0:a56be39653d0 157 if(((ButtonState >> BUTTONSELECT)&1) == 1)now_select_state = 1;
la00noix 0:a56be39653d0 158 else now_select_state = 0;
la00noix 0:a56be39653d0 159
la00noix 0:a56be39653d0 160 if(old_select_state==0&&now_select_state==1&&state_count<1)state_count++;
la00noix 0:a56be39653d0 161 //-----------------------------------------------------
la00noix 0:a56be39653d0 162
la00noix 0:a56be39653d0 163 //startを押してstate_countを減らす（自動方向）--------------
la00noix 0:a56be39653d0 164 old_start_state=now_start_state;
la00noix 0:a56be39653d0 165 if(((ButtonState >> BUTTONSTART)&1) == 1)now_start_state = 1;
la00noix 0:a56be39653d0 166 else now_start_state= 0;
la00noix 0:a56be39653d0 167
la00noix 0:a56be39653d0 168 if(old_start_state==0&&now_start_state==1&&state_count>-1)state_count--;
la00noix 0:a56be39653d0 169 //-----------------------------------------------------
la00noix 0:a56be39653d0 170
la00noix 0:a56be39653d0 171 //printf("select:%d start:%d ",now_select_state,now_start_state);
la00noix 0:a56be39653d0 172
la00noix 0:a56be39653d0 173 //can通信で送る&------------------------------------------
la00noix 0:a56be39653d0 174
la00noix 0:a56be39653d0 175 if(state_count>0) { //manual mode
la00noix 0:a56be39653d0 176 fullcolor_g = 1; //white
la00noix 0:a56be39653d0 177 fullcolor_r = 1;
la00noix 0:a56be39653d0 178 fullcolor_b = 1;
la00noix 0:a56be39653d0 179 can_ashidata0_01 = 2;
la00noix 0:a56be39653d0 180 can_armdata1_34 = 2;
la00noix 0:a56be39653d0 181 } else if(state_count<0) { //auto mode
la00noix 0:a56be39653d0 182 fullcolor_g = 1; //green
la00noix 0:a56be39653d0 183 fullcolor_r = 0;
la00noix 0:a56be39653d0 184 fullcolor_b = 0;
la00noix 0:a56be39653d0 185 can_ashidata0_01 = 1;
la00noix 0:a56be39653d0 186 can_armdata1_34 = 1;
la00noix 0:a56be39653d0 187 } else { //wait mode
la00noix 0:a56be39653d0 188 fullcolor_g = 0.5; //orange
la00noix 0:a56be39653d0 189 fullcolor_r = 1;
la00noix 0:a56be39653d0 190 fullcolor_b = 0;
la00noix 0:a56be39653d0 191 can_ashidata0_01 = 0;
la00noix 0:a56be39653d0 192 can_armdata1_34 = 0;
la00noix 0:a56be39653d0 193 }
la00noix 0:a56be39653d0 194
la00noix 0:a56be39653d0 195 //-----ジョイスティック&ボタン入力をCANで送る----------------------------------------------------------------------------------------------//
la00noix 0:a56be39653d0 196
la00noix 0:a56be39653d0 197 //ジョイスティックの入力を角度に直す
la00noix 0:a56be39653d0 198 alpha1 = atan( (127.5-LSX) / (127.5-LSY) );
la00noix 0:a56be39653d0 199 alpha2 = (alpha1 / PI)*180;
la00noix 0:a56be39653d0 200
la00noix 0:a56be39653d0 201 if(LSX >127.5 && LSY >127.5) {
la00noix 0:a56be39653d0 202 theta = alpha2; //x=127.5 かつ y>127.5 の直線を0°としてθをとる
la00noix 0:a56be39653d0 203 } else if(LSX <127.5 && LSY >127.5) {
la00noix 0:a56be39653d0 204 theta = 360 + alpha2;
la00noix 0:a56be39653d0 205 } else if(LSY <127.5) {
la00noix 0:a56be39653d0 206 theta = 180 + alpha2;
la00noix 0:a56be39653d0 207 }
la00noix 0:a56be39653d0 208
la00noix 0:a56be39653d0 209 //printf("LSX=%d LSY=%d alpha1=%f alpha2=%f theta=%d\n\r",LSX,LSY,alpha1,alpha2,theta);
la00noix 0:a56be39653d0 210
la00noix 0:a56be39653d0 211 can_ashidata[1] = theta>>8;
la00noix 0:a56be39653d0 212 can_ashidata[2] = theta&255;
la00noix 0:a56be39653d0 213
la00noix 0:a56be39653d0 214 if(((ButtonState >> BUTTONR1)&1) == 1) { //buttonR1 on
la00noix 0:a56be39653d0 215 can_ashidata0_2 = 1;
la00noix 0:a56be39653d0 216 } else {
la00noix 0:a56be39653d0 217 can_ashidata0_2 = 0;
la00noix 0:a56be39653d0 218 }
la00noix 0:a56be39653d0 219
la00noix 0:a56be39653d0 220 if(RSX >=100 && RSX <150) {
la00noix 0:a56be39653d0 221 can_ashidata0_34 = 1; //not rotate
la00noix 0:a56be39653d0 222 } else if(RSX >=150) {
la00noix 0:a56be39653d0 223 can_ashidata0_34 = 2; //right rotation
la00noix 0:a56be39653d0 224 } else if(RSX <100) {
la00noix 0:a56be39653d0 225 can_ashidata0_34 = 3; //left rotation
la00noix 0:a56be39653d0 226 }
la00noix 0:a56be39653d0 227
la00noix 0:a56be39653d0 228 if(LSX >=100 && LSX <150 && LSY >=100 && LSY <150) { //left joystick neutral position
la00noix 0:a56be39653d0 229 can_ashidata0_5 = 1;
la00noix 0:a56be39653d0 230 } else {
la00noix 0:a56be39653d0 231 can_ashidata0_5 = 0;
la00noix 0:a56be39653d0 232 }
la00noix 0:a56be39653d0 233
la00noix 0:a56be39653d0 234 can_ashidata[0] = (can_ashidata0_01<<6 | can_ashidata0_2<<5 | can_ashidata0_34<<3 | can_ashidata0_5<<2 | can_ashidata0_6<<1);
la00noix 0:a56be39653d0 235
la00noix 0:a56be39653d0 236 //-----〇ボタン motorR---------------------------------------------------------
la00noix 0:a56be39653d0 237 old_button1=now_button1;
la00noix 0:a56be39653d0 238 if(((ButtonState >> BUTTONCIRCLE)&1) == 1) {
la00noix 0:a56be39653d0 239 now_button1 = 1;
la00noix 0:a56be39653d0 240 } else {
la00noix 0:a56be39653d0 241 now_button1 = 0;
la00noix 0:a56be39653d0 242 }
la00noix 0:a56be39653d0 243
la00noix 0:a56be39653d0 244 if(old_button1==0 &&now_button1==1 &&flag1==0) {
la00noix 0:a56be39653d0 245 can_armdata0_01 = 1;
la00noix 0:a56be39653d0 246 flag1 = 1;
la00noix 0:a56be39653d0 247 } else if(old_button1==0 &&now_button1==1 &&flag1==1) {
la00noix 0:a56be39653d0 248 can_armdata0_01 = 2;
la00noix 0:a56be39653d0 249 flag1 = 0;
la00noix 0:a56be39653d0 250 }
la00noix 0:a56be39653d0 251
la00noix 0:a56be39653d0 252 //-----×ボタン motorL---------------------------------------------------------
la00noix 0:a56be39653d0 253 old_button2=now_button2;
la00noix 0:a56be39653d0 254 if(((ButtonState >> BUTTONCROSS)&1) == 1) {
la00noix 0:a56be39653d0 255 now_button2 = 1;
la00noix 0:a56be39653d0 256 } else {
la00noix 0:a56be39653d0 257 now_button2 = 0;
la00noix 0:a56be39653d0 258 }
la00noix 0:a56be39653d0 259
la00noix 0:a56be39653d0 260 if(old_button2==0 &&now_button2==1 &&flag2==0) {
la00noix 0:a56be39653d0 261 can_armdata0_23 = 1;
la00noix 0:a56be39653d0 262 flag2 = 1;
la00noix 0:a56be39653d0 263 } else if(old_button2==0 &&now_button2==1 &&flag2==1) {
la00noix 0:a56be39653d0 264 can_armdata0_23 = 2;
la00noix 0:a56be39653d0 265 flag2 = 0;
la00noix 0:a56be39653d0 266 }
la00noix 0:a56be39653d0 267
la00noix 0:a56be39653d0 268 //-----□ボタン armR---------------------------------------------------------
la00noix 0:a56be39653d0 269 old_button3=now_button3;
la00noix 0:a56be39653d0 270 if(((ButtonState >> BUTTONSQUARE)&1) == 1) {
la00noix 0:a56be39653d0 271 now_button3 = 1;
la00noix 0:a56be39653d0 272 } else {
la00noix 0:a56be39653d0 273 now_button3 = 0;
la00noix 0:a56be39653d0 274 }
la00noix 0:a56be39653d0 275
la00noix 0:a56be39653d0 276 if(old_button3==0 &&now_button3==1 &&flag3==0) {
la00noix 0:a56be39653d0 277 can_armdata0_4 = 0;
la00noix 0:a56be39653d0 278 flag3 = 1;
la00noix 0:a56be39653d0 279 } else if(old_button3==0 &&now_button3==1 &&flag3==1) {
la00noix 0:a56be39653d0 280 can_armdata0_4 = 1;
la00noix 0:a56be39653d0 281 flag3 = 0;
la00noix 0:a56be39653d0 282 }
la00noix 0:a56be39653d0 283
la00noix 0:a56be39653d0 284 //-----△ボタン armL---------------------------------------------------------
la00noix 0:a56be39653d0 285 old_button4=now_button4;
la00noix 0:a56be39653d0 286 if(((ButtonState >> BUTTONTRIANGEL)&1) == 1) {
la00noix 0:a56be39653d0 287 now_button4 = 1;
la00noix 0:a56be39653d0 288 } else {
la00noix 0:a56be39653d0 289 now_button4 = 0;
la00noix 0:a56be39653d0 290 }
la00noix 0:a56be39653d0 291
la00noix 0:a56be39653d0 292 if(old_button4==0 &&now_button4==1 &&flag4==0) {
la00noix 0:a56be39653d0 293 can_armdata0_5 = 0;
la00noix 0:a56be39653d0 294 flag4 = 1;
la00noix 0:a56be39653d0 295 } else if(old_button4==0 &&now_button4==1 &&flag4==1) {
la00noix 0:a56be39653d0 296 can_armdata0_5 = 1;
la00noix 0:a56be39653d0 297 flag4 = 0;
la00noix 0:a56be39653d0 298 }
la00noix 0:a56be39653d0 299
la00noix 0:a56be39653d0 300 //-----RIGHTボタン sanpou---------------------------------------------------------
la00noix 0:a56be39653d0 301 old_button5=now_button5;
la00noix 0:a56be39653d0 302 if(((ButtonState >> BUTTONRIGHT)&1) == 1) {
la00noix 0:a56be39653d0 303 now_button5 = 1;
la00noix 0:a56be39653d0 304 } else {
la00noix 0:a56be39653d0 305 now_button5 = 0;
la00noix 0:a56be39653d0 306 }
la00noix 0:a56be39653d0 307
la00noix 0:a56be39653d0 308 if(old_button5==0 &&now_button5==1 &&flag5==0) {
la00noix 0:a56be39653d0 309 can_armdata0_6 = 0;
la00noix 0:a56be39653d0 310 flag5 = 1;
la00noix 0:a56be39653d0 311 } else if(old_button5==0 &&now_button5==1 &&flag5==1) {
la00noix 0:a56be39653d0 312 can_armdata0_6 = 1;
la00noix 0:a56be39653d0 313 flag5 = 0;
la00noix 0:a56be39653d0 314 }
la00noix 0:a56be39653d0 315
la00noix 0:a56be39653d0 316 //-----DOWNボタン under_sanpou---------------------------------------------------------
la00noix 0:a56be39653d0 317 old_button6=now_button6;
la00noix 0:a56be39653d0 318 if(((ButtonState >> BUTTONDOWN)&1) == 1) {
la00noix 0:a56be39653d0 319 now_button6 = 1;
la00noix 0:a56be39653d0 320 } else {
la00noix 0:a56be39653d0 321 now_button6 = 0;
la00noix 0:a56be39653d0 322 }
la00noix 0:a56be39653d0 323
la00noix 0:a56be39653d0 324 if(old_button6==0 &&now_button6==1 &&flag6==0) {
la00noix 0:a56be39653d0 325 can_armdata0_7 = 0;
la00noix 0:a56be39653d0 326 flag6 = 1;
la00noix 0:a56be39653d0 327 } else if(old_button6==0 &&now_button6==1 &&flag6==1) {
la00noix 0:a56be39653d0 328 can_armdata0_7 = 1;
la00noix 0:a56be39653d0 329 flag6 = 0;
la00noix 0:a56be39653d0 330 }
la00noix 0:a56be39653d0 331
la00noix 0:a56be39653d0 332 //-----UPボタン above_sanpou---------------------------------------------------------
la00noix 0:a56be39653d0 333 old_button7=now_button7;
la00noix 0:a56be39653d0 334 if(((ButtonState >> BUTTONUP)&1) == 1) {
la00noix 0:a56be39653d0 335 now_button7 = 1;
la00noix 0:a56be39653d0 336 } else {
la00noix 0:a56be39653d0 337 now_button7 = 0;
la00noix 0:a56be39653d0 338 }
la00noix 0:a56be39653d0 339
la00noix 0:a56be39653d0 340 if(old_button7==0 &&now_button7==1 &&flag7==0) {
la00noix 0:a56be39653d0 341 can_armdata1_0 = 0;
la00noix 0:a56be39653d0 342 flag7 = 1;
la00noix 0:a56be39653d0 343 } else if(old_button7==0 &&now_button7==1 &&flag7==1) {
la00noix 0:a56be39653d0 344 can_armdata1_0 = 1;
la00noix 0:a56be39653d0 345 flag7 = 0;
la00noix 0:a56be39653d0 346 }
la00noix 0:a56be39653d0 347
la00noix 0:a56be39653d0 348 //-----LEFTボタン motorC---------------------------------------------------------
la00noix 0:a56be39653d0 349 old_button8=now_button8;
la00noix 0:a56be39653d0 350 if(((ButtonState >> BUTTONLEFT)&1) == 1) {
la00noix 0:a56be39653d0 351 now_button8 = 1;
la00noix 0:a56be39653d0 352 } else {
la00noix 0:a56be39653d0 353 now_button8 = 0;
la00noix 0:a56be39653d0 354 }
la00noix 0:a56be39653d0 355
la00noix 0:a56be39653d0 356 if(old_button8==0 &&now_button8==1 &&flag8==0) {
la00noix 0:a56be39653d0 357 can_armdata1_12 = 1;
la00noix 0:a56be39653d0 358 flag8 = 1;
la00noix 0:a56be39653d0 359 } else if(old_button8==0 &&now_button8==1 &&flag8==1) {
la00noix 0:a56be39653d0 360 can_armdata1_12 = 2;
la00noix 0:a56be39653d0 361 flag8 = 0;
la00noix 0:a56be39653d0 362 }
la00noix 0:a56be39653d0 363
la00noix 0:a56be39653d0 364 //-----TOGGLE SWITCH---------------------------------------------------------
la00noix 0:a56be39653d0 365 //printf("R=%d L=%d\n\r",in_right.read(),in_left.read());
la00noix 0:a56be39653d0 366
la00noix 0:a56be39653d0 367 if(in_right.read() == 1 && in_left.read() == 0) { //right mode
la00noix 0:a56be39653d0 368 can_ashidata0_6 = 0;
la00noix 0:a56be39653d0 369 } else { //left mode
la00noix 0:a56be39653d0 370 can_ashidata0_6 = 1;
la00noix 0:a56be39653d0 371 }
la00noix 0:a56be39653d0 372
la00noix 0:a56be39653d0 373 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);
la00noix 0:a56be39653d0 374 can_armdata[1] = (can_armdata1_0<<7 | can_armdata1_12<<5 | can_armdata1_34<<3);
la00noix 0:a56be39653d0 375 //printf("%d,%d,%d",can_ashidata[0],can_armdata[0],can_armdata[1]);
la00noix 0:a56be39653d0 376
la00noix 0:a56be39653d0 377
la00noix 0:a56be39653d0 378
la00noix 0:a56be39653d0 379 can_sendread();
la00noix 0:a56be39653d0 380 }