File content as of revision 9:e2feaadf504c:

//Micromouse code
#include "mbed.h"
#include "AVEncoder.h"
#include "analogin_api.h"

// set things
Serial pc(SERIAL_TX, SERIAL_RX);
Ticker Systicker;
Timer timer;

PwmOut motor1_forward(PB_10);
PwmOut motor1_reverse(PA_6);
PwmOut motor2_forward(PA_7);
PwmOut motor2_reverse(PB_6);

// TODO: change our encoder pins from AnalogIn into:
// otherwise, we can also use the AVEncoder thing as well.  
AVEncoder l_enco(PA_15, PB_3);
AVEncoder r_enco(PA_1, PA_10);

// gyro
AnalogIn _gyro(PA_0);
// AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. 

//Left Front IR
DigitalOut eLF(PC_3);
AnalogIn rLF(PC_0);
                                                    //PC_4 is an ADC
//Left Side IR
DigitalOut eLS(PC_2);
AnalogIn rLS(PC_1);

//Right Front IR
DigitalOut eRF(PC_12);
AnalogIn rRF(PA_4);

//Right Side IR
DigitalOut eRS(PC_15);
AnalogIn rRS(PB_0);

DigitalOut myled(LED1);

// global variables. 
volatile float gyro_offset = 0;

volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. 
volatile float angl_speed = 0; // should not turn while moving forward.

volatile float line_prevError = 0;
volatile int   enco_prevError = 0;
volatile float gyro_prevError = 0;

volatile float line_accumulator = 0;
volatile float line_decayFactor = 1;
volatile float enco_accumulator = 0;
volatile float enco_decayFactor = 1;
volatile float gyro_accumulator = 0;
volatile float gyro_decayFactor = 1;

volatile float left_speed = 0;
volatile float right_speed = 0;

// pid constants. these need to be tuned. but i set them as a default val for now.
// line refers to the translational speed. 
// enco and gyro will be used primarily for angular speed. 
// (we can change the names later, 
// i added line in after i realized that i already had the angular code)
const int   max_speed = 6; // max speed is 6 encoder pulses per ms.

const float line_propo = 0;
const float line_integ = 0;
const float line_deriv = 0;

const float gyro_propo = 1;
const float gyro_integ = 0;
const float gyro_deriv = 0;

const float enco_propo = 1;
const float enco_integ = 0;
const float enco_deriv = 0;

const float spin_enco_weight = 0.5;
const float spin_gyro_weight = 1 - spin_enco_weight;

// this is just so that we can maintain what state our mouse is in. 
// currently this has no real use, but it may in the future. 
// or we could just remove this entirely. 
typedef enum 
{
    STOPPED, 
    FORWARD, 
    TURNING, 
    UNKNOWN
} STATE;
volatile STATE mouse_state;

// helper functions
void reset();
void offsetCalc();
void stop();
void moveForward();
void turn();
void irReset();

//irPID function to not crash into stuff 

const float leftWall= 0;
const float rightWall= 0;   //we need to find the threshold value for when 
//left or right walls are present 
const float irOffset= 0; // difference between right and left ir sensors when
//mouse is in the middle
const float lirOffset= 0; //middle value for when there is only a wall on one 
const float rirOffset= 0; //side of the mouse (left and right)
volatile float irError = 0;
volatile float irErrorD = 0;
volatile float irErrorI = 0;
volatile float oldirError = 0;
volatile float totalirError= 0; //errors
const float irKp = 0;
const float irKd = 0; 
const float irKi = 0; //constants
volatile float leftDistance = 0;
volatile float rightDistance= 0;
void irPID()       
{    
 eLS = 1;
 eRS = 1;  
 if(rLS > leftWall && rRS > rightWall)//walls on both sides
    {  
        //leftDistance = rLS;
//        rightDistance = rRS;
        irError = rRS-rLS-irOffset; //– leftDistance – irOffset;
        
        irErrorD = irError-oldirError;
        //irErrorD -= oldirError;//(irError – oldirError);
        
        irErrorI += irError;
    }        
    else if(rLS > leftWall) //just left wall
    {
        leftDistance = rLS;
        irError = 2*(lirOffset-leftDistance);//(2 * (lirOffset – leftDistance));
        
        irErrorD=irError-oldirError;
        
        irErrorI += irError;
    }
    else if(rRS > rightWall)//just right wall
    {
        rightDistance = rRS;
        irError=2*(rightDistance-rirOffset); 
        irError = irError-oldirError;
    irErrorI += irError;
    }
    else if(rLS < leftWall && rRS < rightWall)//no walls!! Use encoder PID
    {
        irError = 0;
        irErrorD = 0;
        irErrorI += irError; 
    }
    totalirError =  irError*irKp + irErrorD*irKd + irErrorI*irKi;
    oldirError = irError;
    left_speed -= totalirError;
    right_speed += totalirError; 
    eLS = 0;
    eRS = 0;   
}

const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time
//something like this may be useful for the very temporal present
//doesn't account for current speed/trajectory atm
void dontDriveStraightIntoAWall()
{
    eRF = 1;
    eLF = 1;
    if(rRF > frontWall || rLF > frontWall)
    {
        eRF = 0;
        eLF = 0;
        mouse_state = STOPPED;
        stop();
        return;
    }
}

// PID Control
// this contains the simplistic PID control for the most part. 
// we do have to calibrate constants though. 
void systick()
{
    //pc.printf("systick ran\r\n");
    if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
    {
        // do nothing? 
        // reset?
        //reset();
        offsetCalc();
        return;
    }
    //pc.printf("systick ran while state is FORWARD \r\n");
    
    int dt = timer.read_us(); // should be around 1 ms.
    timer.reset();
    
    //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
    
    float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses());
    int enco_error = l_enco.getPulses() - r_enco.getPulses();
    float gyro_error = _gyro.read() - gyro_offset;
    
    line_accumulator += line_error;
    enco_accumulator += enco_error;
    gyro_accumulator += gyro_error;
    
    float line_pid = 0;
    line_pid += line_propo * line_error;
    line_pid += line_integ * line_accumulator;
    line_pid += line_deriv * (line_error - line_prevError)/dt;
    
    float enco_pid = 0;
    enco_pid += enco_propo * enco_error;
    enco_pid += enco_integ * enco_accumulator;
    enco_pid += enco_deriv * (enco_error - enco_prevError)/dt;
    
    float gyro_pid = 0;
    gyro_pid += gyro_propo * gyro_error;
    gyro_pid += gyro_integ * gyro_accumulator;
    gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt;
        
    // forward moving pid control. 
    if ( mouse_state == FORWARD )
    {
        float x_error = line_pid;
        float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
        left_speed += (x_error - w_error);
        right_speed += (x_error + w_error);   //swapped
        
        //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
        
        moveForward();
        // offsetCalc();
    }
    if ( mouse_state == TURNING )
    {
        // nothing for now. if we turn in place, we assume no pid control.
        // this may have to change when we try curve turns.
    }
    
    line_prevError = line_error;
    enco_prevError = enco_error;
    gyro_prevError = gyro_error;
    
    line_accumulator /= line_decayFactor;
    enco_accumulator /= enco_decayFactor;
    gyro_accumulator /= gyro_decayFactor;
    
    reset();
}

// setup stuff. 
void setup()
{
    pc.printf("Hello World\r\n");
    pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
    pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
    mouse_state = STOPPED;
    
    reset(); 
    
    eRS = 0;
    eRF = 0;
    eLS = 0;
    eLF = 0;
    
    myled = 1;
    
    // repeat this for some time frame. 
    for ( int i = 0; i < 200; i++ )
        offsetCalc();
    Systicker.attach_us(&systick, 1000);
}

void reset()
{
    l_enco.reset();
    r_enco.reset();
}


// computes gyro_offset
// uses a "weighted" average. 
// idea is that the current gyro offset is weighted more than previous ones. 
    // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). 
    // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
    // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
// currently this is only in the setup function. we can run this when the mouse is running in a line
// when we figure out good line running pid.
void offsetCalc()
{
    gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
}


    
int main()
{
    setup();
    pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
    
    wait(2);
    
    mouse_state = FORWARD;
    for ( int i = 0; i < 10; i++ )
    {
        pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
        pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
    }
    stop();
    
    //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
    
    //while(1)
//    {
//        pc.printf("The left motor is going at speed: %d\r\n", left_speed);  
//        pc.printf("The left motor is going at speed: %d\r\n", right_speed);
//        wait(1);
//    }
}


// movement functions.
void moveForward()
{
    //mouse_state = FORWARD;
    
    if ( left_speed > 0 ) // which should be always.
    {
        motor1_forward = left_speed / max_speed;
        motor1_reverse = 0;
    }
    else
    {
        motor1_forward = 0;
        motor1_reverse = -left_speed / max_speed;
    }
    
    if ( right_speed > 0 ) // which again should be always.
    {
        motor2_forward = right_speed / max_speed;
        motor2_reverse = 0;
    }
    else
    {
        motor2_forward = 0;
        motor2_reverse = -right_speed / max_speed;
    }
}

void stop()
{
    mouse_state = STOPPED;
    
    
    motor1_forward = 1.0;
    motor1_reverse = 1.0;
    motor2_forward = 1.0;
    motor2_reverse = 1.0;
    
    
}

void turn()// maybe split this into two functions?
{
    mouse_state = TURNING; 
    float angle = 0;
    while (angle < 0.90)
    {
        float gyro_val = _gyro.read() - gyro_offset;
        angle += gyro_val;
        
        pc.printf("%f\r\n", angle);
        
        motor1_forward = 0.5;
        motor1_reverse = 0;
        motor2_forward = 0;
        motor2_reverse = 0.5;
    }    
}
void irReset() //maybe split resets
{
    eRF = 0;
    eRS = 0;
    eLF = 0;
    eRF = 0;
    }