File content as of revision 6:55e60b9d7ff1:

#include "mbed.h"
#include <string> 
#include "i2cmaster.h"
#include "JY901.h"
#include "PID.h"
#include "MakeSequencer.h"
#include "define.h"

#include <ros.h>
#include <std_msgs/Empty.h>

#include "TextLCD.h"

// robot start when startable is true
bool startable = false;

// for ROS
ros::NodeHandle nh;
void messageCb(const std_msgs::Empty& toggle_msg){
    startable = true;
}
ros::Subscriber<std_msgs::Empty> sub("/robot/start", &messageCb);

// MakeSequencer
#define SIZE 6
#define ArraySize(array) (sizeof(array) / sizeof(array[0]))

// Robot Parameter
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;

// Prototype
int controlMotor(int ch, int frequency);
void coastAllMotor();

// this class Count Stepping Motor Pulse for wheel
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;
};

// this class make Robot Path to complement target value linearly
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];
};

// IIC address
int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
int Register[0x20] = {};

i2c master(p28, p27);
BusOut LEDs(LED1, LED2, LED3, LED4);
Timer timer;
JY901 jy901(&master, &timer);   // gyro sensor
MakePath myPath;

// this function is Robot Main Program
void controlFromGcode()
{
    // Threshold for reaching target value
    float threshold_x     = 5; //[mm]
    float threshold_y     = 5; //[mm]
    float threshold_theta = 1 * 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);
    
    // read the txt file for get target position
    // Gコード読み取り
    LocalFileSystem local("local");
    int array[SIZE] = {};
    FILE *fp = fopen( "/local/guide1.txt", "r");
    MakeSequencer code(fp);
    
    //printf("size:%d\r\n", code.getGcodeSize());
    for(int i = 0; i < code.getGcodeSize(); i++)
    {
        code.getGcode(i,sizeof(array)/sizeof(int),array);
        printf("%d, %d, %d\r\n", array[0], array[1], i);
    }
    
    int row = 1;    // row of txt file
    float v[4] = {};    // frequency each wheel
    
    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)
    {
        nh.spinOnce();
        // 自己位置推定(Estimate Self-localization)
        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.67;
        float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67;
        
        x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot);
        y_robot += dy * cos(theta_robot) + dx * sin(theta_robot);
        
        // caliculate error of current position
        // 目標位置判定
        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);
        //lcd.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 Reach target position 
        // 目標位置到達
        if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta)
        {
            // At first, Stop all Motor
            // 車輪を停止
            coastAllMotor();
            //pid_x.reset();
            //pid_y.reset();
            //pid_theta.reset();
            wait_ms(500);
            
            // gyro reset
            jy901.reset();
            
            // update target position
            // Gコードを次の行に
            row++;
            if(row >= code.getGcodeSize())
            {
                row = 0;
                coastAllMotor();
                startable = false;
                while(!startable)
                {
                    nh.spinOnce();
                    wait_ms(1);
                }
            }
            
            jy901.reset();
            v[0] = 0;
            v[1] = 0;
            v[2] = 0;
            v[3] = 0;
            
            // get next target position
            // 目標位置把握
            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;
        }
        
        // caliculate velosity
        // 目標速度計算
        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);
            
            // Control Motor velocity
            for(int i = 0; i < MOTOR_NUM; i++)
            {
                controlMotor(i, (int)v[i]);
            }
        }
    }
}

int main()
{
    nh.getHardware()->setBaud(9600);
    nh.initNode();
    nh.subscribe(sub);
    coastAllMotor();
    // gyro sensor calibration 
    // Please wait 5 seconds...
    jy901.calibrateAll(5000);
    
    // Robot start when receive the ROS topic
    while(!startable)
    {
        nh.spinOnce();
        wait_ms(1);
    }
    // main function
    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);
    }
}