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@0:798b73e805d9, 2019-02-25 (annotated)

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