Diff: User.cpp

Revision:

16:55c180a4338c

Parent:

15:c5258a4dee75

Child:

17:95cb86ce56a9

--- a/User.cpp	Sat Apr 29 11:48:36 2017 +0000
+++ b/User.cpp	Tue Sep 04 12:10:13 2018 +0000
@@ -5,22 +5,60 @@
 #include "User.h"
 
 #include "mbed.h"
+#include "math.h"
+#include "move3wheel.h"
+
 
 int RSX,RSY,LSX,LSY,BSU,BSL;
-//これより下に関数外に書く要素を記入する
-PwmOut led1(LED1);
-PwmOut led2(LED2);
+/*DigitalOut led1(LED1);
+DigitalOut led2(LED2);
 DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+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;
 
-void UserLoopSetting(){
-    //一度だけ行いたい初期設定をここに書く
-    led1.period_ms(20);
+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 UserLoop(char n,const u8* data){
+
+
+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
+    if(n==0) { //有線Ps3USB.cpp
         RSX = ((ps3report*)data)->RightStickX;
         RSY = ((ps3report*)data)->RightStickY;
         LSX = ((ps3report*)data)->LeftStickX;
@@ -29,7 +67,7 @@
         BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
         //ボタンの処理
         ButtonState =  ((ps3report*)data)->ButtonState;
-    }else {//無線TestShell.cpp
+    } else {//無線TestShell.cpp
         RSX = ((ps3report*)(data + 1))->RightStickX;
         RSY = ((ps3report*)(data + 1))->RightStickY;
         LSX = ((ps3report*)(data + 1))->LeftStickX;
@@ -39,25 +77,398 @@
         //ボタンの処理
         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);  //前進と逆の方向に回す
     
-    //データ取得例
-    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
-        led1 = 1;
-        //○が押されたとき
-    }else{
-        led1 = 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);
         }
         
-    led1=LSX/256.0f;
-    led2=LSY/256.0f;
-    led3=ButtonState & 0x0400;  //L1の状態
-    led4=ButtonState & 0x0800;  //R1の状態
-    //値の取得はps3.hを参照
-    //ここまでプログラム例    実機に乗せるときは消して大丈夫です
+        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;
+
+
+
 }
\ No newline at end of file