Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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

main.cpp@1:133216180125, 2017-04-27 (annotated)

Committer:
tzyoung
Date:
Thu Apr 27 16:47:29 2017 +0000
Revision:
1:133216180125
Parent:
0:ea293bbf9717
Child:
2:892b58e56712
Fixed errors stopping file from compiling

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzyoung 0:ea293bbf9717 1 #include "mbed.h"
tzyoung 0:ea293bbf9717 2 #include "StaticDefs.hpp"
tzyoung 0:ea293bbf9717 3 #include "ltc1298.hpp"
tzyoung 0:ea293bbf9717 4
tzyoung 0:ea293bbf9717 5 int main()
tzyoung 0:ea293bbf9717 6 {
tzyoung 0:ea293bbf9717 7 //start up the system timer so the position velocity estimator has a time signal
tzyoung 0:ea293bbf9717 8 systemTime().start();
tzyoung 0:ea293bbf9717 9
tzyoung 0:ea293bbf9717 10 //setup and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 11 adc().initialize();
tzyoung 0:ea293bbf9717 12 adc().start();
tzyoung 0:ea293bbf9717 13
tzyoung 0:ea293bbf9717 14 //Initialize the position velocity filter. This will consume a couple of seconds for
tzyoung 0:ea293bbf9717 15 //the filter to converge
tzyoung 0:ea293bbf9717 16 pvf().init();
tzyoung 0:ea293bbf9717 17
tzyoung 0:ea293bbf9717 18
tzyoung 0:ea293bbf9717 19 float motor_cmd = 0.0;
tzyoung 0:ea293bbf9717 20 float positionCmd = 250.0;
tzyoung 0:ea293bbf9717 21 float P = 0.10;
tzyoung 0:ea293bbf9717 22 float I = 0.00;
tzyoung 0:ea293bbf9717 23 float D = 0.00;
tzyoung 0:ea293bbf9717 24 char userInput;
tzyoung 0:ea293bbf9717 25
tzyoung 1:133216180125 26 hBridge().run(motor_cmd);
tzyoung 0:ea293bbf9717 27
tzyoung 0:ea293bbf9717 28 //set the intial gains for the position controller
tzyoung 0:ea293bbf9717 29 posCon().setPgain(P);
tzyoung 0:ea293bbf9717 30 posCon().setIgain(I);
tzyoung 0:ea293bbf9717 31 posCon().setDgain(D);
tzyoung 0:ea293bbf9717 32 posCon().writeSetPoint(positionCmd);
tzyoung 0:ea293bbf9717 33
tzyoung 0:ea293bbf9717 34
tzyoung 0:ea293bbf9717 35 while(1) {
tzyoung 0:ea293bbf9717 36
tzyoung 0:ea293bbf9717 37 // update the position velocity filter
tzyoung 0:ea293bbf9717 38 pvf().update();
tzyoung 0:ea293bbf9717 39
tzyoung 0:ea293bbf9717 40 //update the controller with the current numbers in the position guesser
tzyoung 0:ea293bbf9717 41 posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
tzyoung 1:133216180125 42 hBridge().run(posCon().getOutput());
tzyoung 0:ea293bbf9717 43
tzyoung 0:ea293bbf9717 44 if (pc().readable()) {
tzyoung 0:ea293bbf9717 45 // get the key
tzyoung 0:ea293bbf9717 46 userInput = pc().getc();
tzyoung 0:ea293bbf9717 47
tzyoung 0:ea293bbf9717 48 //check command against desired control buttons
tzyoung 0:ea293bbf9717 49 if (userInput == '=') {
tzyoung 0:ea293bbf9717 50 //increment the duty cycle
tzyoung 0:ea293bbf9717 51 positionCmd += 5.0 ;
tzyoung 0:ea293bbf9717 52 } else if (userInput == '-') {
tzyoung 0:ea293bbf9717 53 //decrement the duty cycle
tzyoung 0:ea293bbf9717 54 positionCmd -= 5.0 ;
tzyoung 0:ea293bbf9717 55 }
tzyoung 0:ea293bbf9717 56
tzyoung 0:ea293bbf9717 57 if (userInput == 'w') {
tzyoung 0:ea293bbf9717 58 //increment the P gain
tzyoung 0:ea293bbf9717 59 P += 0.01;
tzyoung 0:ea293bbf9717 60 }
tzyoung 0:ea293bbf9717 61 if (userInput == 's') {
tzyoung 0:ea293bbf9717 62 //decrement the P gain
tzyoung 0:ea293bbf9717 63 P -= 0.01;
tzyoung 0:ea293bbf9717 64 }
tzyoung 0:ea293bbf9717 65 if (userInput == 'i') {
tzyoung 0:ea293bbf9717 66 //increment the D gain
tzyoung 0:ea293bbf9717 67 D += 0.001;
tzyoung 0:ea293bbf9717 68 }
tzyoung 0:ea293bbf9717 69 if (userInput == 'k') {
tzyoung 0:ea293bbf9717 70 //decrement the D gain
tzyoung 0:ea293bbf9717 71 D -= 0.001;
tzyoung 0:ea293bbf9717 72 }
tzyoung 0:ea293bbf9717 73 if (userInput == 't') {
tzyoung 0:ea293bbf9717 74 //increment the I gain
tzyoung 0:ea293bbf9717 75 I += 0.001;
tzyoung 0:ea293bbf9717 76 }
tzyoung 0:ea293bbf9717 77 if (userInput == 'g') {
tzyoung 0:ea293bbf9717 78 //decrement the I gain
tzyoung 0:ea293bbf9717 79 I -= 0.001;
tzyoung 0:ea293bbf9717 80 }
tzyoung 0:ea293bbf9717 81 if (userInput == ' '){
tzyoung 0:ea293bbf9717 82 //reset the h-bridge
tzyoung 1:133216180125 83 hBridge().reset();
tzyoung 0:ea293bbf9717 84 }
tzyoung 0:ea293bbf9717 85
tzyoung 0:ea293bbf9717 86 //if (userInput == ' ') {
tzyoung 0:ea293bbf9717 87 //stop the ride
tzyoung 0:ea293bbf9717 88 // motor_cmd = 0.0 ;
tzyoung 0:ea293bbf9717 89 //}
tzyoung 0:ea293bbf9717 90
tzyoung 0:ea293bbf9717 91 //clip pwm if over range
tzyoung 0:ea293bbf9717 92 //if (motor_cmd > 1.0) {
tzyoung 0:ea293bbf9717 93 // motor_cmd = 1.0;
tzyoung 0:ea293bbf9717 94 //}
tzyoung 0:ea293bbf9717 95 //if (motor_cmd < -1.0) {
tzyoung 0:ea293bbf9717 96 // motor_cmd = -1.0;
tzyoung 0:ea293bbf9717 97 //}
tzyoung 0:ea293bbf9717 98 // assign the shiny new commands to the pins
tzyoung 1:133216180125 99 //hBridge().run(motor_cmd);
tzyoung 0:ea293bbf9717 100 if (userInput == '\r') {
tzyoung 0:ea293bbf9717 101 posCon().writeSetPoint(positionCmd);
tzyoung 0:ea293bbf9717 102 posCon().setPgain(P);
tzyoung 0:ea293bbf9717 103 posCon().setIgain(I);
tzyoung 0:ea293bbf9717 104 posCon().setDgain(D);
tzyoung 0:ea293bbf9717 105 }
tzyoung 0:ea293bbf9717 106
tzyoung 0:ea293bbf9717 107 }
tzyoung 0:ea293bbf9717 108
tzyoung 0:ea293bbf9717 109 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);
tzyoung 0:ea293bbf9717 110
tzyoung 0:ea293bbf9717 111
tzyoung 0:ea293bbf9717 112 }
tzyoung 0:ea293bbf9717 113 }