Diff: main.cpp

Revision:

0:c17bc30288a2

Child:

1:9d2b2b5ec36f

diff -r 000000000000 -r c17bc30288a2 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 04 02:07:55 2020 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+#include "EC.h"
+#include "SpeedController.h"
+#define RESOLUTION 500
+#include "math.h"
+#include "R1370P.h"
+
+//PwmOut motor_1F(PA_5);//1Forward Right motor Forward
+//PwmOut motor_1B(PC_7);//Forward Right motor Back
+//PwmOut motor_2F(PC_9);//2Forward Left motor Forward
+//PwmOut motor_2B(PA_1);//Forward Left motor Back
+//PwmOut motor_3F(PA_10);//3Back Right motor Forward
+//PwmOut motor_3B(PB_4);//Back Right motor Back
+//PwmOut motor_4F(PA_9);//4Back Left motor Forward
+//PwmOut motor_4B(PA_7);//Back Left motor Back
+
+Ec2multi ec[]= {Ec2multi(PC_5,PB_2,RESOLUTION),
+                Ec2multi(PA_11,PB_1,RESOLUTION),
+                Ec2multi(PB_12,PB_15,RESOLUTION),
+                Ec2multi(PC_4,PB_14,RESOLUTION)
+               };  //1逓倍用class
+
+Ec2multi ecXY[]= {Ec2multi(PC_6,PB_8,RESOLUTION),
+                  Ec2multi(PB_9,PC_8,RESOLUTION)
+                 };
+
+SpeedControl motor[]= {SpeedControl(PA_5,PC_7,50,ec[0]),
+                       SpeedControl(PC_9,PA_1,50,ec[1]),
+                       SpeedControl(PA_10,PB_4,50,ec[2]),
+                       SpeedControl(PA_9,PA_7,50,ec[3])
+                      };
+
+//R1370P gyro(PA_11,PA_12);
+
+DigitalIn button(USER_BUTTON);
+
+Ticker ticker;
+
+void calOmega()
+{
+    for(int i=0; i<4; i++) {
+        ec[i].calOmega();
+    }
+}
+
+//自己位置取得
+class Location
+{
+public:
+    Location():x_(0),y_(0)
+    {
+        for(int i =0; i<2; i++) {
+            old_count[i]=0;
+        }
+    }
+    void calXY()
+    {
+        double ec_count[2]= {};
+        double a,b;
+        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;
+        old_count[0]=ec_count[0];
+        old_count[1]=ec_count[1];
+    }
+    double getX()
+    {
+        return x_;
+    }
+    double getY()
+    {
+        return y_;
+    }
+
+private:
+    double x_;
+    double y_;
+    double old_count[2];
+};
+
+
+int plot[5][2]= {
+    {0,0}
+    ,{0,3000}
+    ,{3000,3000}
+    ,{3000,0}
+    ,{0,0}
+};
+
+//目的地決定
+/*class Aim
+{
+    Aim(int plot[][2]):plot_(&plot[0][0]) {}
+public:
+private:
+    int *(plot_[2]);
+};*/
+
+//出力を計算
+double omega[4];
+int x,y;
+void motorOut()
+{
+    for(int i=0; i<4; i++) {
+        motor[i].Sc(omega[i]);
+    }
+}
+int main()
+{
+    ticker.attach(motorOut,0.09);
+    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);
+    motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159);
+    motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645);
+
+    motor[0].setDutyLimit(0.5);
+    motor[1].setDutyLimit(0.5);
+    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 );
+
+    int n=0,dx,dy,aimX,aimY;
+    double vx,vy;
+    Location location;
+    // Aim aim(&(plot[0]));
+    while(1) {
+        //自己位置取得
+        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);
+        //目的地決定(syuusoku check)
+        aimX = plot[n][0];
+        aimY = plot[n][1];
+        //出力を計算(kitai xy);
+        dx=aimX-x;
+        dy=aimY-y;
+        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) {
+                n++;
+                printf("reached Location");
+                while(j<4) {
+                    motor[j].stop();
+                    j++;
+                }
+                wait(1);
+            }
+
+        }
+    }
+
+}