File content as of revision 1:98efd8dd9077:

#include "mbed.h"
#include "AVEncoder.h"

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

PwmOut right_forward(PB_10);
PwmOut right_reverse(PA_6);
PwmOut left_forward(PA_7);
PwmOut left_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);

volatile float gyro_offset = 0;

volatile float line_prevError = 0;
volatile float 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.5;
volatile float right_speed = 0.5;

const float left_max_speed = 6; // max speed is 6 encoder pulses per ms.
const float right_max_speed = 6.2; 

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

const float enco_propo = 0.05;
const float enco_integ = 0;
const float enco_deriv = 5;

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

const float frontWall = 0.7; //need to calibrate this threshold to a value where mouse can stop in time
//something like this may be useful 

volatile float enco_error;
volatile float enco_pid;
volatile float gyro_error;
volatile float gyro_pid;
volatile float w_error;

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

void systick()
{
    watchOut();
    
    enco_error = l_enco.getPulses() - r_enco.getPulses();
    gyro_error = _gyro.read() - gyro_offset;
    
    enco_pid = 0;
    //enco_pid += enco_propo * enco_error;
    //enco_pid += enco_deriv * (enco_error - enco_prevError);
    
    gyro_pid = 0;
    gyro_pid += gyro_propo * gyro_error;
    gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
    
    w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
    left_speed += w_error;
    right_speed -= w_error;
    
    left_forward = left_speed / left_max_speed;
    left_reverse = 0;
    right_forward = right_speed / right_max_speed;
    right_reverse = 0;
    
    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;
}


void stop()
{
    left_forward = 1;
    left_reverse = 1;
    right_forward = 1;
    right_reverse = 1;
    }
    
void watchOut()
{
    eRF = 1;
    eLF = 1;
    if(rRF > frontWall || rLF > frontWall)
    {
        eRF = 0;
        eLF = 0;
        stop();
        return;
    }
}


void setup()
{
    pc.printf("Hello World\r\n");
    
    eRS = 0;
    eRF = 0;
    eLS = 0;
    eLF = 0;
    
    myled = 1;
    
    for ( int i = 0; i < 1000; i++ )
    {
        offsetCalc();
    }
    
    //left_forward = left_speed / left_max_speed;
//    left_reverse = 0;
//    right_forward = right_speed / right_max_speed;
//    right_reverse = 0;
    
    wait(2);
    
    // repeat this for some time frame. 
    Systicker.attach_us(&systick, 1000);
}
    

int main() {
    setup();
    while(1) {
        
        pc.printf("enco_error: %f, gyro_error: %f, w_error: %f\r\n", enco_error, gyro_error, w_error);
        //wait(1);
    }
}