A simple robot with 2 motors and 2 encoders. Motor speed is set to 40 and PID controllers are use to attain that speed
Dependencies: PinDetect mbed mRotaryEncoder PID
main.cpp
- Committer:
- harryeakins
- Date:
- 2011-09-20
- Revision:
- 0:e019949c4c69
File content as of revision 0:e019949c4c69:
#include "mbed.h" #include "mRotaryEncoder.h" #include "HBridgeMotor.h" #include "PID.h" // PID constants #define RATE 0.2 #define Kc 1.5 // Proportional #define Ti 1.0 // Integral #define Td 0.0 // Derivative DigitalOut led1(LED1); // Two PID Controllers PID leftPID(Kc, Ti, Td, RATE); PID rightPID(Kc, Ti, Td, RATE); // Two de-bounced encoders mRotaryEncoder left_enc(p19, p20, p29); mRotaryEncoder right_enc(p26, p27, p28); // Two motor/h-bridge objects HBridgeMotor left_motor(p25, p24); HBridgeMotor right_motor(p21, p22); void initializePidControllers(void) { leftPID.setInputLimits(-300, 300); leftPID.setOutputLimits(-1.0, 1.0); leftPID.setMode(AUTO_MODE); rightPID.setInputLimits(-300, 300); rightPID.setOutputLimits(-1.0, 1.0); rightPID.setMode(AUTO_MODE); } int main() { int left_pulses = 0, right_pulses = 0; initializePidControllers(); // 40 encoder ticks per second target speed leftPID.setSetPoint(40.0); rightPID.setSetPoint(40.0); while (true) { // Get velocities float left_velocity = float(left_enc.Get() - left_pulses)/RATE; float right_velocity = float(right_enc.Get() - right_pulses)/RATE; // Pass the PID Controller the latest velocity value leftPID.setProcessValue(left_velocity); rightPID.setProcessValue(right_velocity); // Query the PID Controllers for the desired power outputs // and set the motors to those powers left_motor.set(leftPID.compute()); right_motor.set(rightPID.compute()); // Save the number of encoder ticks for velocity calculation in the next loop left_pulses = left_enc.Get(); right_pulses = right_enc.Get(); // Toggle the LED and wait led1 = 1 - led1; wait(RATE); } }