2015-2016_Mouserat / Restructured_Test

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 2015-2016_Mouserat

pid.cpp@13:5f08195456a4, 2015-12-15 (annotated)

Committer:
jimmery
Date:
Tue Dec 15 08:56:36 2015 +0000
Revision:
13:5f08195456a4
HUGE RESTRUCTURING OF THE CODE.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jimmery 13:5f08195456a4 1 #include "pid.h"
jimmery 13:5f08195456a4 2 #include "mouse.h"
jimmery 13:5f08195456a4 3 #include "pinouts.h"
jimmery 13:5f08195456a4 4
jimmery 13:5f08195456a4 5 void Mouse::run_pid()
jimmery 13:5f08195456a4 6 {
jimmery 13:5f08195456a4 7 float w_error = rotational_pid();
jimmery 13:5f08195456a4 8 float x_error = translational_pid();
jimmery 13:5f08195456a4 9
jimmery 13:5f08195456a4 10 left_speed = set_x_speed + w_error + x_error;
jimmery 13:5f08195456a4 11 right_speed = set_x_speed - w_error + x_error;
jimmery 13:5f08195456a4 12 }
jimmery 13:5f08195456a4 13
jimmery 13:5f08195456a4 14 float Mouse::rotational_pid()
jimmery 13:5f08195456a4 15 {
jimmery 13:5f08195456a4 16 // this may come into play when we start learning curve turns.
jimmery 13:5f08195456a4 17 //set_w += set_w_speed;
jimmery 13:5f08195456a4 18
jimmery 13:5f08195456a4 19 float enco_error = l_enco.getPulses() - r_enco.getPulses();
jimmery 13:5f08195456a4 20 float gyro_error = _gyro.read() - gyro_offset;
jimmery 13:5f08195456a4 21
jimmery 13:5f08195456a4 22 float enco_pid = 0;
jimmery 13:5f08195456a4 23 enco_pid += enco_propo * enco_error;
jimmery 13:5f08195456a4 24 enco_pid += enco_deriv * (enco_error - enco_prevError);
jimmery 13:5f08195456a4 25
jimmery 13:5f08195456a4 26 float gyro_pid = 0;
jimmery 13:5f08195456a4 27 gyro_pid += gyro_propo * gyro_error;
jimmery 13:5f08195456a4 28 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
jimmery 13:5f08195456a4 29
jimmery 13:5f08195456a4 30 float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
jimmery 13:5f08195456a4 31
jimmery 13:5f08195456a4 32 // set previous error.
jimmery 13:5f08195456a4 33 enco_prevError = enco_error;
jimmery 13:5f08195456a4 34 gyro_prevError = gyro_error;
jimmery 13:5f08195456a4 35
jimmery 13:5f08195456a4 36 // as time goes on, the gyro becomes less and less weighted.
jimmery 13:5f08195456a4 37 spin_gyro_weight /= gyro_falloff;
jimmery 13:5f08195456a4 38 spin_enco_weight = 1 - spin_gyro_weight;
jimmery 13:5f08195456a4 39
jimmery 13:5f08195456a4 40 return w_error;
jimmery 13:5f08195456a4 41 }
jimmery 13:5f08195456a4 42
jimmery 13:5f08195456a4 43 float Mouse::translational_pid()
jimmery 13:5f08195456a4 44 {
jimmery 13:5f08195456a4 45 set_x += 2 * set_x_speed;
jimmery 13:5f08195456a4 46
jimmery 13:5f08195456a4 47 float line_error = l_enco.getPulses() + r_enco.getPulses() - set_x;
jimmery 13:5f08195456a4 48
jimmery 13:5f08195456a4 49 float line_pid = 0;
jimmery 13:5f08195456a4 50 line_pid += line_propo * line_error;
jimmery 13:5f08195456a4 51 line_pid += line_deriv * (line_error - line_prevError);
jimmery 13:5f08195456a4 52
jimmery 13:5f08195456a4 53 line_prevError = line_error;
jimmery 13:5f08195456a4 54
jimmery 13:5f08195456a4 55 return line_error;
jimmery 13:5f08195456a4 56 }
jimmery 13:5f08195456a4 57
jimmery 13:5f08195456a4 58 void Mouse::pid_reset()
jimmery 13:5f08195456a4 59 {
jimmery 13:5f08195456a4 60 spin_enco_weight = 0.5;
jimmery 13:5f08195456a4 61 spin_gyro_weight = 1 - spin_enco_weight;
jimmery 13:5f08195456a4 62
jimmery 13:5f08195456a4 63 line_prevError = 0;
jimmery 13:5f08195456a4 64 enco_prevError = 0;
jimmery 13:5f08195456a4 65 gyro_prevError = 0;
jimmery 13:5f08195456a4 66
jimmery 13:5f08195456a4 67 set_w = 0;
jimmery 13:5f08195456a4 68 set_x = 0;
jimmery 13:5f08195456a4 69
jimmery 13:5f08195456a4 70 // accumulator reset here? if we use integral.
jimmery 13:5f08195456a4 71 }