Revision 2:a583ad08d3d6, committed 2019-03-09

Comitter:

ec30109b

Date:

Sat Mar 09 08:39:58 2019 +0000

Parent:

1:58103ad56096

Child:

3:e8f08f2be311

Commit message:

kourobo2019

Changed in this revision

--- a/main.cpp	Thu Mar 07 09:42:59 2019 +0000
+++ b/main.cpp	Sat Mar 09 08:39:58 2019 +0000
@@ -8,7 +8,7 @@
 Serial pc(USBTX, USBRX,115200);
 AnalogIn analog_value(A0);
 DigitalOut led(LED1);
-Controller pad(PC_10,PC_11,208);
+Controller pad(PC_12,PD_2,198);
 
 
 ikarashiMDC ikarashi[] {
@@ -16,10 +16,12 @@
     ikarashiMDC(&serialcontrol,2,1,SM,&serial),
     ikarashiMDC(&serialcontrol,2,2,SM,&serial),
     ikarashiMDC(&serialcontrol,2,3,SM,&serial),
-    ikarashiMDC(&serialcontrol,3,3,SM,&serial)
+    ikarashiMDC(&serialcontrol,3,0,SM,&serial)
 };
 PwmOut pwm(LED1);
 
+float change(float);
+
 int main()
 {
     serial.baud(115200);
@@ -48,36 +50,41 @@
                 stickNorn[i] = pad.getNorm(i);
                 pc.printf("%f %f\t", stickRadian[i], stickNorn[i]);
             }
-            pc.printf("\n\r");
         
         //足回り
         if(stickNorn[0] != 0){
-            SPe[0] = sin(stickRadian[0] - PI * 3/4);
-            SPe[1] = sin(stickRadian[0] - PI * (-3/4));
-            SPe[2] = sin(stickRadian[0] - PI * (-1/4));
-            SPe[3] = sin(stickRadian[0] - PI * 1/4);
+            SPe[0] = sin(change(stickRadian[0]) - PI * 7/4);
+            SPe[1] = sin(change(stickRadian[0]) - PI * 1/4);
+            SPe[2] = sin(change(stickRadian[0]) - PI * 3/4);
+            SPe[3] = sin(change(stickRadian[0]) - PI * 5/4);
         }
         
         //微調整
-        if(button[3] == 0){        //前
+        if((button[0] == button[1] == button[2] == button[3] == button[4] == button[5] == button[7] == button[8]) && stickNorn[0] == 0){
+            SPe[0] = 0;
+            SPe[1] = 0;
+            SPe[2] = 0;
+            SPe[3] = 0;
+        }
+        if(button[3] == 0 && button[0] == button[1] == button[2] == button[4] == button[5] == button[7] == button[8]){   //前
             SPe[0] = 0.4;
             SPe[1] = 0.4;
             SPe[2] = -0.4;
             SPe[3] = -0.4;
         }
-        if(button[5] == 0){        //後ろ
+        if(button[5] == 0 && button[0] == button[1] == button[2] == button[3] == button[4] == button[7] == button[8]){        //後ろ
             SPe[0] = -0.4;
             SPe[1] = -0.4;
             SPe[2] = 0.4;
             SPe[3] = 0.4;
         }
-        if(button[4] == 0){        //右
+        if(button[4] == 0 && button[0] == button[1] == button[2] == button[3] == button[5] == button[7] == button[8]){        //右
             SPe[0] = -0.4;
             SPe[1] = 0.4;
             SPe[2] = -0.4;
             SPe[3] = 0.4;
         }
-        if(button[2] == 0){         //左
+        if(button[2] == 0 && button[0] == button[1] == button[3] == button[4] == button[5] == button[7] == button[8]){         //左
             SPe[0] = 0.4;
             SPe[1] = -0.4;
             SPe[2] = 0.4;
@@ -85,14 +92,14 @@
         }
         
         //右回転
-        if(button[7] == 0){
+        if(button[7] == 0 && button[0] == button[1] == button[2] == button[3] == button[4] == button[5] == button[8]){
             SPe[0] = -0.4;
             SPe[1] = -0.4;
             SPe[2] = -0.4;
             SPe[3] = -0.4;
         }
         //左回転
-        if(button[8] == 0){
+        if(button[8] == 0 && button[0] == button[1] == button[2] == button[3] == button[4] == button[5] == button[7]){
             SPe[0] = 0.4;
             SPe[1] = 0.4;
             SPe[2] = 0.4;
@@ -100,12 +107,16 @@
         }
         
         //機構
-        if(button[1] == 0){
-            ikarashi[4].setSpeed(-0.3);    //閉じる
+        if(button[0] == button[1] == button[2] == button[3] == button[4] == button[5] == button[7] == button[8] == 1){
+            ikarashi[4].setSpeed(0);
         }
         
-        if(button[0] == 0){                     //開く
-            ikarashi[4].setSpeed(0.3);
+        if(button[1] == 0 && button[0] == button[2] == button[3] == button[4] == button[5] == button[7] == button[8]){         //閉じる
+            ikarashi[4].setSpeed(-0.15);    
+        }
+        
+        if(button[0] == 0 && button[1] == button[2] == button[3] == button[4] == button[5] == button[7] == button[8]){         //開く
+            ikarashi[4].setSpeed(0.15);
         }
         
         
@@ -118,6 +129,8 @@
         pc.printf("%.2f  ",SPe[1]);
         pc.printf("%.2f  ",SPe[2]);
         pc.printf("%.2f  ",SPe[3]);
+        
+        pc.printf("%.3f ",change(stickRadian[0]));
             
         pc.printf("\r\n");
         
@@ -128,9 +141,27 @@
             ikarashi[1].setSpeed(0);
             ikarashi[2].setSpeed(0);
             ikarashi[3].setSpeed(0);
+            
         }
     }
 }        
         
+float change(float stiRad){
+    float RAD;
+    if(stiRad >= 0){
+        RAD = PI - stiRad;
+    }
+    if(stiRad < 0){
+        RAD = -(PI) -(stiRad);
+    }
+    return RAD;
+}
         
+    
+    
+    
+    
+    
+    
+      
   
\ No newline at end of file