Diff: User.cpp

Revision:

17:95cb86ce56a9

Parent:

16:55c180a4338c

Child:

18:b24438ec4fd2

--- 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;