File content as of revision 12:0849b16c2672:

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

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

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;
volatile float gyro_accumulator = 0;
volatile float gyro_decayFactor = 1.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 = 0;//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.3;
const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE. 

const float open_left = 0.22;     //calibrate these two
const float open_right = 0.18;
const float wall_left = 0;
const float wall_right = 0;

volatile float lf_val;
volatile float rf_val;
volatile float ls_val;
volatile float rs_val;

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

typedef enum 
{
    LEFT, 
    RIGHT, 
    INVALID
} 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
const int lmax_turn_count = 620; //I think we need different constants for left/right
const int rmax_turn_count = 0;
const int cell_length_count = 875;
const float gyro_90 = 0.595;
    
volatile int forward_counter = 0;

// 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,
    MOVECELL,
    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 = 0;
volatile int opening_right = 0;

void Scan();
void offsetCalc();
void stop();

void encoder_reset()
{
    l_enco.reset();
    r_enco.reset();
    
    enco_accumulator = 0;
    gyro_accumulator = 0;
}

//combining detect opening and watch out
bool forward_wall_detected()
{
    return lf_val > thresh_LF;
}

bool side_wall_detected()
{
    if (rs_val < open_right) 
    {
        mouse_state = MOVECELL;
        turn_direction = RIGHT;
        return true;
    } 
    else if (ls_val < open_left) 
    {
        mouse_state = MOVECELL;
        turn_direction = LEFT;
        return true;
    }
        
    return false;
}

// updates PID values.
void scan()
{
    eRS = 1;
    wait_us(50);
    rs_val = rRS.read();
    eRS = 0;
    
    eLS  = 1;
    wait_us(50);
    ls_val = rLS.read();
    eLS = 0;
    
    eLF = 1;
    wait_us(50);
    lf_val = rLF.read();
}


void systick()
{
    scan(); // get the IR values.
            // these values are useful regardless of what mouse_state we are in.
    switch (mouse_state)
    {
        case STOPPED: 
            offsetCalc();
            encoder_reset();
            break;
        case FORWARD:
            if ( forward_wall_detected() )
            {
                mouse_state = STOPPED;
                turn_direction = LEFT;
                return;
            }
            
//            if ( side_wall_detected() )
//            {
//                return;
//            }
            
            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;
        
            enco_prevError = enco_error;
            gyro_prevError = gyro_error;
        
            enco_accumulator += enco_error;
            gyro_accumulator += gyro_error;
        
            enco_accumulator /= enco_decayFactor;
            gyro_accumulator /= gyro_decayFactor;
            break;
        default: 
            // don't do anything. 
            break;
    }
}

// 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 = 0;
    left_reverse = 0;
    right_forward = 0;
    right_reverse = 0;
}

void turn() {
    // reopen for business. 
    
    float angle_change = 0;
    
    switch(turn_direction)
    {
    case LEFT:
        while (angle_change < gyro_90)
        {
            left_forward = 0;
            right_forward = set_speed / right_max_speed;
            left_reverse = set_speed / left_max_speed;
            right_reverse = 0;
            
            pc.printf("gyro read: %f\r\n", _gyro.read());
            angle_change += _gyro.read() - gyro_offset;
        }
        break;
    case RIGHT:
        while (angle_change < gyro_90)
        {
            left_forward = set_speed / left_max_speed;
            right_forward = 0;
            left_reverse = 0;
            right_forward = set_speed / right_max_speed;
            
            pc.printf("gyro read: %f\r\n", _gyro.read());
            angle_change += gyro_offset - _gyro.read();
        }
        break;
    default:
        // error state.
        break;
    }
    
//    while ( turn_counter < lmax_turn_count )
//    {
//        left_pulses = l_enco.getPulses();
//        right_pulses = r_enco.getPulses();
//        
//        turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2;
//        
//        switch(turn_direction)
//        {
//            case LEFT: 
//                left_forward = 0;
//                right_forward = set_speed / right_max_speed;
//                left_reverse = set_speed / left_max_speed;
//                right_reverse = 0;
//                break;
//            case RIGHT:
//                left_forward = set_speed / left_max_speed;
//                right_forward = 0;
//                left_reverse = 0;
//                right_reverse = set_speed / right_max_speed;
//                break;
//            default: 
//                // invalid state. did not set a turn direction.
//                mouse_state = STOPPED;
//                break;
//        }
//        
//    }
}

// TODO move_cell is not complete. this is copied code from systick.
void move_cell() {
    //bacl in business boys.
    if (mouse_state == MOVECELL) 
    {
        forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
        if (forward_counter > 2000) 
        {
            forward_counter = 0;
            mouse_state = TURNING;
                    if (opening_left_ahead) 
                    { 
                    opening_left_ahead = 0;
                    turn_direction = LEFT;
                    } 
                    else if (opening_right_ahead) 
                    {
                    opening_right_ahead = 0;
                    } 
            encoder_reset();   //right? prepare for turn
        }
    }
}
    
void setup()
{
    eRS = 0;
    eRF = 0;
    eLS = 0;
    eLF = 0;
    
    mouse_state = FORWARD;
    turn_direction = INVALID;
    myled = 1;
    for ( int i = 0; i < 1000; i++ )
    {
        offsetCalc();
    }
    wait(2);
    
    Systicker.attach_us(&systick, 1000);
}

int main() {
    setup();
    while(1) {
        switch (mouse_state)
        {
        case STOPPED:
            stop();
            wait(1);
            if ( turn_direction == INVALID )
            {
                mouse_state = FORWARD;
            }
            else
            {
                mouse_state = TURNING;
            }
            break;
        case FORWARD:
            left_forward = left_speed / left_max_speed;
            left_reverse = 0;
            right_forward = right_speed / right_max_speed;
            right_reverse = 0;
            break;
        case TURNING:
            turn();
            mouse_state = STOPPED;
            turn_direction = INVALID;
            break;
        case MOVECELL:
            move_cell();
            mouse_state = STOPPED;
            break;
        default: 
            // idk lmao. show some error message?
            break;
        }
    }
}


void test_IR_sensors()
{
    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);
}

void test_motors()
{
    encoder_reset();
    
    right_forward = 1;
    left_forward = 1;
    right_reverse = 0;
    left_reverse = 0;
    
    wait(10);
    pc.printf("left pulses: %d, right pulses: %d", l_enco.getPulses(), r_enco.getPulses());
}