File content as of revision 1:f102831401a8:

#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 wheel_d = 127;           // メカナム直径[mm]
float wheel_r = 63.5;
float deg_per_pulse = 0.0072;   // ステッピングモータ(AR46SAK-PS50-1)

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] = {};

Serial PC(USBTX, USBRX);
i2c master(p28, p27);
BusOut LEDs(LED1, LED2, LED3, LED4);
Timer timer;
JY901 jy901(&master, &timer);
MakePath myPath;

void controlFromGcode()
{
    float threshold_x     = 5; //[mm]
    float threshold_y     = 5; //[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(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");
    int array[SIZE] = {};
    FILE *fp = fopen( "/local/guide1.txt", "r");
    MakeSequencer code(fp);
    
    int row = 1;
    float v[4] = {};
    
    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;
        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;
        
        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(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;
            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);
    //controlFromWASD();
    PC.printf("\r\nI'm ready to start. Press Enter\r\n");
    bool startable = false;
    while(!startable)
    {
        startable = PC.readable() > 0;
    }
    controlFromGcode();
}

int controlMotor(int ch, int frequency)
{    
    int dir = COAST;
    int size = 4;
    if(ch < 0 || ch > 3)
    {
        //channel error
        return 0;
    }
    else
    {
        if(frequency > 0)
        {
            dir = CW;
        }
        else if(frequency < 0)
        {
            dir = CCW;
            frequency = -frequency;
        }
        else
        {
            dir = BRAKE;
        }
        // 周波数制限 脱調を防ぐ
        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;
    }   
}


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);
    }
}