Diff: main.cpp

Revision:

1:9d2b2b5ec36f

Parent:

0:c17bc30288a2

Child:

2:08b8f6b05c22

Child:

3:8b9ba783512e

--- a/main.cpp	Wed Mar 04 02:07:55 2020 +0000
+++ b/main.cpp	Wed Mar 04 05:31:39 2020 +0000
@@ -33,6 +33,8 @@
 //R1370P gyro(PA_11,PA_12);
 
 DigitalIn button(USER_BUTTON);
+Serial pc(USBTX, USBRX); // tx, rx
+R1370P gyro(PC_10,PC_11);   // tx, rx
 
 Ticker ticker;
 
@@ -44,6 +46,7 @@
 }
 
 //自己位置取得
+double theta=0;
 class Location
 {
 public:
@@ -56,13 +59,15 @@
     void calXY()
     {
         double ec_count[2]= {};
-        double a,b;
+        double ax,ay,bx,by;
         ec_count[0]=ecXY[0].getCount();
         ec_count[1]=ecXY[1].getCount();
-        a = (ec_count[0]-old_count[0])/sqrt(2.0);
-        b = (ec_count[1]-old_count[1])/sqrt(2.0);
-        x_=x_+a + b;
-        y_=y_+a - b;
+        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);
+        x_=x_+ax + bx;
+        y_=y_+ay - by;
         old_count[0]=ec_count[0];
         old_count[1]=ec_count[1];
     }
@@ -82,35 +87,39 @@
 };
 
 
+
+//目的地決定
 int plot[5][2]= {
     {0,0}
-    ,{0,3000}
-    ,{3000,3000}
-    ,{3000,0}
+    ,{0,10000}
+    ,{5000,10000}
+    ,{5000,0}
     ,{0,0}
 };
 
-//目的地決定
-/*class Aim
-{
-    Aim(int plot[][2]):plot_(&plot[0][0]) {}
-public:
-private:
-    int *(plot_[2]);
-};*/
-
 //出力を計算
 double omega[4];
 int x,y;
+//出力
+double vx,vy;
+void setOmega(int max)
+{
+    omega[0]=max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
+    omega[1]=-max*vx/sqrt(2.0)-max*vy/sqrt(2.0);
+    omega[2]=-max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
+    omega[3]=max*vx/sqrt(2.0)+max*vy/sqrt(2.0);
+}
 void motorOut()
 {
     for(int i=0; i<4; i++) {
-        motor[i].Sc(omega[i]);
+        motor[i].Sc((int)omega[i]);
     }
 }
 int main()
 {
-    ticker.attach(motorOut,0.09);
+    gyro.initialize();    //main関数の最初に一度だけ実行
+    gyro.acc_offset();
+    ticker.attach(motorOut,0.05);
     printf("start\r\n");
     motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
     motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806);
@@ -128,15 +137,16 @@
     motor[3].setPDparam( 0.004801, 0.026645 );
 
     int n=0,dx,dy,aimX,aimY;
-    double vx,vy;
     Location location;
-    // Aim aim(&(plot[0]));
     while(1) {
         //自己位置取得
+        theta=gyro.getAngle();    //角度の値を受け取る
         location.calXY();
+
         x=location.getX();
         y=location.getY();
-        printf("%lf  %lf  %lf  %lf  location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);
+        printf("%3.3f  %3.3f  %3.3f  %3.3f  location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);
+
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
         aimY = plot[n][1];
@@ -146,25 +156,25 @@
         vx=dx/sqrt((double)dx*dx+dy*dy);
         vy=dy/sqrt((double)dx*dx+dy*dy);
 
-        //cal 4 wheel
-        omega[0]= -30*vx/sqrt(2.0)+30*vy/sqrt(2.0);
-        omega[1]=-30*vx/sqrt(2.0)-30*vy/sqrt(2.0);
-        omega[2]=  30*vx/sqrt(2.0)-30*vy/sqrt(2.0);
-        omega[3]=  30*vx/sqrt(2.0)+30*vy/sqrt(2.0);
-        
-        int j=0;
-        if(dx<50 &&dx>-50) {
-            if(dy<50 && dy>-50) {
+        //四輪の出力計算
+        setOmega(10);
+
+        if(dx<100 &&dx>-100) {
+            if(dy<100 && dy>-100) {
                 n++;
                 printf("reached Location");
-                while(j<4) {
+                for(int j=0; j<4; j++) {
                     motor[j].stop();
-                    j++;
                 }
-                wait(1);
+                wait(2);
             }
 
         }
+        if(n>4) {
+            for(int j=0; j<4; j++) {
+                motor[j].stop();
+            }
+        }
     }
 
 }