this code is completely restructured, but should do the same thing. did not want to directly commit, since it may not work at all. compiles though.
Dependencies: AVEncoder mbed-src-AV
Fork of Test by
pid.cpp
- Committer:
- jimmery
- Date:
- 2015-12-15
- Revision:
- 13:5f08195456a4
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. }