Diff: main.cpp

Revision:

0:f1459eec7228

Child:

1:f102831401a8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 16 10:38:07 2019 +0000
@@ -0,0 +1,271 @@
+#include "mbed.h"
+#include <string> 
+#include "i2cmaster.h"
+#include "JY901.h"
+#include "PID.h"
+#include "MakeSequencer.h"
+#include "define.h"
+
+#include "TextLCD.h"
+
+
+// MakeSequencer
+#define SIZE 6
+#define ArraySize(array) (sizeof(array) / sizeof(array[0]))
+
+float DEG_TO_RAD = PI/180.0;
+
+void controlMotor(int ch, int frequency);
+void coastAllMotor();
+void controlFrequencyFromTerminal();
+void serialRead();
+
+int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
+int Register[0x20] = {};
+
+Serial PC(USBTX, USBRX);
+i2c master(p28, p27);
+BusOut LEDs(LED1, LED2, LED3, LED4);
+Timer timer;
+JY901 jy901(&master, &timer);
+
+
+void controlFromGcode()
+{
+    float threshold_x     = 0; //[mm]
+    float threshold_y     = 0; //[mm]
+    float threshold_theta = 5 * DEG_TO_RAD;
+    
+    // 角度補正係数
+    float L = 233; //[mm]
+    
+    Timer timer2;
+    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);
+    
+    // Gコード読み取り
+    LocalFileSystem local("local");
+    int array[SIZE] = {};
+    FILE *fp = fopen( "/local/guide1.txt", "r");
+    MakeSequencer code(fp);
+    
+    int row = 1;
+    float v[4] = {};
+    
+    TextLCD lcd(p24, p26, p27, p28, p29, p30);
+    
+    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;
+        
+        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;
+        
+        // 目標位置判定
+        float err_x = x_desire - x_robot;
+        float err_y = y_desire - y_robot;
+        float err_theta = theta_desire - theta_robot;
+        
+        // 目標位置到達
+        if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
+        {
+            // 車輪を停止
+            coastAllMotor();
+            
+            // Gコードを次の行に
+            row++;
+            code.getGcode(row, ArraySize(array), array);
+        }
+        
+        // 目標速度計算
+        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 x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot;
+            float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot;
+            
+            // 各車輪の速度
+            v[0] = -x_dot - y_dot - L * theta_dot;
+            v[1] =  x_dot - y_dot - L * theta_dot;
+            v[2] =  x_dot + y_dot - L * theta_dot;
+            v[3] = -x_dot + y_dot - L * theta_dot;
+            
+            // 本当はこうするべき
+            // f = v * ppr / ( 2*PI * r);
+            
+            for(int i = 0; i < MOTOR_NUM; i++)
+            {
+                controlMotor(i, (int)v[i] );
+            }
+        }
+    }
+}
+
+int main()
+{
+    coastAllMotor();
+    PC.baud(9600);
+    PC.attach(serialRead);
+    //jy901.calibrateAll(5000);
+    
+    controlFromGcode();
+}
+
+
+void controlMotor(int ch, int frequency)
+{    
+    int dir = COAST;
+    int size = 4;
+    if(ch < 0 || ch > 3)
+    {
+        //channel error
+    }
+    else
+    {
+        if(frequency > 0)
+        {
+            dir = CW;
+        }
+        else if(frequency < 0)
+        {
+            dir = CCW;
+            frequency = -frequency;
+        }
+        else
+        {
+            dir = BRAKE;
+        }
+        // 周波数制限 脱調を防ぐ
+        if(frequency > MaxFrequency) frequency = MaxFrequency;
+        
+        master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
+        master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
+    }   
+}
+
+
+void coastAllMotor()
+{
+    for(int i = 0; i < MOTOR_NUM; i++)
+    {
+        master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
+    }
+}
+
+void serialRead()
+{
+    int reg = 0;
+    uint8_t data[4] = {};
+    if(PC.readable() > 0)
+    {
+        reg = PC.getc();
+        data[0] = PC.getc();
+        data[1] = PC.getc();
+        data[2] = PC.getc();
+        data[3] = PC.getc();
+    }
+    Register[reg % 0x20] = 0;
+    for(int i = 0; i < 4; i++)
+        Register[reg % 0x20] |= int(data[i]) << (i*8);
+}
+
+
+
+/*Function for Test***********************************************************/
+
+void controlFrequencyFromTerminal()
+{
+    int ch, speed;
+    if(PC.readable() > 0)
+    {
+        PC.scanf("M%d:%d", &ch, &speed);
+        PC.printf("M%d:%d\r\n", ch, speed);
+        if(ch < 0 || ch > 3)
+            PC.printf("channnel error\r\n");
+        else
+        {
+            controlMotor(ch, speed);
+        }
+    }
+}
+
+void controlFromWASD()
+{
+    float L = 4.0;
+    float v[4] = {};
+    char input = 0;
+    while(1)
+    {
+        if(PC.readable() > 0)
+        {
+            input = PC.getc();
+            //printf("%c\r\n", input);
+        }
+        
+        float xI_dot = 0;
+        float yI_dot = 0;
+        switch(input)
+        {
+            case 'a':
+                xI_dot = -1;
+                yI_dot = 0;
+                break;
+            case 'd':
+                xI_dot = 1;
+                yI_dot = 0;
+                break;
+            case 'w':
+                xI_dot = 0;
+                yI_dot = 1;
+                break;
+            case 's':
+                xI_dot = 0;
+                yI_dot = -1;
+                break;
+            case ' ':
+                xI_dot = 0;
+                yI_dot = 0;
+                break;
+        }
+        //master.getSlaveRegistarData(addr, 1, &data, size);
+        
+        float theta_z = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD;
+        
+        float x_dot = cos(theta_z) * xI_dot + sin(theta_z) * yI_dot;
+        float y_dot = -sin(theta_z) * xI_dot + cos(theta_z) * yI_dot;
+        float theta_dot = 0.0 - theta_z;
+        
+        v[1] =  x_dot - y_dot - L * theta_dot;
+        v[2] =  x_dot + y_dot - L * theta_dot;
+        v[3] = -x_dot + y_dot - L * theta_dot;
+        v[0] = -x_dot - y_dot - L * theta_dot;
+        
+        for(int i = 0; i < MOTOR_NUM; i++)
+        {
+            controlMotor(i, v[i] * 20000.0);
+        }
+        
+        PC.printf("%f, %f, %f, %f\r\n", v[0], v[1], v[2], v[3]);
+        
+        //PC.printf("%f\r\n", theta_z);
+    }
+}