Diff: main.cpp

Revision:

0:ea293bbf9717

Child:

1:133216180125

diff -r 000000000000 -r ea293bbf9717 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 27 13:26:58 2017 +0000
@@ -0,0 +1,113 @@
+#include "mbed.h"
+#include "StaticDefs.hpp"
+#include "ltc1298.hpp"
+
+int main()
+{
+    //start up the system timer so the position velocity estimator has a time signal
+    systemTime().start();
+
+    //setup and start the adc. This runs on a fixed interval and is interrupt driven
+    adc().initialize();
+    adc().start();
+
+    //Initialize the position velocity filter. This will consume a couple of seconds for
+    //the filter to converge
+    pvf().init();
+
+
+    float motor_cmd = 0.0;
+    float positionCmd = 250.0;
+    float P = 0.10;
+    float I = 0.00;
+    float D = 0.00;
+    char userInput;
+
+    bce().run(motor_cmd);
+
+    //set the intial gains for the position controller
+    posCon().setPgain(P);
+    posCon().setIgain(I);
+    posCon().setDgain(D);
+    posCon().writeSetPoint(positionCmd);
+
+
+    while(1) {
+
+        // update the position velocity filter
+        pvf().update();
+
+        //update the controller with the current numbers in the position guesser
+        posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
+        bce().run(posCon().getOutput());
+
+        if (pc().readable()) {
+            // get the key
+            userInput = pc().getc();
+
+            //check command against desired control buttons
+            if (userInput == '=') {
+                //increment the duty cycle
+                positionCmd += 5.0 ;
+            } else if (userInput == '-') {
+                //decrement the duty cycle
+                positionCmd -= 5.0 ;
+            }
+
+            if (userInput == 'w') {
+                //increment the P gain
+                P += 0.01;
+            }
+            if (userInput == 's') {
+                //decrement the P gain
+                P -= 0.01;
+            }
+            if (userInput == 'i') {
+                //increment the D gain
+                D += 0.001;
+            }
+            if (userInput == 'k') {
+                //decrement the D gain
+                D -= 0.001;
+            }
+            if (userInput == 't') {
+                //increment the I gain
+                I += 0.001;
+            }
+            if (userInput == 'g') {
+                //decrement the I gain
+                I -= 0.001;
+            }
+            if (userInput == ' '){
+                //reset the h-bridge
+                bce().reset();    
+            }
+
+            //if (userInput == ' ') {
+            //stop the ride
+            //    motor_cmd = 0.0 ;
+            //}
+
+            //clip pwm if over range
+            //if (motor_cmd > 1.0) {
+            //    motor_cmd = 1.0;
+            //}
+            //if (motor_cmd < -1.0) {
+            //    motor_cmd = -1.0;
+            //}
+            // assign the shiny new commands to the pins
+            //bce().run(motor_cmd);
+            if (userInput == '\r') {
+                posCon().writeSetPoint(positionCmd);
+                posCon().setPgain(P);
+                posCon().setIgain(I);
+                posCon().setDgain(D);
+            }
+
+        }
+
+        pc().printf("pos: %3.0f mm  vel: % 2.2f mm/s  Set Point %3.0f  controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D);
+
+
+    }
+}