hello
Dependencies: AVEncoder QEI mbed-src-AV
Diff: main.cpp
- Revision:
- 5:f9837617817b
- Parent:
- 4:453d534b1c1d
- Child:
- 6:32d9b855b90f
--- a/main.cpp Sat Nov 14 00:15:41 2015 +0000 +++ b/main.cpp Sat Nov 14 01:12:47 2015 +0000 @@ -68,16 +68,16 @@ // (we can change the names later, // i added line in after i realized that i already had the angular code) const float line_propo = 1; -const float line_integ = 1; -const float line_deriv = 1; +const float line_integ = 0; +const float line_deriv = 0; const float gyro_propo = 1; -const float gyro_integ = 1; -const float gyro_deriv = 1; +const float gyro_integ = 0; +const float gyro_deriv = 0; const float enco_propo = 1; -const float enco_integ = 1; -const float enco_deriv = 1; +const float enco_integ = 0; +const float enco_deriv = 0; const float spin_enco_weight = 0.5; const float spin_gyro_weight = 1 - spin_enco_weight; @@ -117,6 +117,7 @@ // we do have to calibrate constants though. void systick() { + pc.printf("systick ran\r\n"); if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) { // do nothing? @@ -125,6 +126,7 @@ offsetCalc(); return; } + pc.printf("systick ran while state is FORWARD \r\n"); int dt = timer.read_us(); // should be around 1 ms. timer.reset(); @@ -159,6 +161,10 @@ float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid; left_speed += x_error + w_error; right_speed += x_error - w_error; + + pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed); + + moveForward(); // offsetCalc(); } if ( mouse_state == TURNING ) @@ -174,11 +180,14 @@ line_accumulator /= line_decayFactor; enco_accumulator /= enco_decayFactor; gyro_accumulator /= gyro_decayFactor; + + reset(); } // setup stuff. void setup() { + pc.printf("Hello World\r\n"); mouse_state = STOPPED; eRS = 0; @@ -189,14 +198,15 @@ myled = 1; // repeat this for some time frame. - offsetCalc(); + for ( int i = 0; i < 200; i++ ) + offsetCalc(); Systicker.attach_us(&systick, 1000); } void reset() { - l_pulses = 0; - r_pulses = 0; + l_enco.reset(); + r_enco.reset(); } @@ -218,12 +228,19 @@ int main() { setup(); - while(1) - { - pc.printf("The left motor is going at speed: %d\r\n", left_speed); - pc.printf("The left motor is going at speed: %d\r\n", right_speed); - wait(1); - } + mouse_state = FORWARD; + + wait(1.5); + stop(); + + pc.printf("DONE\r\n"); + + //while(1) +// { +// pc.printf("The left motor is going at speed: %d\r\n", left_speed); +// pc.printf("The left motor is going at speed: %d\r\n", right_speed); +// wait(1); +// } } @@ -259,10 +276,29 @@ { mouse_state = STOPPED; + motor1_forward = 1.0; motor1_reverse = 1.0; motor2_forward = 1.0; motor2_reverse = 1.0; + + } -void turn(int dir); // maybe split this into two functions? \ No newline at end of file +void turn()// maybe split this into two functions? +{ + mouse_state = TURNING; + float angle = 0; + while (angle < 0.9) + { + float gyro_val = _gyro.read() - gyro_offset; + angle += gyro_val; + + pc.printf("%f\r\n", angle); + + motor1_forward = 0.5; + motor1_reverse = 0; + motor2_forward = 0; + motor2_reverse = 0.5; + } +} \ No newline at end of file