Diff: main.cpp

Revision:

0:e019949c4c69

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 20 12:55:47 2011 +0000
@@ -0,0 +1,68 @@
+#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);
+    }
+}