update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()

Dependencies:   mbed MODSERIAL FATFileSystem

main.cpp

Committer:
tzyoung
Date:
2017-04-27
Revision:
1:133216180125
Parent:
0:ea293bbf9717
Child:
2:892b58e56712

File content as of revision 1:133216180125:

#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;

    hBridge().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()) ;
        hBridge().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
                hBridge().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
            //hBridge().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);


    }
}