File content as of revision 0:f1459eec7228:

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