File content as of revision 13:5f08195456a4:

#include "sensors.h"
#include "mbed.h"
#include "mouse.h"
#include "pinouts.h"

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

bool Mouse::forward_wall_detected()
{
    return lf_val > thresh_LF;
}

bool Mouse::side_opening_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;
}

// 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 Mouse::offsetCalc()
{
    gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
}

void Mouse::sensor_reset()
{
    l_enco.reset();
    r_enco.reset();
}

void Mouse::sensor_init()
{
    gyro_offset = 0;
    
    lf_val = 0;
    rf_val = 0;
    ls_val = 0;
    rs_val = 0;
    
    eLF = 0;
    eRF = 0;
    eLS = 0;
    eRS = 0;
    
    sensor_reset();
}