File content as of revision 13:5f08195456a4:

#include "pid.h"
#include "mouse.h"
#include "pinouts.h"

void Mouse::run_pid()
{
    float w_error = rotational_pid();
    float x_error = translational_pid(); 
    
    left_speed = set_x_speed + w_error + x_error;
    right_speed = set_x_speed - w_error + x_error;
}

float Mouse::rotational_pid()
{
    // this may come into play when we start learning curve turns.
    //set_w += set_w_speed;
    
    float enco_error = l_enco.getPulses() - r_enco.getPulses();
    float gyro_error = _gyro.read() - gyro_offset;
  
    float enco_pid = 0;
    enco_pid += enco_propo * enco_error;
    enco_pid += enco_deriv * (enco_error - enco_prevError);
        
    float gyro_pid = 0;
    gyro_pid += gyro_propo * gyro_error;
    gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
    
    float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
    
    // set previous error. 
    enco_prevError = enco_error;
    gyro_prevError = gyro_error;
    
    // as time goes on, the gyro becomes less and less weighted. 
    spin_gyro_weight /= gyro_falloff;
    spin_enco_weight = 1 - spin_gyro_weight;
    
    return w_error;
}

float Mouse::translational_pid()
{
    set_x += 2 * set_x_speed;
    
    float line_error = l_enco.getPulses() + r_enco.getPulses() - set_x;
    
    float line_pid = 0;
    line_pid += line_propo * line_error;
    line_pid += line_deriv * (line_error - line_prevError);
    
    line_prevError = line_error;
    
    return line_error;
}

void Mouse::pid_reset()
{
    spin_enco_weight = 0.5;
    spin_gyro_weight = 1 - spin_enco_weight;
    
    line_prevError = 0;
    enco_prevError = 0;
    gyro_prevError = 0;
    
    set_w = 0;
    set_x = 0;
    
    // accumulator reset here? if we use integral. 
}