仮
Dependencies: mbed move3wheel
Fork of F3RC-mbed-new by
User.cpp
- Committer:
- yuki0701
- Date:
- 2018-09-04
- Revision:
- 16:55c180a4338c
- Parent:
- 15:c5258a4dee75
- Child:
- 17:95cb86ce56a9
File content as of revision 16:55c180a4338c:
#include "Utils.h" #include "USBHost.h" #include "hci.h" #include "ps3.h" #include "User.h" #include "mbed.h" #include "math.h" #include "move3wheel.h" int RSX,RSY,LSX,LSY,BSU,BSL; /*DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4);*/ SPI spi(p5,p6,p7); DigitalOut cs(p8); int data1 = 0; int a; //aは定数 double receive_data/*,true_data*/; double now_angle,target_angle,out; PwmOut motor0CW(p21); //0番motor 外から見て時計回り PwmOut motor0CCW(p22); //0番motor 反時計回り PwmOut motor1CW(p23); //1番motor PwmOut motor1CCW(p24); //1番motor PwmOut motor2CW(p26); //2番motor PwmOut motor2CCW(p25); //2番motor void motor(double cw0,double ccw0,double cw1,double ccw1,double cw2,double ccw2) { motor0CW = cw0; motor0CCW = ccw0; motor1CW = cw1; motor1CCW = ccw1; motor2CW = cw2; motor2CCW = ccw2; } void UserLoopSetting() { spi.format(16,3); spi.frequency(1000000); motor0CW.period_us(50); motor0CCW.period_us(50); motor1CW.period_us(50); motor1CCW.period_us(50); motor2CW.period_us(50); motor2CCW.period_us(50); } void UserLoop(char n,const u8* data) { u16 ButtonState; if(n==0) { //有線Ps3USB.cpp RSX = ((ps3report*)data)->RightStickX; RSY = ((ps3report*)data)->RightStickY; LSX = ((ps3report*)data)->LeftStickX; LSY = ((ps3report*)data)->LeftStickY; BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff); BSL = (u8)(((ps3report*)data)->ButtonState >> 8); //ボタンの処理 ButtonState = ((ps3report*)data)->ButtonState; } else {//無線TestShell.cpp RSX = ((ps3report*)(data + 1))->RightStickX; RSY = ((ps3report*)(data + 1))->RightStickY; LSX = ((ps3report*)(data + 1))->LeftStickX; LSY = ((ps3report*)(data + 1))->LeftStickY; BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff); BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8); //ボタンの処理 ButtonState = ((ps3report*)(data + 1))->ButtonState; } ///////////////////////////ここからmotorのプログラム/////////////////////////////// /*int a = (ButtonState >> BUTTONRIGHT)&1; if(a == 0)led4=0; else led4=1;*/ //printf("RSX = %d, RSY = %d\r\n", RSX, RSY); if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150 && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0)) { //ニュートラルポジション motor(0,0,0,0,0,0); /*led1 = 1; led2 = 0; led3 = 0; led4 = 0;*/ } else if(RSX >=100 && RSX <150 && RSY <100) { // ↑ 移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0,0.13,0); }else{ CalMotorOut(0,0.3,0); //走らせたい速さを入れる(X方向,y方向,回転) 回転方向は上から見て反時計回りが正 } if(now_angle > 135 && now_angle < 225){ motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.13,0,0); }else{ CalMotorOut(0.3,0,0); } motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); }else{ motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); //1番motorはCW方向、2番motorはCCW方向に回す } } else if(RSX >=100 && RSX <150 && RSY >=150) { // ↓ 移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0,0.13,0); }else{ CalMotorOut(0,0.3,0); } if(now_angle > 135 && now_angle < 225){ motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.13,0,0); }else{ CalMotorOut(0.3,0,0); } motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); }else{ motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); //前進と逆の方向に回す } } else if(RSX >=150 && RSY >=100 && RSY <150) { // → 移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.13,0,0); }else{ CalMotorOut(0.3,0,0); } if(now_angle > 135 && now_angle < 225){ motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0,0.13,0); }else{ CalMotorOut(0,0.3,0); } motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); }else{ motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); } } else if(RSX <100 && RSY >=100 && RSY <150) { // ← 移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.13,0,0); }else{ CalMotorOut(0.3,0,0); } if(now_angle > 135 && now_angle < 225){ motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2)); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0,0.13,0); }else{ CalMotorOut(0,0.3,0); } motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); }else{ motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0); } } else if(RSX < 100 && RSY <100) { // 左上移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.1,0.1,0); }else{ CalMotorOut(0.2,0.2,0); } if(now_angle > 135 && now_angle < 225){ motor(GetMotorOut(0),0,0,GetMotorOut(1),-GetMotorOut(2),0); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.1,-0.1,0); }else{ CalMotorOut(0.2,-0.2,0); } motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0); }else{ motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2)); } } else if(RSX >= 150 && RSY <100) { // 右上移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(-0.1,0.1,0); }else{ CalMotorOut(-0.2,0.2,0); } if(now_angle > 135 && now_angle < 225){ motor(0,-GetMotorOut(0),0,GetMotorOut(1),-GetMotorOut(2),0); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.1,0.1,0); }else{ CalMotorOut(0.2,0.2,0); } motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2)); }else{ motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2)); } } else if(RSX <100 && RSY >=150) { // 左下移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(0.1,-0.1,0); }else{ CalMotorOut(0.2,-0.2,0); } if(now_angle > 135 && now_angle < 225){ motor(GetMotorOut(0),0,-GetMotorOut(1),0,0,GetMotorOut(2)); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(-0.1,-0.1,0); }else{ CalMotorOut(-0.2,-0.2,0); } motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0); }else{ motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0); } } else if(RSX >= 150 && RSY >=150) { // 右下移動 if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(-0.1,-0.1,0); }else{ CalMotorOut(-0.2,-0.2,0); } if(now_angle > 135 && now_angle < 225){ motor(0,-GetMotorOut(0),-GetMotorOut(1),0,0,GetMotorOut(2)); }else if(now_angle > 225 && now_angle < 315){ if((ButtonState >> BUTTONR1)&1 == 1){ CalMotorOut(-0.1,-0.1,0); }else{ CalMotorOut(-0.2,0.2,0); } motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2)); }else{ motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0); } } if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150) { //右スティックニュートラルポジション if(LSX >=100 && LSX <150 ) { //左スティックニュートラルポジション motor(0,0,0,0,0,0); } else if(LSX >=150 ) { //右回転 motor(0,0.2,0,0.2,0,0.2); } else if(LSX <100 ) { //左回転 motor(0.2,0,0.2,0,0.2,0); } else { motor(0,0,0,0,0,0); } } ////////////////////////////ここからヌクレオのプログラム///////////////////////////// if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1 wait(0.1); if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { data1=1; } } if((ButtonState >> BUTTONCIRCLE)&1 == 1) { //servo2 wait(0.1); if((ButtonState >> BUTTONCIRCLE)&1 == 1) { data1=2; } } if((ButtonState >> BUTTONCROSS)&1 == 1) { //servo3 wait(0.1); if((ButtonState >> BUTTONCROSS)&1 == 1) { data1=3; } } if((ButtonState >> BUTTONSQUARE)&1 == 1) { //servo4 wait(0.1); if((ButtonState >> BUTTONSQUARE)&1 == 1) { data1=4; } } if((ButtonState >> BUTTONLEFT)&1 == 1) { //エアシリンダー wait(0.1); if((ButtonState >> BUTTONLEFT)&1 == 1) { data1=5; } } ////////////////////////ここからジャイロの角度指定プログラム////////////////////////// if(receive_data >= 0) { //反時計回りに0~359°となるよう修正 a = receive_data / 360; now_angle = receive_data - (360 * a); //現在の角度 } else { a = receive_data / 360; now_angle = - receive_data + (360 * a); } /*true_data = receive_data - 65536; } else { true_data = receive_data; }*/ //now_angle = true_data; //現在の角度 //out = 0.01 * (target_angle - now_angle); //P(目標の角度 - 現在の角度) Pは自分で決めた定数 if((ButtonState >> BUTTONRIGHT)&1 == 1) { wait(0.1); if((ButtonState >> BUTTONRIGHT)&1 == 1) { target_angle = 270; out = 0.01 * (target_angle - now_angle); //printf("%f\r\n",now_angle); //printf("%f\r\n",out); if(now_angle >= 268 && now_angle <= 272){ motor(0,0,0,0,0,0); } else if(out > 1.8 || out <= -0.3) { //0~89°と270~359°のときは時計回りに回転 motor(0.3,0,0.3,0,0.3,0); //printf("cw 0.3\r\n"); } else if(out > -0.3 && out <= 0) { motor(-out,0,-out,0,-out,0); //printf("cw -out\r\n"); } else if(out > 0 && out <= 0.3) { //90~269°のときは反時計回りに回転 motor(0,out,0,out,0,out); //printf("ccw out\r\n"); } else if(out <= 1.8 && out > 0.3) { motor(0,0.3,0,0.3,0,0.3); //printf("ccw 0.3\r\n"); } } } if((ButtonState >> BUTTONDOWN)&1 == 1) { wait(0.1); if((ButtonState >> BUTTONDOWN)&1 == 1) { target_angle = 180; out = 0.01 * (target_angle - now_angle); if(now_angle >= 178 && now_angle <= 182){ motor(0,0,0,0,0,0); } else if(out > 0.3) { //0~178°のときは反時計回りに回転 motor(0,0.3,0,0.3,0,0.3); } else if(out <= 0.3 && out > 0) { motor(0,out,0,out,0,out); } else if(out <= 0 && out > -0.3) { //180~359°のときは時計回りに回転 motor(-out,0,-out,0,-out,0); } else if(out <= -0.3) { motor(0.3,0,0.3,0,0.3,0); } } } if((ButtonState >> BUTTONUP)&1 == 1) { wait(0.1); if((ButtonState >> BUTTONUP)&1 == 1) { if(0 <= now_angle && now_angle < 180) { target_angle = 0; out = -0.01 * (target_angle - now_angle); if(now_angle <= 1) { motor(0,0,0,0,0,0); } else if(out > 0.3) { //0~179°のときは時計回りに回転 motor(0.3,0,0.3,0,0.3,0); } else if(out <= 0.3 && out > 0) { motor(out,0,out,0,out,0); } } if(180 <= now_angle && now_angle< 360) { target_angle = 360; out = 0.01 * (target_angle - now_angle); if(now_angle >= 359) { motor(0,0,0,0,0,0); } else if(out > 0.3) { //180~359°のときは反時計回りに回転 motor(0,0.3,0,0.3,0,0.3); } else if(out <= 0.3 && out > 0) { motor(0,out,0,out,0,out); } } } } cs=0; receive_data = spi.write(data1); cs=1; data1=0; }