2015-2016_Mouserat
PID Test
Dependencies: AVEncoder mbed-src-AV
Diff: main.cpp
- Revision:
- 9:ad6f60953086
- Parent:
- 8:03e5c3aaa9c9
- Child:
- 10:d2907773f9a3
--- a/main.cpp Fri Dec 04 04:21:55 2015 +0000
+++ b/main.cpp Fri Dec 04 07:23:38 2015 +0000
@@ -49,7 +49,7 @@
volatile float line_accumulator = 0;
volatile float line_decayFactor = 1;
volatile float enco_accumulator = 0;
-volatile float enco_decayFactor = 1.6;
+volatile float enco_decayFactor = 1;
volatile float gyro_accumulator = 0;
volatile float gyro_decayFactor = 1;
@@ -65,7 +65,7 @@
const float gyro_deriv = 10;
const float enco_propo = 6;
-const float enco_integ = 10;//1;
+const float enco_integ = 0;//1;
const float enco_deriv = 1000;//.0002;
const float spin_enco_weight = 1;
@@ -79,7 +79,6 @@
const float thresh_LF = 0.25;
const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE.
-
const float open_left = 0.22; //calibrate these two
const float open_right = 0.18;
const float wall_left = 0;
@@ -95,11 +94,12 @@
typedef enum
{
LEFT,
- RIGHT
+ RIGHT,
+ INVALID
} TURN_DIRECTION;
volatile TURN_DIRECTION turn_direction;
-volatile int turn_counter;
+//volatile int turn_counter;
volatile int right_turn_count;
volatile int examined_left;
volatile int examined_right;
@@ -144,8 +144,8 @@
//combining detect opening and watch out
void Scan()
{
- eRS = 1;
- wait_us(50);
+ eRS = 1;
+ wait_us(50);
float right_side = rRS.read();
eRS = 0;
@@ -158,11 +158,16 @@
// float right_front = rRF.read(); //right front not working so screw it
// eRF = 0;
eLF = 1;
- wait_us(50);
+ wait_us(50);
float left_front = rLF.read();
eLF = 0;
- if (right_side < open_right) {
+ if ( left_front > thresh_LF )
+ {
+ mouse_state = STOPPED;
+ }
+
+ /*if (right_side < open_right) {
mouse_state = MOVECELL;
opening_right_ahead = 1;
} else if (left_side < open_left) {
@@ -178,103 +183,53 @@
encoder_reset();
mouse_state = TURNING;
turn_direction = LEFT;
- }
+ } */
}
void systick()
{
- if ( mouse_state == STOPPED )
+ Scan(); // get the IR values.
+ // these values are useful regardless of what mouse_state we are in.
+ switch (mouse_state)
{
- offsetCalc();
- stop();
- encoder_reset(); // maybe problems with this, reconsider
- return;
- } else if (mouse_state == FORWARD) {
- Scan();
-
-
- enco_error = l_enco.getPulses() - r_enco.getPulses();
- gyro_error = _gyro.read() - gyro_offset;
-
- enco_accumulator += enco_error;
- gyro_accumulator += gyro_error;
-
- enco_pid = 0;
- enco_pid += enco_propo * enco_error;
- enco_pid += enco_integ * enco_accumulator;
- enco_pid += enco_deriv * (enco_error - enco_prevError);
-
- gyro_pid = 0;
- gyro_pid += gyro_propo * gyro_error;
- gyro_pid += gyro_integ * gyro_accumulator;
- gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
-
- w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
- left_speed = set_speed + w_error;
- right_speed = set_speed - w_error;
-
- left_forward = left_speed / left_max_speed;
- left_reverse = 0;
- right_forward = right_speed / right_max_speed;
- right_reverse = 0;
-
- enco_prevError = enco_error;
- gyro_prevError = gyro_error;
-
- enco_accumulator += enco_error;
- gyro_accumulator += gyro_error;
-
- enco_accumulator /= enco_decayFactor;
- gyro_accumulator /= gyro_decayFactor;
+ case STOPPED:
+ offsetCalc();
+ encoder_reset();
+ break;
+ case FORWARD:
+ enco_error = l_enco.getPulses() - r_enco.getPulses();
+ gyro_error = _gyro.read() - gyro_offset;
+
+ enco_accumulator += enco_error;
+ gyro_accumulator += gyro_error;
+
+ enco_pid = 0;
+ enco_pid += enco_propo * enco_error;
+ enco_pid += enco_integ * enco_accumulator;
+ enco_pid += enco_deriv * (enco_error - enco_prevError);
-
- }
- else if (mouse_state == TURNING) {
- if (turn_counter < max_turn_count) {
- int left_pulses = l_enco.getPulses();
- int right_pulses = r_enco.getPulses();
- turn_counter = (left_pulses + right_pulses)/2;
-
- //maybe eventually replace this with PID control -> systicker
- if (turn_direction == LEFT) {
- left_forward = 0;
- left_reverse = set_speed / left_max_speed;
- right_forward = set_speed / right_max_speed;
- right_reverse = 0;
- } else {
- 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;
-
- }
- }
- else if (mouse_state == MOVECELL)
- {
- forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
- if (forward_counter > 2000)
- {
- forward_counter = 0;
- mouse_state = TURNING;
- if (opening_left_ahead)
- {
- opening_left_ahead = 0;
- turn_direction = LEFT;
- }
- else if (opening_right_ahead)
- {
- opening_right_ahead = 0;
- }
- encoder_reset(); //right? prepare for turn
- }
+ gyro_pid = 0;
+ gyro_pid += gyro_propo * gyro_error;
+ gyro_pid += gyro_integ * gyro_accumulator;
+ gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
+
+ w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
+ left_speed = set_speed + w_error;
+ right_speed = set_speed - w_error;
+
+ enco_prevError = enco_error;
+ gyro_prevError = gyro_error;
+
+ enco_accumulator += enco_error;
+ gyro_accumulator += gyro_error;
+
+ enco_accumulator /= enco_decayFactor;
+ gyro_accumulator /= gyro_decayFactor;
+ break;
+ default:
+ // don't do anything.
+ break;
}
}
@@ -300,25 +255,72 @@
right_reverse = 1;
}
-
-
-
void turn() {
- //closed for business moved to systick
+ // reopen for business.
+
+ int turn_counter = 0;
+ int base_left_pulses = l_enco.getPulses();
+ int base_right_pulses = r_enco.getPulses();
+
+ int left_pulses;
+ int right_pulses;
+
+ while ( turn_counter < max_turn_count )
+ {
+ left_pulses = l_enco.getPulses();
+ right_pulses = r_enco.getPulses();
+
+ turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2;
+
+ switch(turn_direction)
+ {
+ case LEFT:
+ left_forward = 0;
+ right_forward = set_speed / right_max_speed;
+ left_reverse = set_speed / left_max_speed;
+ right_reverse = 0;
+ break;
+ case RIGHT:
+ left_forward = set_speed / left_max_speed;
+ right_forward = 0;
+ left_reverse = 0;
+ right_reverse = set_speed / right_max_speed;
+ break;
+ default:
+ // invalid state. did not set a turn direction.
+ mouse_state = STOPPED;
+ }
+
+ }
}
void move_cell() {
//out of business moved to algorithm
-
+ if (mouse_state == MOVECELL)
+ {
+ forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
+ if (forward_counter > 2000)
+ {
+ forward_counter = 0;
+ mouse_state = TURNING;
+ if (opening_left_ahead)
+ {
+ opening_left_ahead = 0;
+ turn_direction = LEFT;
+ }
+ else if (opening_right_ahead)
+ {
+ opening_right_ahead = 0;
+ }
+ encoder_reset(); //right? prepare for turn
+ }
+ }
}
void algorithm() { //moved to systick
-
-
-
//encoder_reset();
// mouse_state = TURNING;
// turn_direction = LEFT;
@@ -342,12 +344,13 @@
void setup()
{
-eRS = 0;
-eRF = 0;
-eLS = 0;
-eLF = 0;
+ eRS = 0;
+ eRF = 0;
+ eLS = 0;
+ eLF = 0;
mouse_state = FORWARD;
+ turn_direction = INVALID;
myled = 1;
for ( int i = 0; i < 1000; i++ )
{
@@ -359,34 +362,51 @@
//action_ticker.attach_us(&turn, 1000);
//algorithm_ticker.attach_us(&algorithm, 1000); let's try to stick with one ticker
}
-
+
+void test_IR_sensors()
+{
+ eRS = 1;
+ wait_us(50);
+ float right_side = rRS.read();
+ eRS = 0;
+ eLS = 1;
+ wait_us(50);
+ float left_side = rLS.read();
+ eLS = 0;
+ eRF = 1;
+ wait_us(50);
+ float right_front = rRF.read();
+ eRF = 0;
+ eLF = 1;
+ wait_us(50);
+ float left_front = rLF.read();
+ eLF = 0;
+ pc.printf("right side: %f, left side: %f right_front: %f, left_front: %f \r\n", right_side, left_side, right_front, left_front);
+ wait(1);
+}
int main() {
setup();
while(1) {
-//
-// pc.printf("left pulss %f, right pulses %f \r\n", l_enco.getPulses(), r_enco.getPulses());
-// //wait(1);
- //wait(1);
- /*eRS = 1;
- wait_us(50);
- float right_side = rRS.read();
- eRS = 0;
- eLS = 1;
- wait_us(50);
- float left_side = rLS.read();
- eLS = 0;
- eRF = 1;
- wait_us(50);
- float right_front = rRF.read();
- eRF = 0;
- eLF = 1;
- wait_us(50);
- float left_front = rLF.read();
- eLF = 0;
- pc.printf("right side: %f, left side: %f right_front: %f, left_front: %f \r\n", right_side, left_side, right_front, left_front);
- wait(1);*/
+ switch (mouse_state)
+ {
+ case STOPPED:
+ stop();
+ break;
+ case FORWARD:
+ left_forward = left_speed / left_max_speed;
+ left_reverse = 0;
+ right_forward = right_speed / right_max_speed;
+ right_reverse = 0;
+ break;
+ case TURNING:
+ turn();
+ break;
+ case MOVECELL:
+ break;
+ default:
+ // idk lmao
+ break;
+ }
}
- // pc.printf("left pulses %d, right pulses %d \r\n", l_enco.getPulses(), r_enco.getPulses()); //encoder testing
- //wait(1);
}