File content as of revision 7:b866e3aae05f:

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

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

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 Side IR
DigitalOut eRS(PC_12);
AnalogIn rRS(PA_4);

//Right Front IR
DigitalOut eRF(PC_15);
AnalogIn rRF(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.6;
volatile float gyro_accumulator = 0;
volatile float gyro_decayFactor = 1;

volatile float set_speed = .75;
volatile float left_speed = .75;
volatile float right_speed = .75;

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

const float gyro_propo = 6.5;
const float gyro_integ = 0;
const float gyro_deriv = 10;

const float enco_propo = 6;
const float enco_integ = 10;//1;
const float enco_deriv = 1000;//.0002;

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

const float base_RS = 0.2;
const float base_LS = 0.1;
const float base_RF = 0.002;
const float base_LF = 0.15;

const float thresh_LF = 0.25;
const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE. 
//
//const float

const float frontWall = 0.07; //need to calibrate this threshold to a value where mouse can stop in time
const float open_left = 0.20;
const float open_right = 0.17;
//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;

////////////////////////////////////ir PID constants////////////////////////////////////////////////
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 leftD;
volatile float rightD;

void irPID()       
{    
 eLS = 1;
 leftD = rLS.read();
 eLS = 0;
 eRS = 1;  
 rightD = rRS.read();
 eRS = 0;
 if(leftD > leftWall && rightD > rightWall)//walls on both sides
    {  
        irError = rightD-leftD-irOffset; 
        
        irErrorD = irError-oldirError;
        
        irErrorI += irError;
    }        
    else if(leftD > leftWall) //just left wall
    {
    
        irError = 2*(lirOffset-leftD);
        
        irErrorD=irError-oldirError;
        
        irErrorI += irError;
    }
    else if(rightD > rightWall)//just right wall
    {
        irError=2*(rightD-rirOffset); 
        
        irError = irError-oldirError;
    
        irErrorI += irError;
    }
    else if(leftD < leftWall && rightD < 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;  
}

// 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,
    FINISHED, 
    UNKNOWN
} STATE;
volatile STATE mouse_state;

volatile int wall_in_front = 0;
volatile int opening_left_ahead = 0;
volatile int opening_right_ahead = 0;
volatile int opening_left;
volatile int opening_right;

void watchOut();
void offsetCalc();
void stop();

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

void detect_opening() {
    eRS = 1;
    float right_side = rRS.read();
    eRS = 0;
    eLS  = 1;
    float left_side = rLS.read();
    eLS = 0;
    if (right_side < open_right) {
        mouse_state = STOPPED;
        opening_right_ahead = 1; //handler for this condition sets back to 0
    } else if (left_side < open_left) {
        mouse_state = STOPPED;
        opening_left_ahead = 1; //handler fot this sets back to 0 like above
    }
} 

void systick()
{
    //watchOut();
    if ( mouse_state == STOPPED )
    {
        offsetCalc();
        stop();
        encoder_reset(); // maybe problems with this, reconsider
        return;
    } else if (mouse_state == FORWARD) {
        watchOut();
        //detect_opening();
        
        enco_error = l_enco.getPulses() - r_enco.getPulses();
        gyro_error = _gyro.read() - gyro_offset;
        
        enco_accumulator += enco_error;
        gyro_accumulator += gyro_error;
        
        enco_pid = 0;
        enco_pid += enco_propo * enco_error;
        enco_pid += enco_integ * enco_accumulator;
        enco_pid += enco_deriv * (enco_error - enco_prevError);
        
        gyro_pid = 0;
        gyro_pid += gyro_propo * gyro_error;
        gyro_pid += gyro_integ * gyro_accumulator;
        gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
        
        w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
        left_speed = set_speed + w_error;
        right_speed = set_speed - w_error;
        
        left_forward = left_speed / left_max_speed;
        left_reverse = 0;
        right_forward = right_speed / right_max_speed;
        right_reverse = 0;
        
        enco_prevError = enco_error;
        gyro_prevError = gyro_error;
        
        enco_accumulator += enco_error;
        gyro_accumulator += gyro_error;
        
        enco_accumulator /= enco_decayFactor;
        gyro_accumulator /= gyro_decayFactor;
        
        //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;
}

typedef enum 
{
    LEFT, 
    RIGHT
} TURN_DIRECTION;
volatile TURN_DIRECTION turn_direction;

volatile int turn_counter;
volatile int right_turn_count;
volatile int examined_left;
volatile int examined_right;

//time to make a 90 degree turn and move forward one cell length, repectively
//should be replaced with encoder counts
volatile int max_turn_count = 800;
volatile int cell_length_count = 800;
    
void watchOut()
{
    //if (mouse_state == FORWARD) {
        eRF = 1;
        float right = rRF.read();
        eRF = 0;
        eLF = 1;
        float left = rLF.read();
        eLF = 0;
        if(left > thresh_LF || right > thresh_RF )
        {
            mouse_state = STOPPED;
            wall_in_front = 1; //handler for this condition sets back to zero
            
            encoder_reset();
            mouse_state = TURNING;
            turn_direction = LEFT;
        } 
}


void turn() {
    if (mouse_state == TURNING) {
        if (turn_counter < max_turn_count) {
            int left_pulses = l_enco.getPulses();
            int right_pulses = r_enco.getPulses();
            turn_counter = (left_pulses + right_pulses)/2;

            //maybe eventually replace this with PID control -> systicker
            if (turn_direction == LEFT) {
                left_forward = 0;
                left_reverse = set_speed / left_max_speed;
                right_forward = set_speed / right_max_speed;
                right_reverse = 0;
            } else {
                left_forward = set_speed / left_max_speed;
                left_reverse = 0;
                right_forward = 0;
                right_reverse = set_speed / right_max_speed;
            }   
        } else {
            
            stop();
            turn_counter = 0;
            encoder_reset();
            mouse_state = FORWARD;
        }
    }
}

volatile int forward_counter;

void move_cell() {
    mouse_state = FORWARD;
    forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
    if (forward_counter > 2000) {
        action_ticker.detach();
        forward_counter = 0;
        //encoder_reset();
        mouse_state = STOPPED;
        if (opening_left_ahead) { //takes care of case where opening on both sides - defaults to left turn
            opening_left_ahead = 0;
            opening_left = 1;
        } else if (opening_right_ahead) {
            opening_right_ahead = 0;
            opening_right = 1;
        } else {
            //we fucked up
        }
    } 
}
    
    
void algorithm() {
    if (mouse_state == STOPPED) {
        encoder_reset();
        mouse_state = TURNING;
        turn_direction = LEFT;
        //action_ticker.attach_us(&turn, 1000);
        
        /*if (opening_left_ahead || opening_right_ahead) {
            action_ticker.attach_us(&move_cell, 1000);    
        }
        if (opening_left) {
            mouse_state = TURNING;
            turn_direction = LEFT;
            action_ticker.attach_us(&turn, 1000);
        }
        if (opening_right) {
            mouse_state = TURNING;
            turn_direction = RIGHT;
            action_ticker.attach_us(&turn, 1000);
        }*/
    }
}


void setup()
{
    pc.printf("Hello World\r\n");
    
    eRS = 0;
    eRF = 0;
    eLS = 0;
    eLF = 0;
    
    

    mouse_state = FORWARD;
    
    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);
    
    //encoder_reset();
    //mouse_state = TURNING;
    //turn_direction = LEFT;
    action_ticker.attach_us(&turn, 1000);
    
    //ir_ticker.attach_us(&irPID, 100000);
    
    //algorithm_ticker.attach_us(&algorithm, 1000);

    

        
}
    

int main() {
    setup();
    
   
    while(1) {
//        
//        pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses());
//        //wait(1);
        //wait(1);
        /*eRS = 1;
        wait_us(50);
        float right_side = rRS.read();
        eRS = 0;
        eLS = 1;
        wait_us(50);
        float left_side = rLS.read();
        eLS = 0;
        eRF = 1;
        wait_us(50);
        float right_front = rRF.read();
        eRF = 0;
        eLF = 1;
        wait_us(50);
        float left_front = rLF.read();
        eLF = 0;
        pc.printf("right side: %f, left side: %f right_front: %f, left_front: %f \r\n", right_side, left_side, right_front, left_front);
        wait(1);*/
    }
  //  pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses());    //encoder testing 
        //wait(1);
}