2015-2016_Mouserat
PID Test
Dependencies: AVEncoder mbed-src-AV
Diff: main.cpp
- Revision:
- 6:61b503990cd6
- Parent:
- 5:f704940c9c7e
- Child:
- 7:b866e3aae05f
--- a/main.cpp Wed Dec 02 01:53:48 2015 +0000
+++ b/main.cpp Thu Dec 03 01:26:44 2015 +0000
@@ -5,6 +5,8 @@
Serial pc(SERIAL_TX, SERIAL_RX);
Ticker Systicker;
Timer timer;
+Ticker action_ticker;
+Ticker algorithm_ticker;
PwmOut right_forward(PB_10);
PwmOut right_reverse(PA_6);
@@ -62,14 +64,16 @@
const float gyro_integ = 0;
const float gyro_deriv = 10;
-const float enco_propo = 10;
-const float enco_integ = 15;//1;
-const float enco_deriv = 2000;//.0002;
+const float enco_propo = 6;
+const float enco_integ = 10;//1;
+const float enco_deriv =800;//.0002;
const float spin_enco_weight = 1;
const float spin_gyro_weight = 1 - spin_enco_weight;
-const float frontWall = 0.2; //need to calibrate this threshold to a value where mouse can stop in time
+const float frontWall = 0.07; //need to calibrate this threshold to a value where mouse can stop in time
+const float open_left = 0.20;
+const float open_right = 0.17;
//something like this may be useful
volatile float enco_error;
@@ -155,6 +159,12 @@
} STATE;
volatile STATE mouse_state;
+volatile int wall_in_front = 0;
+volatile int opening_left_ahead = 0;
+volatile int opening_right_ahead = 0;
+volatile int opening_left;
+volatile int opening_right;
+
void watchOut();
void offsetCalc();
void stop();
@@ -165,6 +175,22 @@
r_enco.reset();
}
+void detect_opening() {
+ eRS = 1;
+ float right_side = rRS.read();
+ eRS = 0;
+ eLS = 1;
+ float left_side = rLS.read();
+ eLS = 0;
+ if (right_side < open_right) {
+ mouse_state = STOPPED;
+ opening_right_ahead = 1; //handler for this condition sets back to 0
+ } else if (left_side < open_left) {
+ mouse_state = STOPPED;
+ opening_left_ahead = 1; //handler fot this sets back to 0 like above
+ }
+}
+
void systick()
{
//watchOut();
@@ -172,9 +198,11 @@
{
offsetCalc();
stop();
+ encoder_reset(); // maybe problems with this, reconsider
return;
} else if (mouse_state == FORWARD) {
watchOut();
+ //detect_opening();
enco_error = l_enco.getPulses() - r_enco.getPulses();
gyro_error = _gyro.read() - gyro_offset;
@@ -235,6 +263,23 @@
right_forward = 1;
right_reverse = 1;
}
+
+typedef enum
+{
+ LEFT,
+ RIGHT
+} TURN_DIRECTION;
+volatile TURN_DIRECTION turn_direction;
+
+volatile int turn_counter;
+volatile int right_turn_count;
+volatile int examined_left;
+volatile int examined_right;
+
+//time to make a 90 degree turn and move forward one cell length, repectively
+//should be replaced with encoder counts
+volatile int max_turn_count = 800;
+volatile int cell_length_count = 800;
void watchOut()
{
@@ -248,92 +293,90 @@
if(left > frontWall || right > frontWall)
{
mouse_state = STOPPED;
- }
- //}
+ wall_in_front = 1; //handler for this condition sets back to zero
+
+ encoder_reset();
+ mouse_state = TURNING;
+ turn_direction = LEFT;
+ }
}
-typedef enum
-{
- LEFT,
- RIGHT
-} TURN_DIRECTION;
-volatile TURN_DIRECTION turn_direction;
-
-volatile int turn_counter;
-volatile int forward_counter;
-volatile int right_turn_count;
-volatile int examined_left;
-volatile int examined_right;
-
-//time to make a 90 degree turn and move forward one cell length, repectively
-//should be replaced with encoder counts
-volatile int max_turn_count = 100;
-volatile int cell_length_count = 100;
-
void turn() {
if (mouse_state == TURNING) {
if (turn_counter < max_turn_count) {
int left_pulses = l_enco.getPulses();
int right_pulses = r_enco.getPulses();
- if (left_pulses < 0) left_pulses = -left_pulses;
- if (right_pulses < 0) right_pulses = -right_pulses;
turn_counter = (left_pulses + right_pulses)/2;
- } else {
+
+ //maybe eventually replace this with PID control -> systicker
if (turn_direction == LEFT) {
- examined_left = 1;
+ left_forward = 0;
+ left_reverse = set_speed / left_max_speed;
+ right_forward = set_speed / right_max_speed;
+ right_reverse = 0;
} else {
- if (right_turn_count > 1) {
- examined_right = 1;
- } else {
- right_turn_count++;
- mouse_state = STOPPED;
- return;
- }
- }
+ left_forward = set_speed / left_max_speed;
+ left_reverse = 0;
+ right_forward = 0;
+ right_reverse = set_speed / right_max_speed;
+ }
+ } else {
+
+ stop();
+ turn_counter = 0;
encoder_reset();
mouse_state = FORWARD;
}
}
}
+
+volatile int forward_counter;
+
+void move_cell() {
+ mouse_state = FORWARD;
+ forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
+ if (forward_counter > 2000) {
+ action_ticker.detach();
+ forward_counter = 0;
+ //encoder_reset();
+ mouse_state = STOPPED;
+ if (opening_left_ahead) { //takes care of case where opening on both sides - defaults to left turn
+ opening_left_ahead = 0;
+ opening_left = 1;
+ } else if (opening_right_ahead) {
+ opening_right_ahead = 0;
+ opening_right = 1;
+ } else {
+ //we fucked up
+ }
+ }
+}
-void find_opening() {
+
+void algorithm() {
if (mouse_state == STOPPED) {
encoder_reset();
- if (!examined_left && !examined_right) {
- turn_direction = LEFT;
- left_forward = 0;
- left_reverse = set_speed / left_max_speed;
- right_forward = set_speed / right_max_speed;
- right_reverse = 0;
- mouse_state = TURNING;
- } else if (examined_left && !examined_right) {
- turn_direction = RIGHT;
- left_forward = set_speed / left_max_speed;
- left_reverse = 0;
- right_forward = 0;
- right_reverse = set_speed / right_max_speed;
+ mouse_state = TURNING;
+ turn_direction = LEFT;
+ //action_ticker.attach_us(&turn, 1000);
+
+ /*if (opening_left_ahead || opening_right_ahead) {
+ action_ticker.attach_us(&move_cell, 1000);
+ }
+ if (opening_left) {
mouse_state = TURNING;
- } else if (!examined_left && examined_right) {
- //this should never happen
- mouse_state = FINISHED;
- } else {
- //we are done
- mouse_state = FINISHED;
+ turn_direction = LEFT;
+ action_ticker.attach_us(&turn, 1000);
}
- } else if (mouse_state == FORWARD) {
- if (forward_counter > cell_length_count) {
- examined_left = 0;
- examined_right = 0;
- right_turn_count = 0;
- forward_counter = 0;
- }
- forward_counter = (l_enco.getPulses() + r_enco.getPulses())/2;
+ if (opening_right) {
+ mouse_state = TURNING;
+ turn_direction = RIGHT;
+ action_ticker.attach_us(&turn, 1000);
+ }*/
}
}
-Ticker turn_ticker;
-Ticker algorithm_ticker;
void setup()
{
@@ -344,6 +387,8 @@
eLS = 0;
eLF = 0;
+
+
mouse_state = FORWARD;
myled = 1;
@@ -361,10 +406,21 @@
wait(2);
// repeat this for some time frame.
+
Systicker.attach_us(&systick, 1000);
+
+ //encoder_reset();
+ //mouse_state = TURNING;
+ //turn_direction = LEFT;
+ action_ticker.attach_us(&turn, 1000);
+
//ir_ticker.attach_us(&irPID, 100000);
- algorithm_ticker.attach_us(&find_opening, 1000);
- turn_ticker.attach_us(&turn, 1000);
+
+ //algorithm_ticker.attach_us(&algorithm, 1000);
+
+
+
+
}
@@ -376,6 +432,13 @@
//
// pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses());
// //wait(1);
+ /*eLS = 1;
+ eRS = 1;
+ float right_side = rRS.read();
+ float left_side = rLS.read();
+ pc.printf("right side IR %f, left side IR %f \r\n", right_side, left_side);
+ eLS = 0;
+ eRS = 0;*/
}
// pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses()); //encoder testing
//wait(1);