Diff: main.cpp

Revision:

6:7ec6c3d3a30a

Parent:

5:0160b73f3d9e

Child:

7:3a7e49ed1162

diff -r 0160b73f3d9e -r 7ec6c3d3a30a main.cpp
--- a/main.cpp	Wed Sep 12 05:35:59 2018 +0000
+++ b/main.cpp	Thu Sep 13 04:17:15 2018 +0000
@@ -4,7 +4,7 @@
 #include "EC.h"
 #include "R1370P.h"
 #include "enc_1multi.h"
-#define BASIC_SPEED 30  //モーターはこの角速度で駆動させる
+#define BASIC_SPEED 15  //モーターはこの角速度で駆動させる
 
 SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1);  //right  enc migi ue
 SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7);    //left   enc hidari sita  //ok
@@ -164,7 +164,7 @@
 
 void susumu_ang(double a,double b,double goal)//kakudo
 {
-    if(x<angle-5&&a>b) {//usetsu
+    if(goal-5>angle&&a<b) {//usetsu
         t.start();
         pc.printf("R=%f L=%f\r\n",target_R,target_L);
         if(t.read_ms()<100) {
@@ -178,10 +178,10 @@
             t.stop();
             pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(angle<goal&&angle>=goal-5&&a>b) {
+    } else if(angle<goal&&angle>=goal-5&&a<b) {
         tgt(a/3,b/3);
 
-    } else if(angle>goal+5&&a<b) { //sasetsu
+    } else if(angle>goal+5&&a>b) { //sasetsu
         t.start();
         pc.printf("R=%f L=%f\r\n",target_R,target_L);
         if(t.read_ms()<100) {
@@ -195,7 +195,7 @@
             t.stop();
             pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(angle>goal&&angle<=goal+5&&a<b) {
+    } else if(angle>goal&&angle<=goal+5&&a>b) {
         tgt(a/3,b/3);
 
     } else {
@@ -252,6 +252,10 @@
         else motorR.Sc(target_R);
         if(target_L==0) motorL.stop();
         else motorL.Sc(target_L);
+        
+        new_dist=EC1.getDistance_mm();
+            d_dist=new_dist-old_dist;
+            old_dist=new_dist;
 
         double d_x=d_dist*sin(angle);
         double d_y=d_dist*cos(angle);
@@ -261,9 +265,7 @@
 
 
         if(kai>=3) {
-            new_dist=EC1.getDistance_mm();
-            d_dist=new_dist-old_dist;
-            old_dist=new_dist;
+            
 
             //double d_x=d_dist*sin(angle);;
             //double d_y=d_dist*cos(angle);;
@@ -302,13 +304,13 @@
         susumu_y(1,1,start_x+178);//x,x+178
     }
     if(i==2) {
-        susumu_ang(1/2,1,45);//x+121.5,x+471.5
+        susumu_ang(0.5,1,45);//x+121.5,x+471.5
     }
     if(i==3) {
         susumu_xl(1,1,728.5);//728.5,1078.5
     }
     if(i==4) {
-        susumu_ang(1,1/2,0);//850,1372
+        susumu_ang(1,0.5,0);//850,1372
     }
     if(i==5) {
         susumu_y(1,1,1700);//850,1700
@@ -373,7 +375,9 @@
     if(i==12) {
         motorR.stop();
         motorL.stop();
-        servo.pulsewidth_us(3000);
+        servo.pulsewidth_us(2100);
+        wait(1.5);
+        servo.pulsewidth_us(2400);
         wait(1);
         i++;
     }
@@ -405,7 +409,7 @@
         denjiben=1;
         wait(0.5);
         servo.pulsewidth_us(900);
-        wait(1);
+        wait(2);
         i++;
     }
     if(i==16) {