hello
Dependencies: AVEncoder QEI mbed-src-AV
Diff: main.cpp
- Revision:
- 4:453d534b1c1d
- Parent:
- 2:5f9b78950a17
- Child:
- 5:f9837617817b
--- a/main.cpp Sat Nov 14 00:00:15 2015 +0000 +++ b/main.cpp Sat Nov 14 00:15:41 2015 +0000 @@ -1,27 +1,30 @@ +//Micromouse code #include "mbed.h" -#include "QEI.h" +#include "AVEncoder.h" +// set things +Serial pc(SERIAL_TX, SERIAL_RX); +Ticker Systicker; +Timer timer; + +PwmOut motor1_forward(PB_10); PwmOut motor1_reverse(PA_6); -PwmOut motor1_forward(PB_10); PwmOut motor2_forward(PA_7); PwmOut motor2_reverse(PB_6); -//DigitalIn encoder1A(PA_15); -//DigitalIn encoder1B(PB_3); -//DigitalIn encoder2A(PA_1); -//DigitalIn encoder2B(PA_10); +// TODO: change our encoder pins from AnalogIn into: +// otherwise, we can also use the AVEncoder thing as well. +AVEncoder l_enco(PA_15, PB_3); +AVEncoder r_enco(PA_1, PA_10); - - -//Gyro +// gyro AnalogIn _gyro(PA_0); -AnalogIn gyro_cal(PC_1); //no +// AnalogIn gyro_cal(PC_1) ?? currently this isn't connected. //Left Front IR DigitalOut eLF(PC_3); AnalogIn rLF(PC_0); //PC_4 is an ADC - //Left Side IR DigitalOut eLS(PC_2); AnalogIn rLS(PC_1); @@ -36,132 +39,230 @@ DigitalOut myled(LED1); -QEI LENC (PA_15, PB_3, NC, 100 ); -QEI RENC (PA_1, PA_10, NC, 100 ); +// global variables. +volatile float gyro_offset = 0; + +volatile float line_speed = 1; // currently is in terms of encoder pulses / ms. +volatile float angl_speed = 0; // should not turn while moving forward. + +volatile float line_prevError = 0; +volatile int enco_prevError = 0; +volatile float gyro_prevError = 0; -int distance = 0; +volatile float line_accumulator = 0; +volatile float line_decayFactor = 1.5; +volatile float enco_accumulator = 0; +volatile float enco_decayFactor = 1.5; +volatile float gyro_accumulator = 0; +volatile float gyro_decayFactor = 1.5; + +volatile float left_speed = 0; +volatile float right_speed = 0; -void rightForward(float speed); -void leftForward(float speed); +volatile unsigned long l_pulses = 0; +volatile unsigned long r_pulses = 0; -float read_gyro( void ); +// pid constants. these need to be tuned. but i set them as a default val for now. +// line refers to the translational speed. +// enco and gyro will be used primarily for angular speed. +// (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; -Serial pc (SERIAL_TX, SERIAL_RX); +const float gyro_propo = 1; +const float gyro_integ = 1; +const float gyro_deriv = 1; + +const float enco_propo = 1; +const float enco_integ = 1; +const float enco_deriv = 1; + +const float spin_enco_weight = 0.5; +const float spin_gyro_weight = 1 - spin_enco_weight; -int main() { - // LENC.reset(); - // RENC.reset(); - eRS = 0; - eRF = 0; - eLS = 0; - eLF = 0; - - myled = 1; - while(1) { - - motor1_reverse = 0; - motor1_forward = 0; - - motor2_forward = 0; - motor2_reverse = 0; - - eRS = 0; - eRF = 0; - eLS = 0; - eLF = 0; - - pc.printf("LEDS off\r\n"); - wait(1); - pc.printf("Left Front: \t%f\r\n", rLF.read()); - pc.printf("Left Side: \t%f\r\n", rLS.read()); - pc.printf("Right Front: \t%f\r\n", rRS.read()); - pc.printf("Right Side: \t%f\r\n\n", rRF.read()); - wait(.5); - - eRS = 1; - eRF = 1; - eLS = 1; - eLF = 1; - - pc.printf("LEDS on\r\n"); - wait(1); - pc.printf("Left Front: \t%f\r\n", rLF.read()); - pc.printf("Left Side: \t%f\r\n", rLS.read()); - pc.printf("Right Front: \t%f\r\n", rRS.read()); - pc.printf("Right Side: \t%f\r\n\n", rRF.read()); - // pc.printf("%f\r\n", _gyro.read()); -// wait(0.5); - } +// this is just so that we can maintain what state our mouse is in. +// currently this has no real use, but it may in the future. +// or we could just remove this entirely. +typedef enum +{ + STOPPED, + FORWARD, + TURNING, + UNKNOWN +} STATE; +STATE mouse_state; + +// helper functions +void reset(); +void offsetCalc(); +void stop(); +void moveForward(); +void turn(); - - // int in = encoder1A.read(); - // if(in==1) - // myled=1; - // else - // myled=0; +// interrupt stuff. +void incLENC() +{ + l_pulses++; +} + +void incRENC() +{ + r_pulses++; +} - //} - // -// motor1_reverse = 1.0; -// motor1_forward = 1.0; -// -// motor2_forward = 1.0; -// motor2_reverse = 1.0; - //rightForward(0.5); - //leftForward(-0.5); -} - - - float read_gyro ( void ){ - // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset - float r_gyro = _gyro.read(); // Get the Analog value from the STM32 - r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine - r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign - r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise) - return r_gyro; +// PID Control +// this contains the simplistic PID control for the most part. +// we do have to calibrate constants though. +void systick() +{ + if ( mouse_state == STOPPED || mouse_state == UNKNOWN ) + { + // do nothing? + // reset? + reset(); + offsetCalc(); + return; } -void moveForward(float speed) + int dt = timer.read_us(); // should be around 1 ms. + timer.reset(); + + float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses()); + int enco_error = l_enco.getPulses() - r_enco.getPulses(); + float gyro_error = _gyro.read() - gyro_offset; + + line_accumulator += line_error; + enco_accumulator += enco_error; + gyro_accumulator += gyro_error; + + float line_pid = 0; + line_pid += line_propo * line_error; + line_pid += line_integ * line_accumulator; + line_pid += line_deriv * (line_error - line_prevError)/dt; + + float enco_pid = 0; + enco_pid += enco_propo * enco_error; + enco_pid += enco_integ * enco_accumulator; + enco_pid += enco_deriv * (enco_error - enco_prevError)/dt; + + float gyro_pid = 0; + gyro_pid += gyro_propo * gyro_error; + gyro_pid += gyro_integ * gyro_accumulator; + gyro_pid += gyro_deriv * (gyro_error - gyro_prevError)/dt; + + // forward moving pid control. + if ( mouse_state == FORWARD ) + { + float x_error = line_pid; + 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; + // offsetCalc(); + } + if ( mouse_state == TURNING ) + { + // nothing for now. if we turn in place, we assume no pid control. + // this may have to change when we try curve turns. + } + + line_prevError = line_error; + enco_prevError = enco_error; + gyro_prevError = gyro_error; + + line_accumulator /= line_decayFactor; + enco_accumulator /= enco_decayFactor; + gyro_accumulator /= gyro_decayFactor; +} + +// setup stuff. +void setup() { - motor1_forward = speed; - motor2_forward = speed; + mouse_state = STOPPED; + + eRS = 0; + eRF = 0; + eLS = 0; + eLF = 0; + + myled = 1; - motor1_reverse = 0; - motor2_reverse = 0; + // repeat this for some time frame. + offsetCalc(); + Systicker.attach_us(&systick, 1000); +} + +void reset() +{ + l_pulses = 0; + r_pulses = 0; } +// computes gyro_offset +// uses a "weighted" average. +// idea is that the current gyro offset is weighted more than previous ones. + // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). + // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.) + // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight. +// currently this is only in the setup function. we can run this when the mouse is running in a line +// when we figure out good line running pid. +void offsetCalc() +{ + gyro_offset = gyro_offset / 2 + _gyro.read() / 2; +} -void rightForward(float speed) { - if (speed == 0) { - motor1_forward = 1.0; - motor1_reverse = 1.0; - } + - if (speed > 0) { - motor1_forward = speed; - motor1_reverse = 0; - } - - else { - motor1_forward = 0; - motor1_reverse = -speed; +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); } } -void leftForward(float speed) { - if (speed == 0) { - motor2_forward = 1.0; - motor2_reverse = 1.0; + +// movement functions. +void moveForward() +{ + mouse_state = FORWARD; + + if ( left_speed > 0 ) // which should be always. + { + motor1_forward = left_speed; + motor1_reverse = 0; + } + else + { + motor1_forward = 0; + motor1_reverse = -left_speed; } - if (speed > 0) { - motor2_forward = speed; + if ( right_speed > 0 ) // which again should be always. + { + motor2_forward = right_speed; motor2_reverse = 0; } - - else { + else + { motor2_forward = 0; - motor2_reverse = -speed; + motor2_reverse = -right_speed; } -} \ No newline at end of file +} + +void stop() +{ + 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