robostep8th / Mbed 2 deprecated harurobo_PS3_com7

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

User.cpp@1:d15f6e5eda54, 2019-03-02 (annotated)

Committer:
la00noix
Date:
Sat Mar 02 07:50:03 2019 +0000
Revision:
1:d15f6e5eda54
Parent:
0:798b73e805d9
Child:
2:817bcbc3431e
a

Who changed what in which revision?

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