春ロボ1班(元F3RC4班+)
仮
Dependencies: mbed move3wheel
Fork of
F3RC-mbed-new
by 
春ロボ1班(元F3RC4班+)
Diff: User.cpp
- Revision:
- 17:95cb86ce56a9
- Parent:
- 16:55c180a4338c
--- a/User.cpp Tue Sep 04 12:10:13 2018 +0000
+++ b/User.cpp Wed Sep 05 11:42:29 2018 +0000
@@ -21,6 +21,7 @@
int a; //aは定数
double receive_data/*,true_data*/;
double now_angle,target_angle,out;
+int count=0;
PwmOut motor0CW(p21); //0番motor 外から見て時計回り
PwmOut motor0CCW(p22); //0番motor 反時計回り
@@ -86,231 +87,240 @@
//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)) { //ニュートラルポジション
+ && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0) && (((ButtonState >> BUTTONUP)&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);
+ if((ButtonState >> BUTTONR1)&1 == 1) {
+ motor(0,0.13,0,0.13,0,0.13);
+ } else {
+ motor(0,0.3,0,0.3,0,0.3);
+ }
} else if(LSX <100 ) { //左回転
- motor(0.2,0,0.2,0,0.2,0);
+ if((ButtonState >> BUTTONR1)&1 == 1) {
+ motor(0.13,0,0.13,0,0.13,0);
+ } else {
+ motor(0.3,0,0.3,0,0.3,0);
+ }
} else {
motor(0,0,0,0,0,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((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1
@@ -360,7 +370,7 @@
now_angle = receive_data - (360 * a); //現在の角度
} else {
a = receive_data / 360;
- now_angle = - receive_data + (360 * a);
+ now_angle = 360 + receive_data - (360 * a);
}
@@ -376,14 +386,14 @@
if((ButtonState >> BUTTONRIGHT)&1 == 1) {
wait(0.1);
if((ButtonState >> BUTTONRIGHT)&1 == 1) {
- target_angle = 270;
+ target_angle = 285;
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);
-
+ if(now_angle >= 284 && now_angle <= 286) {
+ 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");
@@ -406,12 +416,12 @@
if((ButtonState >> BUTTONDOWN)&1 == 1) {
wait(0.1);
if((ButtonState >> BUTTONDOWN)&1 == 1) {
- target_angle = 180;
+ target_angle = 197;
out = 0.01 * (target_angle - now_angle);
- if(now_angle >= 178 && now_angle <= 182){
+ if(now_angle >= 196 && now_angle <= 198) {
motor(0,0,0,0,0,0);
-
+
} else if(out > 0.3) { //0～178°のときは反時計回りに回転
motor(0,0.3,0,0.3,0,0.3);
@@ -426,7 +436,7 @@
}
}
}
-
+
if((ButtonState >> BUTTONUP)&1 == 1) {
wait(0.1);
if((ButtonState >> BUTTONUP)&1 == 1) {
@@ -461,10 +471,15 @@
}
}
}
-
+
+
+
+ //printf("%f\t",receive_data);
+
+ printf("%f\n\r",now_angle);
cs=0;
- receive_data = spi.write(data1);
+ receive_data = spi.write(data1)*0.01;
cs=1;
data1=0;