Diff: main.cpp

Revision:

4:6a8cd13da02d

Parent:

3:8b9ba783512e

Child:

5:36cefeaeb594

--- a/main.cpp	Thu Mar 05 02:13:38 2020 +0000
+++ b/main.cpp	Thu Mar 05 07:09:27 2020 +0000
@@ -21,7 +21,7 @@
                };  //1逓倍用class
 
 Ec2multi ecXY[]= {Ec2multi(PC_6,PB_8,RESOLUTION),
-                  Ec2multi(PB_9,PC_8,RESOLUTION)
+                  Ec2multi(PC_8,PB_9,RESOLUTION)
                  };
 
 SpeedControl motor[]= {SpeedControl(PA_5,PC_7,50,ec[0]),
@@ -60,14 +60,18 @@
     {
         double ec_count[2]= {};
         double ax,ay,bx,by;
+        double atheta,btheta;
+        atheta = (45+theta)/180*3.14;
+        btheta = (135+theta)/180*3.14;
+
         ec_count[0]=ecXY[0].getCount();
         ec_count[1]=ecXY[1].getCount();
-        ax = (ec_count[0]-old_count[0])*cos(45+theta);
-        ay = (ec_count[0]-old_count[0])*sin(45+theta);
-        bx = (ec_count[1]-old_count[1])*cos(45-theta);
-        by = (ec_count[1]-old_count[1])*sin(45-theta);
+        ax = (ec_count[0]-old_count[0])*cos(atheta);
+        ay = (ec_count[0]-old_count[0])*sin(atheta);
+        bx = (ec_count[1]-old_count[1])*cos(btheta);
+        by = (ec_count[1]-old_count[1])*sin(btheta);
         x_=x_+ax + bx;
-        y_=y_+ay - by;
+        y_=y_+ay + by;
         old_count[0]=ec_count[0];
         old_count[1]=ec_count[1];
     }
@@ -91,9 +95,9 @@
 //目的地決定
 int plot[5][2]= {
     {0,0}
-    ,{0,10000}
-    ,{5000,10000}
-    ,{5000,0}
+    ,{0,4000}
+    ,{2000,4000}
+    ,{2000,0}
     ,{0,0}
 };
 
@@ -143,9 +147,9 @@
         motor[i].Sc(omega.getOmega(i));
         if(a%10==1) {
             for(int i=0; i<4; i++) {
-                printf("%d  %.2f",i,omega.getOmega(i));
+                //printf("%d %.2f /",i,omega.getOmega(i));
             }
-
+            //printf("\r\n");
         }
         a++;
     }
@@ -173,6 +177,7 @@
     while(1) {
         printf("waiting\r\n");
         if(button==0) {
+            wait(1);
             ticker.attach(motorOut,0.05);
             break;
         }
@@ -183,12 +188,12 @@
     Location location;
     while(1) {
         //自己位置取得
-        //theta=gyro.getAngle();    //角度の値を受け取る
+        theta=gyro.getAngle();    //角度の値を受け取る
         location.calXY();
 
         x=location.getX();
         y=location.getY();
-        //printf("location %d,%d \r\n",x,y);
+        printf("X=%d,Y=%d theta=%5.3f \r\n",x,y,theta);
 
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
@@ -200,26 +205,30 @@
         vy=dy/sqrt((double)dx*dx+dy*dy);
 
         //四輪の出力計算
-        omega.setOmega(20);
+        omega.setOmega(10);
         omega.setVxy(vx,vy);
         omega.calOmega();
 
-        if(dx<100 &&dx>-100) {
-            if(dy<100 && dy>-100) {
+        if(dx<300 &&dx>-300) {
+            if(dy<300 && dy>-300) {
                 n++;
-                printf("reached Location");
+                printf("reached Location %d\r\n",n);
+                ticker.detach();
                 for(int j=0; j<4; j++) {
-                    motor[j].stop();
+                    motor[j].Sc(0);
                 }
                 wait(2);
+                 ticker.attach(motorOut,0.05);
             }
 
         }
         if(n>4) {
             for(int j=0; j<4; j++) {
-                motor[j].stop();
+                motor[j].Sc(0);
+            }
+            while(1) {
+                printf("fin");
             }
         }
     }
-
 }