File content as of revision 21:093c8525349a:

#include "mbed.h"

#include "Ping.h"
#include "autocar.h"

const int MOTOR_M1 = 0;
const int MOTOR_M2 = 1;
const int DIR_FORWARD = 0;
const int DIR_BACKWARD = 1;
const int PWR_STOP = 0;

// used for IR sensors
int left, middle, right;

// PID factors
int interror = 0;
int olderror = 0;
int limit = 4095; // 12-bit ADC in STM32

// used for output message via serial
Serial pc(SERIAL_TX, SERIAL_RX);

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void readIR(bool* left, bool* middle, bool* right, int threshold)
{
    // not on track: > 500
    // on track (black): < 500
    *left = leftIR.read_u16() < threshold ? true : false;
    *middle = middleIR.read_u16() < threshold ? true : false;
    *right = rightIR.read_u16() < threshold ? true : false;
}

int readIRValues()
{
    left = leftIR.read_u16();
    middle = middleIR.read_u16();
    right = rightIR.read_u16();

    int ret = left * (-1) + middle * 0 + right * 1;

    //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
    pc.printf("IR values: %d\r\n", ret);

    return ret;
}

void readSensor(bool* left, bool* middle, bool* right, bool* hasObstacle, int threshold, int range)
{
    int distance;
    ultrasonic.Send();
    wait_ms(30);
    distance = ultrasonic.Read_cm();
    *hasObstacle = distance < range ? true : false;
    
    // not on track: > 500
    // on track (black): < 500
    *left = leftIR.read_u16() < threshold ? true : false;
    *middle = middleIR.read_u16() < threshold ? true : false;
    *right = rightIR.read_u16() < threshold ? true : false;
}

void readSensorValues(int *values, bool *hasObstacle, int range)
{
    left = leftIR.read_u16();
    middle = middleIR.read_u16();
    right = rightIR.read_u16();
    *values = left * (-1) + middle * 0 + right * 1;
    
    int distance;
    ultrasonic.Send();
    wait_ms(30);
    distance = ultrasonic.Read_cm();
    *hasObstacle = distance < range ? true : false;
}

// m: 0 => M1      1 => M2
// power 0~10     dir: 0=>正向　1=>反向
void DriveSingleMotor(int m, int speed, int dir)
{
    /*int _speed = 0;

    //　設定速度
    if (speed>10) speed=10;
    _speed = map(speed, 1, 10, 100, 255);

    if (speed<=0) {
        speed=0;
        _speed=0;
    } else
        _speed = map(speed, 1, 10, 100, 255);*/

    if (m == MOTOR_M1) {
        //　設定方向
        if (speed == PWR_STOP) {
            M1_in1 = 0;
            M1_in2 = 0;
        } else if (dir ==  DIR_FORWARD) {
            // right wheel go forward
            M1_in1 = 1;
            M1_in2 = 0;
        } else {
            // right wheel go backward
            M1_in1 = 0;
            M1_in2 = 1;
        }
        //M1_enable.write_u16(_speed); // 驅動馬達  右輪
        M1_enable.write(1);

    } else if (m == MOTOR_M2) {
        //　設定方向
        if (speed == PWR_STOP) {
            M2_in3 = 0;
            M2_in4 = 0;
        } else if (dir == DIR_FORWARD) {
            //左輪前進
            M2_in3 = 1;
            M2_in4 = 0;
        } else {
            //左輪倒轉
            M2_in3 = 0;
            M2_in4 = 1;
        }
        //M2_enable.write_u16(_speed); // 驅動馬達  左輪
        M2_enable.write(1);
    } else {
        //M1_enable.write_u16(PWR_STOP); // right wheel
        //M2_enable.write_u16(PWR_STOP); // left wheel
        M1_enable.write(0);
        M2_enable.write(0);
    }
}

void driveMotor(bool left, bool middle, bool right)
{
    int status = left * 4 + middle * 2 + right;
    switch(status) {
        case 7: // go straight
            forward();
            break;
        case 6: // turn left
            turnLeft();
            break;
        case 5: // go straight
            forward();
            break;
        case 4: // turn left
            turnLeft();
            break;
        case 3: // turn right
            turnRight();
            break;
        case 2: // go straight
            forward();
            break;
        case 1: // turn right
            turnRight();
            break;
        case 0: // go straight
            forward();
            break;
        default:
            pc.printf("invalid\r\n");
            break;
    }
}

void driveMotor(bool left, bool middle, bool right, bool hasObstacle)
{
    if(hasObstacle)
    {
        turnRight();
        wait(0.3);
        forward();
        wait(0.5);
        turnLeft();
        wait(0.3);
        forward();
        wait(0.5);
        turnLeft();
        wait(0.3);
        forward();
        wait(0.5);
        turnRight();
        wait(0.3);
        
        return;
    }
    
    int status = left * 4 + middle * 2 + right;
    switch(status) {
        case 7: // go straight
            forward();
            break;
        case 6: // turn left
            turnLeft();
            break;
        case 5: // go straight
            forward();
            break;
        case 4: // turn left
            turnLeft();
            break;
        case 3: // turn right
            turnRight();
            break;
        case 2: // go straight
            forward();
            break;
        case 1: // turn right
            turnRight();
            break;
        case 0: // go straight
            forward();
            break;
        default:
            pc.printf("invalid\r\n");
            break;
    }
}

void driveMotorPID(int values, float Kp, float Ki, float Kd)
{
    int error = values; // 'P'
    interror += error;
    int lasterror = error - olderror;
    olderror = error; // 注意olderror的順序
     int power = error * Kp + interror * Ki + lasterror * Kd;
    /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */
    
    // limit PID output value (for 12-bit ADC in STM32)
    if(power > limit) {
        power = limit;
    } else if(power < -limit) {
        power = -limit;
    }
    
    //pc.printf("%d \r\n", power);
    
    // control the direction of auto car
    if(power < -800) {
        turnLeft();
    } else if (power > 800) {
        turnRight();
    } else {
        forward();
    }
}

void run(int values, float Kp, float Ki, float Kd, bool hasObstacle)
{
    if(hasObstacle)
    {
        turnRight();
        wait(0.3);
        forward();
        wait(0.5);
        turnLeft();
        wait(0.3);
        forward();
        wait(0.5);
        turnLeft();
        wait(0.3);
        forward();
        wait(0.5);
        turnRight();
        wait(0.3);
        
        return;
    }
    
    int error = values; // 'P'
    interror += error;
    int lasterror = error - olderror;
    olderror = error; // 注意olderror的順序
     int power = error * Kp + interror * Ki + lasterror * Kd;
    /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */
    
    // limit PID output value (for 12-bit ADC in STM32)
    if(power > limit) {
        power = limit;
    } else if(power < -limit) {
        power = -limit;
    }
    
    //pc.printf("%d \r\n", power);
    
    // control the direction of auto car
    if(power < -800) {
        turnLeft();
    } else if (power > 800) {
        turnRight();
    } else {
        forward();
    }
}

void init()
{
    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
}

void stop()
{
    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
}

void forward()
{
    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
}

void turnLeft()
{
    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
}

void turnRight()
{
    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
}