Diff: main.cpp

Revision:

5:0160b73f3d9e

Parent:

4:e3d739bf09b4

Child:

6:7ec6c3d3a30a

--- a/main.cpp	Tue Sep 11 06:42:51 2018 +0000
+++ b/main.cpp	Wed Sep 12 05:35:59 2018 +0000
@@ -36,8 +36,14 @@
 double new_dist=0;
 double old_dist=0;
 double d_dist=0;
-double x=185;
-double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu
+double x;
+double y;
+double asari_x=900;
+double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu
+double goal_x=1120;
+double goal_y=1700;
+double start_x=185;
+double start_y=150;
 Timer t;
 int i=1;
 
@@ -205,6 +211,10 @@
 
 int main(void)
 {
+    
+    gyro.initialize();    //main関数の最初に一度だけ実行
+    gyro.acc_offset();    //やってもやらなくてもいい
+
     printf("start\r\n");
     motor_tick.attach(&calOmega,0.05);
     motorR.setDOconstant(34.1);
@@ -213,25 +223,26 @@
     motorR.setPDparam(0,0);//pd//akirameta
 
 
-    gyro.initialize();    //main関数の最初に一度だけ実行
     EC1.setDiameter_mm(50);//sokuteirinnhannkei
     double  getDistance_mm();
     //int EC1.getCount()=0;
+    void reset  ();
     EC1.reset();
-    void reset  ();
 
 
     servo.period_ms(20);
 
     motor_f.period_ms(30);
     motor_b.period_ms(30);//arm
+    x=start_x;
+    y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu
 
     while(1) {
         // motorR.turnF(0.3);
         //motorL.turnF(0.3);//for debug
         motorR.Sc(target_R);
         motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
-        angle=gyro.getAngle();    //角度の値を受け取る
+        angle=gyro.getAngle()*(-1);    //角度の値を受け取る
 
         EC1.getDistance_mm();
         // EC1.CalOmega();
@@ -285,22 +296,22 @@
                 y=2500;
                 i=14;
             }
-        }
+        }//x,y
     }
     if(i==1) {
-        susumu_y(1,1,407);
+        susumu_y(1,1,start_x+178);//x,x+178
     }
     if(i==2) {
-        susumu_ang(1/3,1,45);
+        susumu_ang(1/2,1,45);//x+121.5,x+471.5
     }
     if(i==3) {
-        susumu_xl(1,1,709);
+        susumu_xl(1,1,728.5);//728.5,1078.5
     }
     if(i==4) {
-        susumu_ang(1,1/3,0);
+        susumu_ang(1,1/2,0);//850,1372
     }
     if(i==5) {
-        susumu_y(1,1,1700);
+        susumu_y(1,1,1700);//850,1700
     }
     if(i==6) {
         motorR.stop();
@@ -348,22 +359,22 @@
         }
     }//kakudo tyousei
     if(i==8) {
-        susumu_xr(1,1,700);
+        susumu_xr(1,1,700);//700,1700
     }
     if(i==9) {
-        susumu_ang(1/3,1,0);
+        susumu_ang(1/3,1,0);//390,2010
     }
     if(i==10) {
-        susumu_y(1,1,2200);
+        susumu_y(1,1,asari_y-310);//390,y-310
     }
     if(i==11) {
-        susumu_ang(1/3,1,90);
+        susumu_ang(1/3,1,90);//700,y
     }
     if(i==12) {
         motorR.stop();
         motorL.stop();
-        servo.pulsewidth_us(1000);
-        wait(0.5);
+        servo.pulsewidth_us(3000);
+        wait(1);
         i++;
     }
     if(i==13) {
@@ -385,7 +396,7 @@
         }
     }
     if(i==14) {
-        susumu_xl(1,1,1000);
+        susumu_xl(1,1,asari_x);//x,y
     }
     if(i==15) {
         motorR.stop();
@@ -393,29 +404,28 @@
         wait(0.5);
         denjiben=1;
         wait(0.5);
-        servo.pulsewidth_us(1500);
-        wait(0.5);
+        servo.pulsewidth_us(900);
+        wait(1);
         i++;
     }
     if(i==16) {
-        susumu_xr(-1,-1,700);
+        susumu_xr(-1,-1,700);//700,y
     }
     if(i==17) {
-        susumu_ang(-1/3,-1,0);
+        susumu_ang(-1/3,-1,0);//390,y-310
     }
     if(i==18) {
-        susumu_y(-1,-1,2000);
+        susumu_y(-1,-1,goal_y+310);//390,y+310
     }
     if(i==19) {
-        susumu_ang(-1/3,-1,-90);
+        susumu_ang(-1/3,-1,-90);//700,y
     }
     if(i==20) {
-        susumu_xl(-1,-1,1100);
+        susumu_xl(-1,-1,goal_x);//x,y
     }
     if(i==21) {
         motorR.stop();
         motorL.stop();
-        servo.pulsewidth_us(1500);
         wait(0.5);
         denjiben=0;
         wait(0.5);