A robot rover with distance sensing and audiovisual effects
Dependencies: 4DGL-uLCD-SE Motordriver PID mbed
Fork of PIDRover by
Diff: main.cpp
- Revision:
- 1:c70bc01ebfdd
- Parent:
- 0:be99ed42340d
- Child:
- 2:f4d6c9ba05d0
diff -r be99ed42340d -r c70bc01ebfdd main.cpp --- a/main.cpp Fri Sep 10 13:32:59 2010 +0000 +++ b/main.cpp Wed Mar 16 00:52:47 2016 +0000 @@ -3,25 +3,46 @@ * the PWM signal to H-bridges connected to the motors to attempt to maintain * a constant velocity. */ + /*Sources: mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover + InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn + */ + +#include "mbed.h" +#include "motordriver.h" +#include "PID.h" +//one full revolution=193 counts -#include "Motor.h" -#include "QEI.h" -#include "PID.h" -Motor leftMotor(p21, p20, p19); //pwm, inB, inA -Motor rightMotor(p22, p17, p18); //pwm, inA, inB -QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr -QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr +class Counter { + public: + Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter + _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance + } + + void increment() { + _count++; + } + + int read() { + return _count; + } + +private: + InterruptIn _interrupt; + volatile int _count; +}; + +Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake +Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake +Counter leftPulses(p9), rightPulses (p10); //Tuning parameters calculated from step tests; //see http://mbed.org/cookbook/PID for examples. -PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td +//PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td old +PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td +DigitalOut led(LED1); int main() { - - leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. - rightMotor.period(0.00005); - //Input and output limits have been determined //empirically with the specific motors being used. //Change appropriately for different motors. @@ -29,54 +50,55 @@ //Output units: PwmOut duty cycle as %. //Default limits are for moving forward. leftPid.setInputLimits(0, 3000); - leftPid.setOutputLimits(0.0, 0.9); + leftPid.setOutputLimits(0.0, 0.8); leftPid.setMode(AUTO_MODE); - rightPid.setInputLimits(0, 3200); - rightPid.setOutputLimits(0.0, 0.9); + rightPid.setInputLimits(0, 3000); + rightPid.setOutputLimits(0.0, 0.8); rightPid.setMode(AUTO_MODE); - - int leftPulses = 0; //How far the left wheel has travelled. - int leftPrevPulses = 0; //The previous reading of how far the left wheel + + int leftPrevPulses = 0, leftActPulses=0; //The previous reading of how far the left wheel //has travelled. float leftVelocity = 0.0; //The velocity of the left wheel in pulses per //second. - int rightPulses = 0; //How far the right wheel has travelled. - int rightPrevPulses = 0; //The previous reading of how far the right wheel + int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel //has travelled. float rightVelocity = 0.0; //The velocity of the right wheel in pulses per //second. - int distance = 6000; //Number of pulses to travel. + + int distance = 1000; //Number of pulses to travel. + led=0; - wait(5); //Wait a few seconds before we start moving. + wait(0.5); //Wait a few seconds before we start moving. //Velocity to mantain in pulses per second. leftPid.setSetPoint(1000); - rightPid.setSetPoint(975); + rightPid.setSetPoint(1000); - while ((leftPulses < distance) || (rightPulses < distance)) { + while ((leftActPulses < distance) || (rightActPulses < distance)) { //Get the current pulse count and subtract the previous one to //calculate the current velocity in counts per second. - leftPulses = leftQei.getPulses(); - leftVelocity = (leftPulses - leftPrevPulses) / 0.01; - leftPrevPulses = leftPulses; + leftVelocity = (leftActPulses - leftPrevPulses) / 0.01; + leftPrevPulses = leftActPulses; //Use the absolute value of velocity as the PID controller works //in the % (unsigned) domain and will get confused with -ve values. leftPid.setProcessValue(fabs(leftVelocity)); leftMotor.speed(leftPid.compute()); - rightPulses = rightQei.getPulses(); - rightVelocity = (rightPulses - rightPrevPulses) / 0.01; - rightPrevPulses = rightPulses; + rightVelocity = (rightActPulses - rightPrevPulses) / 0.01; + rightPrevPulses = rightActPulses; rightPid.setProcessValue(fabs(rightVelocity)); rightMotor.speed(rightPid.compute()); - + wait(0.01); - + led=!led; + + leftActPulses=leftPulses.read(); + rightActPulses=rightPulses.read(); } - leftMotor.brake(); - rightMotor.brake(); + leftMotor.stop(0.5); + rightMotor.stop(0.5); }