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

sensors.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 "sensors.h"
jimmery 13:5f08195456a4 2 #include "mbed.h"
jimmery 13:5f08195456a4 3 #include "mouse.h"
jimmery 13:5f08195456a4 4 #include "pinouts.h"
jimmery 13:5f08195456a4 5
jimmery 13:5f08195456a4 6 // updates PID values.
jimmery 13:5f08195456a4 7 void Mouse::scan()
jimmery 13:5f08195456a4 8 {
jimmery 13:5f08195456a4 9 eRS = 1;
jimmery 13:5f08195456a4 10 wait_us(50);
jimmery 13:5f08195456a4 11 rs_val = rRS.read();
jimmery 13:5f08195456a4 12 eRS = 0;
jimmery 13:5f08195456a4 13
jimmery 13:5f08195456a4 14 eRF = 1;
jimmery 13:5f08195456a4 15 wait_us(50);
jimmery 13:5f08195456a4 16 rf_val = rRF.read();
jimmery 13:5f08195456a4 17
jimmery 13:5f08195456a4 18 eLS = 1;
jimmery 13:5f08195456a4 19 wait_us(50);
jimmery 13:5f08195456a4 20 ls_val = rLS.read();
jimmery 13:5f08195456a4 21 eLS = 0;
jimmery 13:5f08195456a4 22
jimmery 13:5f08195456a4 23 eLF = 1;
jimmery 13:5f08195456a4 24 wait_us(50);
jimmery 13:5f08195456a4 25 lf_val = rLF.read();
jimmery 13:5f08195456a4 26 }
jimmery 13:5f08195456a4 27
jimmery 13:5f08195456a4 28 bool Mouse::forward_wall_detected()
jimmery 13:5f08195456a4 29 {
jimmery 13:5f08195456a4 30 return lf_val > thresh_LF;
jimmery 13:5f08195456a4 31 }
jimmery 13:5f08195456a4 32
jimmery 13:5f08195456a4 33 bool Mouse::side_opening_detected()
jimmery 13:5f08195456a4 34 {
jimmery 13:5f08195456a4 35 if (rs_val < open_right)
jimmery 13:5f08195456a4 36 {
jimmery 13:5f08195456a4 37 mouse_state = MOVECELL;
jimmery 13:5f08195456a4 38 turn_direction = RIGHT;
jimmery 13:5f08195456a4 39 return true;
jimmery 13:5f08195456a4 40 }
jimmery 13:5f08195456a4 41 else if (ls_val < open_left)
jimmery 13:5f08195456a4 42 {
jimmery 13:5f08195456a4 43 mouse_state = MOVECELL;
jimmery 13:5f08195456a4 44 turn_direction = LEFT;
jimmery 13:5f08195456a4 45 return true;
jimmery 13:5f08195456a4 46 }
jimmery 13:5f08195456a4 47
jimmery 13:5f08195456a4 48 return false;
jimmery 13:5f08195456a4 49 }
jimmery 13:5f08195456a4 50
jimmery 13:5f08195456a4 51 // computes gyro_offset
jimmery 13:5f08195456a4 52 // uses a "weighted" average.
jimmery 13:5f08195456a4 53 // idea is that the current gyro offset is weighted more than previous ones.
jimmery 13:5f08195456a4 54 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
jimmery 13:5f08195456a4 55 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
jimmery 13:5f08195456a4 56 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
jimmery 13:5f08195456a4 57 // currently this is only in the setup function. we can run this when the mouse is running in a line
jimmery 13:5f08195456a4 58 // when we figure out good line running pid.
jimmery 13:5f08195456a4 59 void Mouse::offsetCalc()
jimmery 13:5f08195456a4 60 {
jimmery 13:5f08195456a4 61 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
jimmery 13:5f08195456a4 62 }
jimmery 13:5f08195456a4 63
jimmery 13:5f08195456a4 64 void Mouse::sensor_reset()
jimmery 13:5f08195456a4 65 {
jimmery 13:5f08195456a4 66 l_enco.reset();
jimmery 13:5f08195456a4 67 r_enco.reset();
jimmery 13:5f08195456a4 68 }
jimmery 13:5f08195456a4 69
jimmery 13:5f08195456a4 70 void Mouse::sensor_init()
jimmery 13:5f08195456a4 71 {
jimmery 13:5f08195456a4 72 gyro_offset = 0;
jimmery 13:5f08195456a4 73
jimmery 13:5f08195456a4 74 lf_val = 0;
jimmery 13:5f08195456a4 75 rf_val = 0;
jimmery 13:5f08195456a4 76 ls_val = 0;
jimmery 13:5f08195456a4 77 rs_val = 0;
jimmery 13:5f08195456a4 78
jimmery 13:5f08195456a4 79 eLF = 0;
jimmery 13:5f08195456a4 80 eRF = 0;
jimmery 13:5f08195456a4 81 eLS = 0;
jimmery 13:5f08195456a4 82 eRS = 0;
jimmery 13:5f08195456a4 83
jimmery 13:5f08195456a4 84 sensor_reset();
jimmery 13:5f08195456a4 85 }