File content as of revision 1:e71cba217586:

#include "mbed.h"
#include "motordriver.h"
#include "LSM9DS0.h"
#include "IMUfilter.h"

#define RATE  0.1

// Callback to update the PID controller
Ticker pid_updater;
// Set a PID update rate of 10 Hz
const float PID_RATE = 0.1;
// Call back to update the IMU samples
Ticker imu_updater;
Ticker heading_reseter;
// Set a sample rate of 200 Hz for the IMU
const float IMU_RATE = 0.025;
const int NUM_IMU_SAMPLES = 4;
// Count the number of times the IMU is sampled to average every 100 samples.
int imu_count = 0;
Ticker dist_updater;
const float DIST_RATE = 0.1;
const float DIST_LIMIT = 0.5;

const float INITIAL_SPEED_LEFT = 0.7;
const float INITIAL_SPEED_RIGHT = 0.609;

const float TURN_TIME = 0.55;

/*/////// PID control gains
 / These values need to be adjusted in accordance with the individual
 / actuators (motors) either by trial and error or computation. The information
 / here should help: 
*////////
/*
 * Done with Ziegler-Nichols method
 
#define Kp    0.002
#define Ki    0.00002
#define Kd    0.075
 */
/*
 * Done with random method from stack overflow which is more upvoted than Ziegler-Nichols
 
#define Kp 0.192
#define Ki 0.0
#define Kd 0.048
 */

#define Kp 0.0001
#define Ki 0.0
#define Kd 0.00001
/*
 */
// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW

// peripheral objects
DigitalOut f(LED1); // forward
DigitalOut b(LED2); // backward
DigitalOut l(LED3); // left
DigitalOut r(LED4); // right
//See http://mbed.org/cookbook/Motor
//Connections to dual H-brdige driver for the two drive motors
Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
Motor right(p26, p25, p24, 1);
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // imu sensor
Serial pc(USBTX, USBRX); // usb serial for debugging

// function prototypes
void setup();
void initIMU();
float getHeading();
void updatePID();
void updateIMU();
float GetAvgHeading();
void UpdateMotorSpeed(float pid_output);
void updateDist();
void headingReset();
void turnRight();
void turnLeft();
void calibrateAccel();
void turnToAngle(float angle);

// global variables
int count; // counter for IR

float calcHeading; // calculated heading
float heading[1]; // index 0 = goal, index 1 = current
float heading_avg[NUM_IMU_SAMPLES]; // moving average array
float sum;

float previousError; // previous error
float pidError; // error
float steadyError; // steady-state error

float P; // proportional error
float I; // integral error
float D; // derivative error

float kp; // proportional constant
float ki; // integral constant
float kd; // derivative constant

float output; // PID controller output

float dt; // update frequency

float motorSpeed1; // motor speed for left motor
float motorSpeed2; // motor speed for right motor
float dist[2]; // 0 = x, 1 = y
float vel[2]; // 0 = x, 1 = y
float accel[2]; // 0 = x, 1 = y
float cur_dist; // resultant distance



// function implementations
void setup()
{
    // Use the begin() function to initialize the LSM9DS0 library.
    // You can either call it with no parameters (the easy way):
    uint16_t status = imu.begin();

    // Make sure communication is working
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
    pc.printf("Should be 0x49D4\n\n");
    initIMU();
    
    // Initialize motor speeds
    motorSpeed1 = INITIAL_SPEED_LEFT;
    motorSpeed2 = INITIAL_SPEED_RIGHT;
    // Attach the PID Update routine to a callback
    pid_updater.attach(&updatePID, PID_RATE);
    // Attach the IMU update routine to a callback to computer a moving average
    imu_updater.attach(&updateIMU, IMU_RATE);
    dist_updater.attach(&updateDist, DIST_RATE);
    // set motor speeds
    left.speed(motorSpeed1);
    right.speed(motorSpeed2);
}

// obtains the initial heading upon bootup
void initIMU()
{
    imu.readMag();
    imu.readAccel();
    imu.calcBias();
    imu.readMag();
    wait(0.1);
    // initial heading
    float sum = 0;
    for( int i = 0; i < NUM_IMU_SAMPLES; ++i) {
        sum += getHeading();
    }
    float initHeading = sum/NUM_IMU_SAMPLES;
        
    heading[0] = initHeading;
    heading[1] = initHeading;
}

// obtains an updated heading
float getHeading()
{
    imu.readMag(); // reads current value
    calcHeading = imu.calcHeading(); // sets current value

    // pc.printf("Compass Heading (in degrees): ");
    // pc.printf("%2f\r\n",calcHeading);
    // pc.printf("%2f\r\n",heading[0]);

    return calcHeading;
}

// PID update attached to a ticker.
void updatePID()
{
    float current = heading[1];
    float target = heading[0];
    float dt = PID_RATE;
    // pc.printf("current=%0.2f, target=%0.2f, dt=%0.2f\r\n", current, target, dt);
    // calculate difference between actual and goal values
    pidError = target - current;
    if (pidError < -270) pidError += 360;
    if (pidError >  270) pidError -= 360;

    // accumulates error over time
    steadyError += pidError*dt;

    // integrator windup compensation (saturates to actuator's output)
    if (steadyError < -10.0) steadyError = -10.0;
    if (steadyError > 10.0) steadyError = 10.0;

    P = kp*pidError; // proportional error
    I = ki*steadyError; // integral error
    D = kd*(pidError - previousError)/dt;  // derivative error

    // save current error
    previousError = pidError;

    float pidOutput = P + I + D;  // calculate the PID output
    pidOutput /= 300.0;  // scale it down to get it within the range of the actuator - probably needs tuning
    if(pidOutput < -0.1) pidOutput = -0.1;
    if(pidOutput > 0.1) pidOutput = 0.1;

    // pc.printf("P = %f | I = %f | D = %f | Output = %f\r\n",P,I,D,pidOutput);
    UpdateMotorSpeed(pidOutput);
}

void UpdateMotorSpeed(float pid_output) {
    // updates new motor speeds
    motorSpeed1 -= pid_output;
    motorSpeed2 += pid_output;

    // pc.printf("Wheel speed difference = %0.2f\r\n", motorSpeed1-motorSpeed2);
    // set motor speeds
    left.speed(motorSpeed1);
    right.speed(motorSpeed2);
}

float GetAvgHeading() {
    float sum = 0;
    for( int i = 0; i < NUM_IMU_SAMPLES; ++i ) {
        sum += heading_avg[i];
    }
    return sum/NUM_IMU_SAMPLES;
}
        

// Get reach the minimum number of samples and then use a moving average filter
// Keep the count below 2*num_samples to prevent overflow, but keep at a minimum of 1*num_samples
void updateIMU() {
    // Get current index into the ring buffer (heading_avg)
    int moving_idx = imu_count % NUM_IMU_SAMPLES;
    // Add the current heading into the buffer
    heading_avg[moving_idx] = getHeading();
    
    // Keep the IMU count between num_samples & 2*num_samples
    // Get the average heading as long as the minimum number of samples have been reached.
    if( imu_count >= NUM_IMU_SAMPLES && imu_count <= 2*NUM_IMU_SAMPLES ) {
        if( imu_count == 2*NUM_IMU_SAMPLES ) {
            imu_count = NUM_IMU_SAMPLES;
        }
        heading[1] = GetAvgHeading();
        // pc.printf("av_heading=%0.2f\r\n", heading[1]);
    }
    imu_count += 1;
}

// Update acceleration, velocity, and distance vectors.
void updateDist() {
    // Get latest acceleration reading.
    imu.readAccel();
    
    // Get acceleration vector
    accel[0] = imu.ax - imu.abias[0];
    accel[1] = imu.ay - imu.abias[1];
    
    // Get velocity vector
    vel[0] += accel[0]*RATE;
    vel[1] += accel[1]*RATE;
    
    // Get distance vector
    dist[0] += vel[0]*RATE;
    dist[1] += vel[1]*RATE;
    
    // Get resultant distance
    // d = (dx^2 + dy^2)^(1/2)
    cur_dist = sqrt(dist[0]*dist[0] + dist[1]*dist[1]);
    if( cur_dist > DIST_LIMIT ) {
        turnRight();
        // Reset vel and dist to get accurate distance measurement to next move
        vel[0] = 0;
        vel[1] = 0;
        dist[0] = 0;
        dist[1] = 0;
        cur_dist = 0;
        pidError = 0;
        previousError = 0;
    }
    // pc.printf("ax=%0.2f, ay=%0.2f\r\nvx=%0.2f, vy=%0.2f\r\ndx=%0.2f, dy=%0.2f, d=%0.2f\r\n", accel[0], accel[1], vel[0], vel[1], dist[0], dist[1], cur_dist);
}

void headingReset() {
    motorSpeed1 = INITIAL_SPEED_LEFT;
    motorSpeed2 = INITIAL_SPEED_RIGHT;
    
    // set motor speeds
    left.speed(motorSpeed1);
    right.speed(motorSpeed2);

    heading[0] = getHeading();
    heading[1] = getHeading();
}

void turnRight() {
    
    left.stop(0);
    right.stop(0);
    left.speed(-1);
    right.speed(1);
    
    /*
    int turn_ang = heading[0] + 90;
    turn_ang = turn_ang% 360;
    turnToAngle(turn_ang);
    */
    
    wait(TURN_TIME);
    headingReset();
}

void turnToAngle(float angle) {
    bool hasTurned = false;
    left.stop(0);
    right.stop(0);
    left.speed(-1);
    right.speed(1);
    while(!hasTurned) {
        float cur_head = getHeading();
        pc.printf("%0.2f, %0.2f\r\n", cur_head, angle);
        if( cur_head > angle - 20 && cur_head < angle + 20 ) {
            hasTurned = true;
            heading[0] = cur_head;
            left.stop(0);
            right.stop(0);
        }
        
    }
}
void turnLeft() {
    left.stop(0);
    right.stop(0);
    left.speed(1);
    right.speed(-1);
    
    /*
    int turn_ang = heading[0] - 90;
    turn_ang = turn_ang% 360;
    turnToAngle(turn_ang);
    */
    
    wait(TURN_TIME);
    headingReset();
}

int main()
{
    // initialization functions
    setup();

    // variables
    dt = RATE;
    kp = Kp;
    ki = Ki;
    kd = Kd;
    
    /*
    // Attach the PID Update routine to a callback
    pid_updater.attach(&updatePID, PID_RATE);
    // Attach the IMU update routine to a callback to computer a moving average
    imu_updater.attach(&updateIMU, IMU_RATE);
    dist_updater.attach(&updateDist, DIST_RATE);

    */
    // spin in the main loop. Updates will happen asychronously.
    while(1) {
    }
}