Diff: main.cpp

Revision:

3:8b9ba783512e

Parent:

1:9d2b2b5ec36f

Child:

4:6a8cd13da02d

--- a/main.cpp	Wed Mar 04 05:31:39 2020 +0000
+++ b/main.cpp	Thu Mar 05 02:13:38 2020 +0000
@@ -98,28 +98,62 @@
 };
 
 //出力を計算
-double omega[4];
 int x,y;
-//出力
-double vx,vy;
-void setOmega(int max)
+
+class WheelOmega
 {
-    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);
-}
+public:
+    WheelOmega(): max_(0),vx_(0),vy_(0)
+    {
+        for(int i=0; i<4; i++) {
+            omega[i]=0;
+        }
+    }
+    void setOmega(int max)
+    {
+        max_=max;
+    }
+    void setVxy(double vx,double vy)
+    {
+        vx_=vx;
+        vy_=vy;
+    }
+    void calOmega()
+    {
+        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);
+    };
+    double getOmega(int i)
+    {
+        return omega[i];
+    }
+private:
+    double max_,vx_,vy_;
+    double omega[4];
+};
+
+WheelOmega omega;
+//出力
+int a=0;
 void motorOut()
 {
     for(int i=0; i<4; i++) {
-        motor[i].Sc((int)omega[i]);
+        motor[i].Sc(omega.getOmega(i));
+        if(a%10==1) {
+            for(int i=0; i<4; i++) {
+                printf("%d  %.2f",i,omega.getOmega(i));
+            }
+
+        }
+        a++;
     }
 }
 int main()
 {
     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);
@@ -131,21 +165,30 @@
     motor[2].setDutyLimit(0.5);
     motor[3].setDutyLimit(0.5);
 
-    motor[0].setPDparam( 0.004839, 0.0026290 );
-    motor[1].setPDparam( 0.004702, 0.025806 );
-    motor[2].setPDparam( 0.004397, 0.025159 );
-    motor[3].setPDparam( 0.004801, 0.026645 );
+    motor[0].setPDparam( 0.02000, 0.0005 );
+    motor[1].setPDparam( 0.02000, 0.0005 );
+    motor[2].setPDparam( 0.02000, 0.0005 );
+    motor[3].setPDparam( 0.02000, 0.0005);
 
-    int n=0,dx,dy,aimX,aimY;
+    while(1) {
+        printf("waiting\r\n");
+        if(button==0) {
+            ticker.attach(motorOut,0.05);
+            break;
+        }
+    }
+
+    int n=1,dx,dy,aimX,aimY;
+    double vx,vy;
     Location location;
     while(1) {
         //自己位置取得
-        theta=gyro.getAngle();    //角度の値を受け取る
+        //theta=gyro.getAngle();    //角度の値を受け取る
         location.calXY();
 
         x=location.getX();
         y=location.getY();
-        printf("%3.3f  %3.3f  %3.3f  %3.3f  location %d,%d \r\n",omega[0],omega[1],omega[2],omega[3],x,y);
+        //printf("location %d,%d \r\n",x,y);
 
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
@@ -157,7 +200,9 @@
         vy=dy/sqrt((double)dx*dx+dy*dy);
 
         //四輪の出力計算
-        setOmega(10);
+        omega.setOmega(20);
+        omega.setVxy(vx,vy);
+        omega.calOmega();
 
         if(dx<100 &&dx>-100) {
             if(dy<100 && dy>-100) {