Revision 1:f102831401a8, committed 2019-12-17

Comitter:

sgrsn

Date:

Tue Dec 17 04:41:19 2019 +0000

Parent:

0:f1459eec7228

Child:

2:83fa142c5bcc

Commit message:

Fix for Self-localization Estimate

Changed in this revision

--- a/define.h	Mon Dec 16 10:38:07 2019 +0000
+++ b/define.h	Tue Dec 17 04:41:19 2019 +0000
@@ -31,6 +31,7 @@
 #define IIC_ADDR4        0xE0
 
 #define MaxFrequency   70000
+#define MinFrequency   0//50
 
 // Register Map from PC
 #define MARKER_X        0x05

--- a/main.cpp	Mon Dec 16 10:38:07 2019 +0000
+++ b/main.cpp	Tue Dec 17 04:41:19 2019 +0000
@@ -13,12 +13,91 @@
 #define SIZE 6
 #define ArraySize(array) (sizeof(array) / sizeof(array[0]))
 
-float DEG_TO_RAD = PI/180.0;
+float wheel_d = 127;           // メカナム直径[mm]
+float wheel_r = 63.5;
+float deg_per_pulse = 0.0072;   // ステッピングモータ(AR46SAK-PS50-1)
 
-void controlMotor(int ch, int frequency);
+float DEG_TO_RAD = PI/180.0;
+float RAD_TO_DEG = 180.0/PI;
+
+int controlMotor(int ch, int frequency);
 void coastAllMotor();
 void controlFrequencyFromTerminal();
 void serialRead();
+void controlFromWASD();
+
+class CountWheel
+{
+    public:
+    CountWheel(Timer *t)
+    {
+        _t = t;
+        _t -> start();
+    }
+    float getRadian(float frequency)
+    {
+        last_time = current_time;
+        current_time = _t -> read();
+        float dt = current_time - last_time;
+        float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
+        return delta_rad;
+    }
+    
+    private:
+    Timer *_t;
+    float last_time;
+    float current_time;
+};
+
+class MakePath
+{
+    public:
+    MakePath()
+    {
+    }
+    int makePath(int targetX, int targetY, int targetZ, int x, int y, int z)
+    {
+        int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0;   //5mm間隔
+        //printf("num = %d\r\n", num);
+        for(int i = 1; i <= num; i++)
+        {
+            float a = float(i) / num;
+            PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD);
+            PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD);
+            PATH[i-1][2] = z + float(targetZ - z) * a;
+        }
+        if(num > 0)
+        {
+            PATH[num-1][0] = targetX;
+            PATH[num-1][1] = targetY;
+            PATH[num-1][2] = targetZ;
+        }
+        else
+        {
+            PATH[0][0] = targetX;
+            PATH[0][1] = targetY;
+            PATH[0][2] = targetZ;
+            num = 1;
+        }
+        return num;
+    }
+    
+    int getPathX(int i)
+    {
+        return PATH[i][0];
+    }
+    int getPathY(int i)
+    {
+        return PATH[i][1];
+    }
+    int getPathZ(int i)
+    {
+        return PATH[i][2];
+    }
+    
+    private:
+    int PATH[500][3];
+};
 
 int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
 int Register[0x20] = {};
@@ -28,12 +107,12 @@
 BusOut LEDs(LED1, LED2, LED3, LED4);
 Timer timer;
 JY901 jy901(&master, &timer);
-
+MakePath myPath;
 
 void controlFromGcode()
 {
-    float threshold_x     = 0; //[mm]
-    float threshold_y     = 0; //[mm]
+    float threshold_x     = 5; //[mm]
+    float threshold_y     = 5; //[mm]
     float threshold_theta = 5 * DEG_TO_RAD;
     
     // 角度補正係数
@@ -43,9 +122,9 @@
     PID pid_x(&timer2);
     PID pid_y(&timer2);
     PID pid_theta(&timer2);
-    pid_x.setParameter(100.0, 0.0, 0.0);
-    pid_y.setParameter(100.0, 0.0, 0.0);
-    pid_theta.setParameter(1.0, 0.0, 0.0);
+    pid_x.setParameter(200.0, 0.0, 0.0);
+    pid_y.setParameter(200.0, 0.0, 0.0);
+    pid_theta.setParameter(100.0, 0.0, 0.0);
     
     // Gコード読み取り
     LocalFileSystem local("local");
@@ -56,48 +135,89 @@
     int row = 1;
     float v[4] = {};
     
-    TextLCD lcd(p24, p26, p27, p28, p29, p30);
+    Timer temp_t;
+    CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)};
+    float wheel_rad[4] = {};
+    
+    //TextLCD lcd(p24, p26, p27, p28, p29, p30);
+    
+    float x_robot = 0;
+    float y_robot = 0;
+    float theta_robot = 0;
+    
+    
+    // 目標位置把握
+    code.getGcode(row,sizeof(array)/sizeof(int),array);
+    float x_desire = array[0];
+    float y_desire = array[1];
+    float theta_desire = float(array[2]) *DEG_TO_RAD;
+    int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot);
+    
+    int path = 0;
     
     while(1)
     {
    
         // 自己位置推定
-        float x_robot = Register[MARKER_X];
-        float y_robot = Register[MARKER_Y];
-        float theta_robot = float(Register[MARKER_YAW]) / 1000.0;
-        float theta_robot_formJyro = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
+        //float x_robot = Register[MARKER_X];
+        //float y_robot = Register[MARKER_Y];
+        //float theta_robot = float(Register[MARKER_YAW]) / 1000.0;
+        theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
+        for(int i = 0; i < MOTOR_NUM; i++)
+        {
+            wheel_rad[i] = counter[i].getRadian(v[i]);
+        }
+        float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.7;
+        float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.7;
         
-        lcd.printf("%d,%d,%d\n", (int)x_robot, (int)y_robot, (int)(theta_robot*180/PI));
-        
-        // 目標位置把握
-        code.getGcode(row,sizeof(array)/sizeof(int),array);
-        float x_desire = array[0];
-        float y_desire = array[1];
-        float theta_desire = float(array[2]) *DEG_TO_RAD;
+        x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot);
+        y_robot += dy * cos(theta_robot) + dx * sin(theta_robot);
         
         // 目標位置判定
         float err_x = x_desire - x_robot;
         float err_y = y_desire - y_robot;
         float err_theta = theta_desire - theta_robot;
         
+        //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row);
+        printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row);
+        //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row);
+        
         // 目標位置到達
         if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
         {
             // 車輪を停止
             coastAllMotor();
+            //pid_x.reset();
+            //pid_y.reset();
+            //pid_theta.reset();
+            wait_ms(500);
+            jy901.reset();
             
             // Gコードを次の行に
             row++;
+            if(row > code.getGcodeSize())
+            {
+                row = 0;
+            }
+            
+            // 目標位置把握
             code.getGcode(row, ArraySize(array), array);
+            x_desire = array[0];
+            y_desire = array[1];
+            theta_desire = float(array[2]) *DEG_TO_RAD;
+            pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG);
+            path = 0;
         }
         
         // 目標速度計算
         else
         {
             // 慣性座標での速度
-            float xI_dot = pid_x.controlPID(x_desire, x_robot);
-            float yI_dot = pid_y.controlPID(y_desire, y_robot);
-            float theta_dot = pid_theta.controlPID(theta_desire, theta_robot);
+            float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot);
+            float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot);
+            float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot);
+            path++;
+            if(path >= pathSize) path = pathSize-1;
             
             // ロボット座標での速度
             float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
@@ -114,7 +234,7 @@
             
             for(int i = 0; i < MOTOR_NUM; i++)
             {
-                controlMotor(i, (int)v[i] );
+                controlMotor(i, (int)v[i]);
             }
         }
     }
@@ -124,20 +244,26 @@
 {
     coastAllMotor();
     PC.baud(9600);
-    PC.attach(serialRead);
-    //jy901.calibrateAll(5000);
-    
+    //PC.attach(serialRead);
+    jy901.calibrateAll(5000);
+    //controlFromWASD();
+    PC.printf("\r\nI'm ready to start. Press Enter\r\n");
+    bool startable = false;
+    while(!startable)
+    {
+        startable = PC.readable() > 0;
+    }
     controlFromGcode();
 }
 
-
-void controlMotor(int ch, int frequency)
+int controlMotor(int ch, int frequency)
 {    
     int dir = COAST;
     int size = 4;
     if(ch < 0 || ch > 3)
     {
         //channel error
+        return 0;
     }
     else
     {
@@ -156,9 +282,12 @@
         }
         // 周波数制限 脱調を防ぐ
         if(frequency > MaxFrequency) frequency = MaxFrequency;
+        //else if(frequency < MinFrequency) frequency = MinFrequency;
         
         master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
         master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
+        
+        return frequency;
     }   
 }