春ロボ1班(元F3RC4班+)
/
F3RC-mbed-new4
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 move3wheel
Fork of
F3RC-mbed-new3
by 
春ロボ1班(元F3RC4班+)
Embed:
(wiki syntax)
Show/hide line numbers
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 #include "move3wheel.h" 00010 00011 00012 int RSX,RSY,LSX,LSY,BSU,BSL; 00013 /*DigitalOut led1(LED1); 00014 DigitalOut led2(LED2); 00015 DigitalOut led3(LED3); 00016 DigitalOut led4(LED4);*/ 00017 00018 SPI spi(p5,p6,p7); 00019 DigitalOut cs(p8); 00020 int data1 = 0; 00021 int a; //aは定数 00022 double receive_data/*,true_data*/; 00023 double now_angle,target_angle,out; 00024 int count=0; 00025 00026 PwmOut motor0CW(p21); //0番motor 外から見て時計回り 00027 PwmOut motor0CCW(p22); //0番motor 反時計回り 00028 PwmOut motor1CW(p23); //1番motor 00029 PwmOut motor1CCW(p24); //1番motor 00030 PwmOut motor2CW(p26); //2番motor 00031 PwmOut motor2CCW(p25); //2番motor 00032 00033 00034 void motor(double cw0,double ccw0,double cw1,double ccw1,double cw2,double ccw2) 00035 { 00036 motor0CW = cw0; 00037 motor0CCW = ccw0; 00038 motor1CW = cw1; 00039 motor1CCW = ccw1; 00040 motor2CW = cw2; 00041 motor2CCW = ccw2; 00042 } 00043 00044 00045 00046 void UserLoopSetting() 00047 { 00048 spi.format(16,3); 00049 spi.frequency(1000000); 00050 00051 motor0CW.period_us(50); 00052 motor0CCW.period_us(50); 00053 motor1CW.period_us(50); 00054 motor1CCW.period_us(50); 00055 motor2CW.period_us(50); 00056 motor2CCW.period_us(50); 00057 } 00058 00059 void UserLoop(char n,const u8* data) 00060 { 00061 u16 ButtonState; 00062 if(n==0) { //有線Ps3USB.cpp 00063 RSX = ((ps3report*)data)->RightStickX; 00064 RSY = ((ps3report*)data)->RightStickY; 00065 LSX = ((ps3report*)data)->LeftStickX; 00066 LSY = ((ps3report*)data)->LeftStickY; 00067 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff); 00068 BSL = (u8)(((ps3report*)data)->ButtonState >> 8); 00069 //ボタンの処理 00070 ButtonState = ((ps3report*)data)->ButtonState; 00071 } else {//無線TestShell.cpp 00072 RSX = ((ps3report*)(data + 1))->RightStickX; 00073 RSY = ((ps3report*)(data + 1))->RightStickY; 00074 LSX = ((ps3report*)(data + 1))->LeftStickX; 00075 LSY = ((ps3report*)(data + 1))->LeftStickY; 00076 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff); 00077 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8); 00078 //ボタンの処理 00079 ButtonState = ((ps3report*)(data + 1))->ButtonState; 00080 } 00081 00082 ///////////////////////////ここからmotorのプログラム/////////////////////////////// 00083 00084 /*int a = (ButtonState >> BUTTONRIGHT)&1; 00085 if(a == 0)led4=0; 00086 else led4=1;*/ 00087 00088 //printf("RSX = %d, RSY = %d\r\n", RSX, RSY); 00089 if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150 00090 && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0) && (((ButtonState >> BUTTONUP)&1) == 0)) { //ニュートラルポジション 00091 motor(0,0,0,0,0,0); 00092 /*led1 = 1; 00093 led2 = 0; 00094 led3 = 0; 00095 led4 = 0;*/ 00096 00097 if(LSX >=100 && LSX <150 ) { //左スティックニュートラルポジション 00098 motor(0,0,0,0,0,0); 00099 00100 } else if(LSX >=150 ) { //右回転 00101 if((ButtonState >> BUTTONR1)&1 == 1) { 00102 motor(0,0.13,0,0.13,0,0.13); 00103 } else { 00104 motor(0,0.3,0,0.3,0,0.3); 00105 } 00106 00107 } else if(LSX <100 ) { //左回転 00108 if((ButtonState >> BUTTONR1)&1 == 1) { 00109 motor(0.13,0,0.13,0,0.13,0); 00110 } else { 00111 motor(0.3,0,0.3,0,0.3,0); 00112 } 00113 00114 } else { 00115 motor(0,0,0,0,0,0); 00116 } 00117 00118 00119 00120 } else if(RSX >=100 && RSX <150 && RSY <100) { // ↑ 移動 00121 00122 if((ButtonState >> BUTTONR1)&1 == 1) { 00123 CalMotorOut(0,0.13,0); 00124 00125 } else { 00126 CalMotorOut(0,0.3,0); //走らせたい速さを入れる(X方向,y方向,回転) 回転方向は上から見て反時計回りが正 00127 } 00128 00129 if(now_angle > 135 && now_angle < 225) { 00130 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); 00131 00132 } else if(now_angle > 225 && now_angle < 315) { 00133 if((ButtonState >> BUTTONR1)&1 == 1) { 00134 CalMotorOut(0.13,0,0); 00135 00136 } else { 00137 CalMotorOut(0.3,0,0); 00138 } 00139 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); 00140 00141 } else { 00142 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); //1番motorはCW方向、2番motorはCCW方向に回す 00143 00144 } 00145 00146 } else if(RSX >=100 && RSX <150 && RSY >=150) { // ↓ 移動 00147 if((ButtonState >> BUTTONR1)&1 == 1) { 00148 CalMotorOut(0,0.13,0); 00149 00150 } else { 00151 CalMotorOut(0,0.3,0); 00152 } 00153 00154 if(now_angle > 135 && now_angle < 225) { 00155 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); 00156 00157 } else if(now_angle > 225 && now_angle < 315) { 00158 if((ButtonState >> BUTTONR1)&1 == 1) { 00159 CalMotorOut(0.13,0,0); 00160 00161 } else { 00162 CalMotorOut(0.3,0,0); 00163 } 00164 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); 00165 00166 } else { 00167 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); //前進と逆の方向に回す 00168 00169 } 00170 00171 } else if(RSX >=150 && RSY >=100 && RSY <150) { // → 移動 00172 if((ButtonState >> BUTTONR1)&1 == 1) { 00173 CalMotorOut(0.13,0,0); 00174 00175 } else { 00176 CalMotorOut(0.3,0,0); 00177 } 00178 00179 if(now_angle > 135 && now_angle < 225) { 00180 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); 00181 00182 } else if(now_angle > 225 && now_angle < 315) { 00183 if((ButtonState >> BUTTONR1)&1 == 1) { 00184 CalMotorOut(0,0.13,0); 00185 00186 } else { 00187 CalMotorOut(0,0.3,0); 00188 } 00189 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); 00190 00191 } else { 00192 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); 00193 00194 } 00195 00196 } else if(RSX <100 && RSY >=100 && RSY <150) { // ← 移動 00197 if((ButtonState >> BUTTONR1)&1 == 1) { 00198 CalMotorOut(0.13,0,0); 00199 00200 } else { 00201 CalMotorOut(0.3,0,0); 00202 } 00203 00204 if(now_angle > 135 && now_angle < 225) { 00205 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); 00206 00207 } else if(now_angle > 225 && now_angle < 315) { 00208 if((ButtonState >> BUTTONR1)&1 == 1) { 00209 CalMotorOut(0,0.13,0); 00210 00211 } else { 00212 CalMotorOut(0,0.3,0); 00213 } 00214 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); 00215 00216 } else { 00217 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); 00218 00219 } 00220 00221 } else if(RSX < 100 && RSY <100) { // 左上移動 00222 if((ButtonState >> BUTTONR1)&1 == 1) { 00223 CalMotorOut(0.1,0.1,0); 00224 00225 } else { 00226 CalMotorOut(0.2,0.2,0); 00227 } 00228 00229 if(now_angle > 135 && now_angle < 225) { 00230 motor(GetMotorOut(0),0,0,GetMotorOut(1),-GetMotorOut(2),0); 00231 00232 } else if(now_angle > 225 && now_angle < 315) { 00233 if((ButtonState >> BUTTONR1)&1 == 1) { 00234 CalMotorOut(0.1,-0.1,0); 00235 00236 } else { 00237 CalMotorOut(0.2,-0.2,0); 00238 } 00239 motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0); 00240 00241 } else { 00242 motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2)); 00243 00244 } 00245 00246 } else if(RSX >= 150 && RSY <100) { // 右上移動 00247 if((ButtonState >> BUTTONR1)&1 == 1) { 00248 CalMotorOut(-0.1,0.1,0); 00249 00250 } else { 00251 CalMotorOut(-0.2,0.2,0); 00252 } 00253 00254 if(now_angle > 135 && now_angle < 225) { 00255 motor(0,-GetMotorOut(0),0,GetMotorOut(1),-GetMotorOut(2),0); 00256 00257 } else if(now_angle > 225 && now_angle < 315) { 00258 if((ButtonState >> BUTTONR1)&1 == 1) { 00259 CalMotorOut(0.1,0.1,0); 00260 00261 } else { 00262 CalMotorOut(0.2,0.2,0); 00263 } 00264 motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2)); 00265 00266 } else { 00267 motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2)); 00268 00269 } 00270 00271 } else if(RSX <100 && RSY >=150) { // 左下移動 00272 if((ButtonState >> BUTTONR1)&1 == 1) { 00273 CalMotorOut(0.1,-0.1,0); 00274 00275 } else { 00276 CalMotorOut(0.2,-0.2,0); 00277 } 00278 00279 if(now_angle > 135 && now_angle < 225) { 00280 motor(GetMotorOut(0),0,-GetMotorOut(1),0,0,GetMotorOut(2)); 00281 00282 } else if(now_angle > 225 && now_angle < 315) { 00283 if((ButtonState >> BUTTONR1)&1 == 1) { 00284 CalMotorOut(-0.1,-0.1,0); 00285 00286 } else { 00287 CalMotorOut(-0.2,-0.2,0); 00288 } 00289 motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0); 00290 00291 } else { 00292 motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0); 00293 00294 } 00295 00296 } else if(RSX >= 150 && RSY >=150) { // 右下移動 00297 if((ButtonState >> BUTTONR1)&1 == 1) { 00298 CalMotorOut(-0.1,-0.1,0); 00299 00300 } else { 00301 CalMotorOut(-0.2,-0.2,0); 00302 } 00303 00304 if(now_angle > 135 && now_angle < 225) { 00305 motor(0,-GetMotorOut(0),-GetMotorOut(1),0,0,GetMotorOut(2)); 00306 00307 } else if(now_angle > 225 && now_angle < 315) { 00308 if((ButtonState >> BUTTONR1)&1 == 1) { 00309 CalMotorOut(-0.1,-0.1,0); 00310 00311 } else { 00312 CalMotorOut(-0.2,0.2,0); 00313 } 00314 motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2)); 00315 00316 } else { 00317 motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0); 00318 00319 } 00320 00321 } 00322 00323 00324 ////////////////////////////ここからヌクレオのプログラム///////////////////////////// 00325 00326 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1 00327 wait(0.1); 00328 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { 00329 data1=1; 00330 00331 } 00332 } 00333 00334 00335 if((ButtonState >> BUTTONCIRCLE)&1 == 1) { //servo2 00336 wait(0.1); 00337 if((ButtonState >> BUTTONCIRCLE)&1 == 1) { 00338 data1=2; 00339 } 00340 } 00341 00342 00343 if((ButtonState >> BUTTONCROSS)&1 == 1) { //servo3 00344 wait(0.1); 00345 if((ButtonState >> BUTTONCROSS)&1 == 1) { 00346 data1=3; 00347 } 00348 } 00349 00350 00351 if((ButtonState >> BUTTONSQUARE)&1 == 1) { //servo4 00352 wait(0.1); 00353 if((ButtonState >> BUTTONSQUARE)&1 == 1) { 00354 data1=4; 00355 } 00356 } 00357 00358 00359 if((ButtonState >> BUTTONLEFT)&1 == 1) { //エアシリンダー 00360 wait(0.1); 00361 if((ButtonState >> BUTTONLEFT)&1 == 1) { 00362 data1=5; 00363 } 00364 } 00365 00366 ////////////////////////ここからジャイロの角度指定プログラム////////////////////////// 00367 00368 /*if(receive_data >= 0) { //反時計回りに0~359°となるよう修正 00369 a = receive_data / 360; 00370 now_angle = receive_data - (360 * a); //現在の角度 00371 } else { 00372 a = receive_data / 360; 00373 now_angle = 360 + receive_data - (360 * a); 00374 }*/ 00375 00376 00377 /*true_data = receive_data - 65536; 00378 } else { 00379 true_data = receive_data; 00380 }*/ 00381 00382 00383 //now_angle = true_data; //現在の角度 00384 //out = 0.01 * (target_angle - now_angle); //P(目標の角度 - 現在の角度) Pは自分で決めた定数 00385 00386 if((ButtonState >> BUTTONRIGHT)&1 == 1) { 00387 wait(0.1); 00388 if((ButtonState >> BUTTONRIGHT)&1 == 1) { 00389 target_angle = 270; 00390 out = 0.01 * (target_angle - now_angle); 00391 00392 //printf("%f\r\n",now_angle); 00393 //printf("%f\r\n",out); 00394 if(now_angle >= 269 && now_angle <= 271) { 00395 motor(0,0,0,0,0,0); 00396 00397 } else if(out > 1.8 || out <= -0.3) { //0～89°と270～359°のときは時計回りに回転 00398 motor(0.3,0,0.3,0,0.3,0); 00399 //printf("cw 0.3\r\n"); 00400 00401 } else if(out > -0.3 && out <= 0) { 00402 motor(-out,0,-out,0,-out,0); 00403 //printf("cw -out\r\n"); 00404 00405 } else if(out > 0 && out <= 0.3) { //90～269°のときは反時計回りに回転 00406 motor(0,out,0,out,0,out); 00407 //printf("ccw out\r\n"); 00408 00409 } else if(out <= 1.8 && out > 0.3) { 00410 motor(0,0.3,0,0.3,0,0.3); 00411 //printf("ccw 0.3\r\n"); 00412 } 00413 } 00414 } 00415 00416 if((ButtonState >> BUTTONDOWN)&1 == 1) { 00417 wait(0.1); 00418 if((ButtonState >> BUTTONDOWN)&1 == 1) { 00419 target_angle = 180; 00420 out = 0.01 * (target_angle - now_angle); 00421 00422 if(now_angle >= 179 && now_angle <= 181) { 00423 motor(0,0,0,0,0,0); 00424 00425 } else if(out > 0.3) { //0～178°のときは反時計回りに回転 00426 motor(0,0.3,0,0.3,0,0.3); 00427 00428 } else if(out <= 0.3 && out > 0) { 00429 motor(0,out,0,out,0,out); 00430 00431 } else if(out <= 0 && out > -0.3) { //180～359°のときは時計回りに回転 00432 motor(-out,0,-out,0,-out,0); 00433 00434 } else if(out <= -0.3) { 00435 motor(0.3,0,0.3,0,0.3,0); 00436 } 00437 } 00438 } 00439 00440 if((ButtonState >> BUTTONUP)&1 == 1) { 00441 wait(0.1); 00442 if((ButtonState >> BUTTONUP)&1 == 1) { 00443 if(0 <= now_angle && now_angle < 180) { 00444 target_angle = 0; 00445 out = -0.01 * (target_angle - now_angle); 00446 00447 if(now_angle <= 1) { 00448 motor(0,0,0,0,0,0); 00449 00450 } else if(out > 0.3) { //0～179°のときは時計回りに回転 00451 motor(0.3,0,0.3,0,0.3,0); 00452 00453 } else if(out <= 0.3 && out > 0) { 00454 motor(out,0,out,0,out,0); 00455 } 00456 } 00457 00458 if(180 <= now_angle && now_angle< 360) { 00459 target_angle = 360; 00460 out = 0.01 * (target_angle - now_angle); 00461 00462 if(now_angle >= 359) { 00463 motor(0,0,0,0,0,0); 00464 00465 } else if(out > 0.3) { //180～359°のときは反時計回りに回転 00466 motor(0,0.3,0,0.3,0,0.3); 00467 00468 } else if(out <= 0.3 && out > 0) { 00469 motor(0,out,0,out,0,out); 00470 } 00471 } 00472 } 00473 } 00474 00475 00476 00477 //printf("%f\t",receive_data); 00478 00479 //printf("%f\n\r",now_angle); 00480 00481 cs=0; 00482 now_angle = spi.write(data1)*0.01; 00483 00484 cs=1; 00485 data1=0; 00486 00487 00488 00489 }
Generated on Wed Jul 13 2022 07:15:44 by
1.7.2