Diff: main.cpp

Revision:

0:d3f432c5fb55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 03 14:42:26 2015 +0000
@@ -0,0 +1,46 @@
+#include "mbed.h"
+#include "PID.h"
+#include "hcsr04.h"
+//#include "HIDScope.h"
+
+//HIDScope    scope(2);
+
+HCSR04 sensor(D2, D3);
+float distance = 0;
+
+PwmOut speedPin(D5);
+DigitalOut dirPin(D4);
+
+const float dt = 0.001;
+PID         controller(-5, 0, 0.00001, dt);
+Ticker      controlLoop;
+//Ticker      scopeTicker;
+
+void control() {
+//    scope.set(0,distance);
+    controller.setProcessValue(distance);
+    controller.setSetPoint(20);
+    double speed = controller.compute();
+//    scope.set(1,speed);
+    if (speed > 0) {
+        dirPin.write(1);
+        speedPin.write(speed);
+    } else {
+        dirPin.write(0);
+        speedPin.write(speed * -1);
+    }
+}
+
+int main()
+{
+    controller.setInputLimits(5, 50);
+    controller.setOutputLimits(-1,1);
+    controller.setBias(0);
+    controller.setMode(AUTO_MODE);
+    
+//    scopeTicker.attach_us(&scope, &HIDScope::send, 2e4);
+    controlLoop.attach(&control, dt);
+    while (true) {
+        distance = sensor.distance();
+    }
+}
\ No newline at end of file