main.cpp

00001 #include "mbed.h"
00002 
00003 #include "mRotaryEncoder.h"
00004 #include "HBridgeMotor.h"
00005 #include "PID.h"
00006 
00007 // PID constants
00008 #define RATE  0.2
00009 #define Kc   1.5 // Proportional
00010 #define Ti   1.0 // Integral
00011 #define Td    0.0 // Derivative
00012 
00013 DigitalOut led1(LED1);
00014 
00015 // Two PID Controllers
00016 PID leftPID(Kc, Ti, Td, RATE);
00017 PID rightPID(Kc, Ti, Td, RATE);
00018 
00019 // Two de-bounced encoders
00020 mRotaryEncoder left_enc(p19, p20, p29);
00021 mRotaryEncoder right_enc(p26, p27, p28);
00022 
00023 // Two motor/h-bridge objects
00024 HBridgeMotor left_motor(p25, p24);
00025 HBridgeMotor right_motor(p21, p22);
00026 
00027 void initializePidControllers(void) {
00028     leftPID.setInputLimits(-300, 300);
00029     leftPID.setOutputLimits(-1.0, 1.0);
00030     leftPID.setMode(AUTO_MODE);
00031 
00032     rightPID.setInputLimits(-300, 300);
00033     rightPID.setOutputLimits(-1.0, 1.0);
00034     rightPID.setMode(AUTO_MODE);
00035 }
00036 
00037 int main() {
00038     int left_pulses = 0, right_pulses = 0;
00039    
00040     initializePidControllers();
00041     
00042     // 40 encoder ticks per second target speed
00043     leftPID.setSetPoint(40.0);
00044     rightPID.setSetPoint(40.0);
00045     
00046     while (true) {
00047         // Get velocities
00048         float left_velocity = float(left_enc.Get() - left_pulses)/RATE;
00049         float right_velocity = float(right_enc.Get() - right_pulses)/RATE;
00050         
00051         // Pass the PID Controller the latest velocity value
00052         leftPID.setProcessValue(left_velocity);
00053         rightPID.setProcessValue(right_velocity);
00054         
00055         // Query the PID Controllers for the desired power outputs
00056         // and set the motors to those powers
00057         left_motor.set(leftPID.compute());
00058         right_motor.set(rightPID.compute());
00059         
00060         // Save the number of encoder ticks for velocity calculation in the next loop
00061         left_pulses = left_enc.Get();
00062         right_pulses = right_enc.Get();
00063         
00064         // Toggle the LED and wait
00065         led1 = 1 - led1;
00066         wait(RATE);
00067     }
00068 }