most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
Diff: main.cpp
- Revision:
- 0:ea293bbf9717
- Child:
- 1:133216180125
--- /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); + + + } +}