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
mouse.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
jimmery | 13:5f08195456a4 | 1 | // CONTAINS ALL MOUSE MOTOR MOVEMENTS. AND LOGIC RELATING TO MOVEMENT. |
jimmery | 13:5f08195456a4 | 2 | |
jimmery | 13:5f08195456a4 | 3 | #include "mouse.h" |
jimmery | 13:5f08195456a4 | 4 | #include "pinouts.h" |
jimmery | 13:5f08195456a4 | 5 | #include "sensors.h" |
jimmery | 13:5f08195456a4 | 6 | |
jimmery | 13:5f08195456a4 | 7 | Mouse::Mouse() |
jimmery | 13:5f08195456a4 | 8 | { |
jimmery | 13:5f08195456a4 | 9 | sensor_init(); |
jimmery | 13:5f08195456a4 | 10 | pid_reset(); |
jimmery | 13:5f08195456a4 | 11 | |
jimmery | 13:5f08195456a4 | 12 | mouse_state = STOPPED; |
jimmery | 13:5f08195456a4 | 13 | turn_direction = INVALID; |
jimmery | 13:5f08195456a4 | 14 | // TODO complete. |
jimmery | 13:5f08195456a4 | 15 | } |
jimmery | 13:5f08195456a4 | 16 | |
jimmery | 13:5f08195456a4 | 17 | void Mouse::stop() |
jimmery | 13:5f08195456a4 | 18 | { |
jimmery | 13:5f08195456a4 | 19 | left_forward = 0; |
jimmery | 13:5f08195456a4 | 20 | left_reverse = 0; |
jimmery | 13:5f08195456a4 | 21 | right_forward = 0; |
jimmery | 13:5f08195456a4 | 22 | right_reverse = 0; |
jimmery | 13:5f08195456a4 | 23 | } |
jimmery | 13:5f08195456a4 | 24 | |
jimmery | 13:5f08195456a4 | 25 | // TODO: consider a logic change. whether it's curve turns or whatever. |
jimmery | 13:5f08195456a4 | 26 | // or just a change from x_speed to w_speed or something. but we should maybe try to |
jimmery | 13:5f08195456a4 | 27 | // separate the x_speed from the w_speed. |
jimmery | 13:5f08195456a4 | 28 | |
jimmery | 13:5f08195456a4 | 29 | // TODO: consider having separate conditions for turning around as well. added in TURN_DIRECTION. |
jimmery | 13:5f08195456a4 | 30 | void Mouse::turn(TURN_DIRECTION dir, int n_times) |
jimmery | 13:5f08195456a4 | 31 | { |
jimmery | 13:5f08195456a4 | 32 | float angle_change = 0; |
jimmery | 13:5f08195456a4 | 33 | |
jimmery | 13:5f08195456a4 | 34 | switch(dir) |
jimmery | 13:5f08195456a4 | 35 | { |
jimmery | 13:5f08195456a4 | 36 | case LEFT: |
jimmery | 13:5f08195456a4 | 37 | while (angle_change < gyro_90) |
jimmery | 13:5f08195456a4 | 38 | { |
jimmery | 13:5f08195456a4 | 39 | left_forward = 0; |
jimmery | 13:5f08195456a4 | 40 | right_forward = set_x_speed / right_max_speed; |
jimmery | 13:5f08195456a4 | 41 | left_reverse = set_x_speed / left_max_speed; |
jimmery | 13:5f08195456a4 | 42 | right_reverse = 0; |
jimmery | 13:5f08195456a4 | 43 | |
jimmery | 13:5f08195456a4 | 44 | pc.printf("gyro read: %f\r\n", _gyro.read()); |
jimmery | 13:5f08195456a4 | 45 | angle_change += _gyro.read() - gyro_offset; |
jimmery | 13:5f08195456a4 | 46 | } |
jimmery | 13:5f08195456a4 | 47 | break; |
jimmery | 13:5f08195456a4 | 48 | case RIGHT: |
jimmery | 13:5f08195456a4 | 49 | while (angle_change < gyro_90) |
jimmery | 13:5f08195456a4 | 50 | { |
jimmery | 13:5f08195456a4 | 51 | left_forward = set_x_speed / left_max_speed; |
jimmery | 13:5f08195456a4 | 52 | right_forward = 0; |
jimmery | 13:5f08195456a4 | 53 | left_reverse = 0; |
jimmery | 13:5f08195456a4 | 54 | right_forward = set_x_speed / right_max_speed; |
jimmery | 13:5f08195456a4 | 55 | |
jimmery | 13:5f08195456a4 | 56 | pc.printf("gyro read: %f\r\n", _gyro.read()); |
jimmery | 13:5f08195456a4 | 57 | angle_change += gyro_offset - _gyro.read(); |
jimmery | 13:5f08195456a4 | 58 | } |
jimmery | 13:5f08195456a4 | 59 | break; |
jimmery | 13:5f08195456a4 | 60 | default: |
jimmery | 13:5f08195456a4 | 61 | // error state. |
jimmery | 13:5f08195456a4 | 62 | break; |
jimmery | 13:5f08195456a4 | 63 | } |
jimmery | 13:5f08195456a4 | 64 | } |
jimmery | 13:5f08195456a4 | 65 | |
jimmery | 13:5f08195456a4 | 66 | // TODO move_cell is not complete. this is copied code from systick. |
jimmery | 13:5f08195456a4 | 67 | void Mouse::move_cell() { |
jimmery | 13:5f08195456a4 | 68 | //bacl in business boys. |
jimmery | 13:5f08195456a4 | 69 | // if (mouse_state == MOVECELL) |
jimmery | 13:5f08195456a4 | 70 | // { |
jimmery | 13:5f08195456a4 | 71 | // forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses()); |
jimmery | 13:5f08195456a4 | 72 | // if (forward_counter > 2000) |
jimmery | 13:5f08195456a4 | 73 | // { |
jimmery | 13:5f08195456a4 | 74 | // forward_counter = 0; |
jimmery | 13:5f08195456a4 | 75 | // mouse_state = TURNING; |
jimmery | 13:5f08195456a4 | 76 | // if (opening_left_ahead) |
jimmery | 13:5f08195456a4 | 77 | // { |
jimmery | 13:5f08195456a4 | 78 | // opening_left_ahead = 0; |
jimmery | 13:5f08195456a4 | 79 | // turn_direction = LEFT; |
jimmery | 13:5f08195456a4 | 80 | // } |
jimmery | 13:5f08195456a4 | 81 | // else if (opening_right_ahead) |
jimmery | 13:5f08195456a4 | 82 | // { |
jimmery | 13:5f08195456a4 | 83 | // opening_right_ahead = 0; |
jimmery | 13:5f08195456a4 | 84 | // } |
jimmery | 13:5f08195456a4 | 85 | // sensor_reset(); //right? prepare for turn |
jimmery | 13:5f08195456a4 | 86 | // } |
jimmery | 13:5f08195456a4 | 87 | // } |
jimmery | 13:5f08195456a4 | 88 | } |