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

Committer:
tzyoung
Date:
Fri Oct 06 13:25:26 2017 +0000
Revision:
2:892b58e56712
Parent:
1:133216180125
Child:
3:7824127c5cfd
cleaned up and re-factored code to make some classes more general and extendable

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 2:892b58e56712 3 #include "config_functions.h"
tzyoung 2:892b58e56712 4
tzyoung 2:892b58e56712 5 #define BCE_P 0.1
tzyoung 2:892b58e56712 6 #define BCE_I 0.0
tzyoung 2:892b58e56712 7 #define BCE_D 0.0
tzyoung 2:892b58e56712 8
tzyoung 2:892b58e56712 9 #define BCE_ZERO 400 //counts
tzyoung 2:892b58e56712 10 #define BCE_LIMIT 498.729 //mm
tzyoung 2:892b58e56712 11 #define POT_SLOPE .121760 // mm/counts
tzyoung 2:892b58e56712 12
tzyoung 2:892b58e56712 13 #define BATT_P 0.1
tzyoung 2:892b58e56712 14 #define BATT_I 0.0
tzyoung 2:892b58e56712 15 #define BATT_D 0.0
tzyoung 2:892b58e56712 16
tzyoung 2:892b58e56712 17 #define BATT_ZERO 100 //counts
tzyoung 2:892b58e56712 18 #define BATT_LIMIT 50.0 //mm
tzyoung 2:892b58e56712 19
tzyoung 2:892b58e56712 20 #include <fstream>
tzyoung 2:892b58e56712 21 #include <iostream>
tzyoung 0:ea293bbf9717 22
tzyoung 0:ea293bbf9717 23 int main()
tzyoung 0:ea293bbf9717 24 {
tzyoung 2:892b58e56712 25 //initialize linear actuator objects (doesn't work)
tzyoung 2:892b58e56712 26 load_BCE_config();
tzyoung 2:892b58e56712 27
tzyoung 2:892b58e56712 28 //Front load the desired parameters into the linear acuator objects.
tzyoung 2:892b58e56712 29 //This could be done using ConfigFile, if it worked
tzyoung 2:892b58e56712 30
tzyoung 2:892b58e56712 31 //I need to also check up in whether the limits are being passed to the linear
tzyoung 2:892b58e56712 32 //actuator's PID objects. I noticed I have defaults that are appropriate for only
tzyoung 2:892b58e56712 33 //the bouyancy engine
tzyoung 2:892b58e56712 34
tzyoung 2:892b58e56712 35 bce().setControllerP(BCE_P);
tzyoung 2:892b58e56712 36 bce().setControllerI(BCE_I);
tzyoung 2:892b58e56712 37 bce().setControllerD(BCE_D);
tzyoung 2:892b58e56712 38 bce().setZeroCounts(BCE_ZERO);
tzyoung 2:892b58e56712 39 bce().setTravelLimit(BCE_LIMIT);
tzyoung 2:892b58e56712 40 bce().setPotSlope(POT_SLOPE);
tzyoung 2:892b58e56712 41
tzyoung 2:892b58e56712 42 batt().setControllerP(BATT_P);
tzyoung 2:892b58e56712 43 batt().setControllerI(BATT_I);
tzyoung 2:892b58e56712 44 batt().setControllerD(BATT_D);
tzyoung 2:892b58e56712 45 batt().setZeroCounts(BATT_ZERO);
tzyoung 2:892b58e56712 46 batt().setTravelLimit(BATT_LIMIT);
tzyoung 2:892b58e56712 47 batt().setPotSlope(POT_SLOPE);
tzyoung 2:892b58e56712 48
tzyoung 2:892b58e56712 49 //start up the system timer
tzyoung 0:ea293bbf9717 50 systemTime().start();
tzyoung 0:ea293bbf9717 51
tzyoung 0:ea293bbf9717 52 //setup and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 53 adc().initialize();
tzyoung 0:ea293bbf9717 54 adc().start();
tzyoung 0:ea293bbf9717 55
tzyoung 2:892b58e56712 56 //start the bce and batt control loops. ADC has to be running first.
tzyoung 2:892b58e56712 57 //The motors will not activate until their respective position filters have
tzyoung 2:892b58e56712 58 //converged
tzyoung 2:892b58e56712 59 bce().start();
tzyoung 2:892b58e56712 60 batt().start();
tzyoung 2:892b58e56712 61
tzyoung 2:892b58e56712 62 //read in a script file describing the dive cycle
tzyoung 2:892b58e56712 63 //I envision entries such as
tzyoung 2:892b58e56712 64 // target pitch angle target depth target depth rate
tzyoung 2:892b58e56712 65 // 10 degrees 5 ft 0.05 ft/s example dive request
tzyoung 2:892b58e56712 66 // -10 degrees 0 ft -0.05 ft/s example surface request
tzyoung 2:892b58e56712 67
tzyoung 2:892b58e56712 68 //this implies two pid controllers
tzyoung 2:892b58e56712 69 // one that manages the batt mover for pitch
tzyoung 2:892b58e56712 70 // the other manages the buoyance engine for dive speed
tzyoung 2:892b58e56712 71
tzyoung 2:892b58e56712 72 // then some logic is needed to check the box when the desired condition is reached
tzyoung 2:892b58e56712 73 // like a waypoint threshold. This allows you to get away from worrying as much about
tzyoung 2:892b58e56712 74 // keeping time
tzyoung 0:ea293bbf9717 75
tzyoung 0:ea293bbf9717 76
tzyoung 0:ea293bbf9717 77
tzyoung 0:ea293bbf9717 78 while(1) {
tzyoung 2:892b58e56712 79
tzyoung 2:892b58e56712 80 //psuedo code to outline what we want to do
tzyoung 2:892b58e56712 81 /*
tzyoung 2:892b58e56712 82 check for and parse IMU data
tzyoung 2:892b58e56712 83
tzyoung 2:892b58e56712 84 poll for depth adc reading (eventually this will come from the external adc)
tzyoung 2:892b58e56712 85
tzyoung 2:892b58e56712 86 run depth data through position velocity filter
tzyoung 2:892b58e56712 87
tzyoung 2:892b58e56712 88 update the PID controllers for Pitch and depth rate
tzyoung 2:892b58e56712 89
tzyoung 2:892b58e56712 90 check whether depth has been triggered
tzyoung 2:892b58e56712 91 if so, move on to the next line of the script
tzyoung 2:892b58e56712 92 if done , surface or repeat
tzyoung 2:892b58e56712 93
tzyoung 2:892b58e56712 94 */
tzyoung 0:ea293bbf9717 95
tzyoung 2:892b58e56712 96 /*
tzyoung 2:892b58e56712 97 This can be ignored for now this was the old serial comms stuff for when I
tzyoung 2:892b58e56712 98 was prototyping the BCE controls
tzyoung 2:892b58e56712 99
tzyoung 0:ea293bbf9717 100 if (pc().readable()) {
tzyoung 0:ea293bbf9717 101 // get the key
tzyoung 0:ea293bbf9717 102 userInput = pc().getc();
tzyoung 0:ea293bbf9717 103
tzyoung 0:ea293bbf9717 104 //check command against desired control buttons
tzyoung 0:ea293bbf9717 105 if (userInput == '=') {
tzyoung 0:ea293bbf9717 106 //increment the duty cycle
tzyoung 0:ea293bbf9717 107 positionCmd += 5.0 ;
tzyoung 0:ea293bbf9717 108 } else if (userInput == '-') {
tzyoung 0:ea293bbf9717 109 //decrement the duty cycle
tzyoung 0:ea293bbf9717 110 positionCmd -= 5.0 ;
tzyoung 0:ea293bbf9717 111 }
tzyoung 0:ea293bbf9717 112
tzyoung 0:ea293bbf9717 113 if (userInput == 'w') {
tzyoung 0:ea293bbf9717 114 //increment the P gain
tzyoung 0:ea293bbf9717 115 P += 0.01;
tzyoung 0:ea293bbf9717 116 }
tzyoung 0:ea293bbf9717 117 if (userInput == 's') {
tzyoung 0:ea293bbf9717 118 //decrement the P gain
tzyoung 0:ea293bbf9717 119 P -= 0.01;
tzyoung 0:ea293bbf9717 120 }
tzyoung 0:ea293bbf9717 121 if (userInput == 'i') {
tzyoung 0:ea293bbf9717 122 //increment the D gain
tzyoung 0:ea293bbf9717 123 D += 0.001;
tzyoung 0:ea293bbf9717 124 }
tzyoung 0:ea293bbf9717 125 if (userInput == 'k') {
tzyoung 0:ea293bbf9717 126 //decrement the D gain
tzyoung 0:ea293bbf9717 127 D -= 0.001;
tzyoung 0:ea293bbf9717 128 }
tzyoung 0:ea293bbf9717 129 if (userInput == 't') {
tzyoung 0:ea293bbf9717 130 //increment the I gain
tzyoung 0:ea293bbf9717 131 I += 0.001;
tzyoung 0:ea293bbf9717 132 }
tzyoung 0:ea293bbf9717 133 if (userInput == 'g') {
tzyoung 0:ea293bbf9717 134 //decrement the I gain
tzyoung 0:ea293bbf9717 135 I -= 0.001;
tzyoung 0:ea293bbf9717 136 }
tzyoung 2:892b58e56712 137 if (userInput == ' ') {
tzyoung 0:ea293bbf9717 138 //reset the h-bridge
tzyoung 2:892b58e56712 139 //hBridge().reset();
tzyoung 0:ea293bbf9717 140 }
tzyoung 0:ea293bbf9717 141
tzyoung 0:ea293bbf9717 142 //if (userInput == ' ') {
tzyoung 0:ea293bbf9717 143 //stop the ride
tzyoung 0:ea293bbf9717 144 // motor_cmd = 0.0 ;
tzyoung 0:ea293bbf9717 145 //}
tzyoung 0:ea293bbf9717 146
tzyoung 0:ea293bbf9717 147 //clip pwm if over range
tzyoung 0:ea293bbf9717 148 //if (motor_cmd > 1.0) {
tzyoung 0:ea293bbf9717 149 // motor_cmd = 1.0;
tzyoung 0:ea293bbf9717 150 //}
tzyoung 0:ea293bbf9717 151 //if (motor_cmd < -1.0) {
tzyoung 0:ea293bbf9717 152 // motor_cmd = -1.0;
tzyoung 0:ea293bbf9717 153 //}
tzyoung 0:ea293bbf9717 154 // assign the shiny new commands to the pins
tzyoung 1:133216180125 155 //hBridge().run(motor_cmd);
tzyoung 0:ea293bbf9717 156 if (userInput == '\r') {
tzyoung 2:892b58e56712 157 bce().setPosition_mm(positionCmd);
tzyoung 2:892b58e56712 158 // posCon().setPgain(P);
tzyoung 2:892b58e56712 159 // posCon().setIgain(I);
tzyoung 2:892b58e56712 160 // posCon().setDgain(D);
tzyoung 2:892b58e56712 161 }*/
tzyoung 0:ea293bbf9717 162
tzyoung 2:892b58e56712 163 // }
tzyoung 0:ea293bbf9717 164
tzyoung 2:892b58e56712 165 //pc().printf("Position: %10.1f mm Velocity: % 2.2f mm/s Output: % 1.3f switch: %d \r", bce().getPosition_mm(), bce().getVelocity_mms(), bce().getOutput(), bce().getSwitch());
tzyoung 2:892b58e56712 166 //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 167
tzyoung 0:ea293bbf9717 168
tzyoung 0:ea293bbf9717 169 }
tzyoung 0:ea293bbf9717 170 }