FSG / Mbed 2 deprecated 7_20_17_FSG_

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BCEmotor Battery_Linear_Actuator_ Controller_ IMU_code_ LTC1298_7_14 MODSERIAL PosVelFilter_7_14 System_ mbed

Fork of 7_14_17_FSG_working by Troy Holley

main.cpp@0:381a84fad08b, 2017-06-09 (annotated)

Committer:
mdavis30
Date:
Fri Jun 09 17:32:33 2017 +0000
Revision:
0:381a84fad08b
Child:
2:c3cb3ea3c9fa
works and doesn't move when reset

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mdavis30 0:381a84fad08b 1 #include "mbed.h"
mdavis30 0:381a84fad08b 2 #include "MODSERIAL.h" //for IMU (and got rid of regular serial library) #include "IMU_code.h"
mdavis30 0:381a84fad08b 3 #include "StaticDefs.hpp"
mdavis30 0:381a84fad08b 4 #include "ltc1298.hpp"
mdavis30 0:381a84fad08b 5
mdavis30 0:381a84fad08b 6 #include <cstdlib>
mdavis30 0:381a84fad08b 7
mdavis30 0:381a84fad08b 8 #include <string>
mdavis30 0:381a84fad08b 9 using namespace std;
mdavis30 0:381a84fad08b 10
mdavis30 0:381a84fad08b 11 #include "IMU_code.h" //IMU code
mdavis30 0:381a84fad08b 12
mdavis30 0:381a84fad08b 13 #include "Battery_Linear_Actuator.h" //Battery Linear Actuator code (TROY) (FIX INCLUSION ISSUES, ports)
mdavis30 0:381a84fad08b 14
mdavis30 0:381a84fad08b 15 Serial PC(USBTX,USBRX); //tx, rx
mdavis30 0:381a84fad08b 16
mdavis30 0:381a84fad08b 17 extern "C" void mbed_reset(); //utilized to reset the mbed through the serial terminal
mdavis30 0:381a84fad08b 18
mdavis30 0:381a84fad08b 19 char Key;
mdavis30 0:381a84fad08b 20
mdavis30 0:381a84fad08b 21 string IMU_STRING = "";
mdavis30 0:381a84fad08b 22
mdavis30 0:381a84fad08b 23 DigitalOut pin_ENA(p23); //ENA //DigitalOut PinName (pin) //Troy: IF WE NEED IT
mdavis30 0:381a84fad08b 24 DigitalOut pin_IN1(p22); //IN1 (changed from pin11 and 12)
mdavis30 0:381a84fad08b 25 DigitalOut pin_IN2(p21); //IN2 //should I use wait or interrupt to control this?
mdavis30 0:381a84fad08b 26
mdavis30 0:381a84fad08b 27 DigitalOut led1(LED1);
mdavis30 0:381a84fad08b 28 DigitalOut led2(LED2);
mdavis30 0:381a84fad08b 29 DigitalOut led3(LED3);
mdavis30 0:381a84fad08b 30 DigitalOut led4(LED4);
mdavis30 0:381a84fad08b 31
mdavis30 0:381a84fad08b 32 DigitalOut enab(p9); // a/d converter enable
mdavis30 0:381a84fad08b 33
mdavis30 0:381a84fad08b 34 AnalogIn pressure_analog_in(A5); //Initialize pin20 (read is float value)
mdavis30 0:381a84fad08b 35
mdavis30 0:381a84fad08b 36 /* ************ These tickers work independent of any while loops ********** */
mdavis30 0:381a84fad08b 37 Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker
mdavis30 0:381a84fad08b 38 Ticker BE_position_ticker; //probably delete soon
mdavis30 0:381a84fad08b 39 Ticker PRESSURE_ticker;
mdavis30 0:381a84fad08b 40
mdavis30 0:381a84fad08b 41 Ticker BCE_ticker; //new 6/5/17
mdavis30 0:381a84fad08b 42 /*float motor_cmd = 0.0;
mdavis30 0:381a84fad08b 43 float positionCmd = 250.0;
mdavis30 0:381a84fad08b 44 float P = 0.10;
mdavis30 0:381a84fad08b 45 float I = 0.00;
mdavis30 0:381a84fad08b 46 float D = 0.00;
mdavis30 0:381a84fad08b 47 */
mdavis30 0:381a84fad08b 48 float positionCmd = 250.0;
mdavis30 0:381a84fad08b 49 /* ************************************************************************* */
mdavis30 0:381a84fad08b 50
mdavis30 0:381a84fad08b 51 void IMU_ticking()
mdavis30 0:381a84fad08b 52 {
mdavis30 0:381a84fad08b 53 led1 = !led1; //flash the IMU LED
mdavis30 0:381a84fad08b 54
mdavis30 0:381a84fad08b 55 if (!IMU_STRING.empty())
mdavis30 0:381a84fad08b 56 PC.printf("%s\n", IMU_STRING); //if there's something there, print it
mdavis30 0:381a84fad08b 57 }
mdavis30 0:381a84fad08b 58
mdavis30 0:381a84fad08b 59 void PRESSURE_ticking()
mdavis30 0:381a84fad08b 60 {
mdavis30 0:381a84fad08b 61 PC.printf("Pressure sensor (PSI)\nPRESS_PSI: %3.3f\n", 3.3*1.503*10 * pressure_analog_in.read()); //read the analog pin
mdavis30 0:381a84fad08b 62 //this voltage has been checked and scaled properly (5/19/2017)
mdavis30 0:381a84fad08b 63 }
mdavis30 0:381a84fad08b 64
mdavis30 0:381a84fad08b 65 void BCE_ticking() //new 6/5/17
mdavis30 0:381a84fad08b 66 {
mdavis30 0:381a84fad08b 67 PC.printf("BE_pos: %3.0f mm BE_vel: %2.2f mm/s Set Point %3.0f controller output: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
mdavis30 0:381a84fad08b 68 }
mdavis30 0:381a84fad08b 69
mdavis30 0:381a84fad08b 70 int main()
mdavis30 0:381a84fad08b 71 {
mdavis30 0:381a84fad08b 72 PC.baud(115200); //mbed to PC serial connection speed
mdavis30 0:381a84fad08b 73 //PC.baud(230400);
mdavis30 0:381a84fad08b 74 //got screwy when i changed it
mdavis30 0:381a84fad08b 75 hBridge().stop();
mdavis30 0:381a84fad08b 76 //start up the system timer so the position velocity estimator has a time signal
mdavis30 0:381a84fad08b 77 systemTime().start();
mdavis30 0:381a84fad08b 78 enab=1;
mdavis30 0:381a84fad08b 79 //setup and start the adc. This runs on a fixed interval and is interrupt driven
mdavis30 0:381a84fad08b 80 adc().initialize();
mdavis30 0:381a84fad08b 81 adc().start();
mdavis30 0:381a84fad08b 82
mdavis30 0:381a84fad08b 83 //Initialize the position velocity filter. This will consume a couple of seconds for
mdavis30 0:381a84fad08b 84 //the filter to converge
mdavis30 0:381a84fad08b 85 pvf().init();
mdavis30 0:381a84fad08b 86
mdavis30 0:381a84fad08b 87 ////CHANGED TO GLOBAL VARIABLES
mdavis30 0:381a84fad08b 88 float motor_cmd = 0.0;
mdavis30 0:381a84fad08b 89 // float positionCmd = 250.0;
mdavis30 0:381a84fad08b 90 float P = 0.10;
mdavis30 0:381a84fad08b 91 float I = 0.00;
mdavis30 0:381a84fad08b 92 float D = 0.00;
mdavis30 0:381a84fad08b 93 float count = 0.0;
mdavis30 0:381a84fad08b 94 //char userInput; //from Trent's code?
mdavis30 0:381a84fad08b 95
mdavis30 0:381a84fad08b 96 hBridge().run(motor_cmd);
mdavis30 0:381a84fad08b 97
mdavis30 0:381a84fad08b 98 //set the intial gains for the position controller
mdavis30 0:381a84fad08b 99 posCon().setPgain(P);
mdavis30 0:381a84fad08b 100 posCon().setIgain(I);
mdavis30 0:381a84fad08b 101 posCon().setDgain(D);
mdavis30 0:381a84fad08b 102 //posCon().writeSetPoint(positionCmd);
mdavis30 0:381a84fad08b 103
mdavis30 0:381a84fad08b 104 /* *************************** LED *************************** */
mdavis30 0:381a84fad08b 105 led1 = 1; //initial values
mdavis30 0:381a84fad08b 106 led2 = 1;
mdavis30 0:381a84fad08b 107 led3 = 1;
mdavis30 0:381a84fad08b 108 led4 = 1;
mdavis30 0:381a84fad08b 109 /* *************************** LED *************************** */
mdavis30 0:381a84fad08b 110
mdavis30 0:381a84fad08b 111 PC.printf("Program Started 6/5/17\n");
mdavis30 0:381a84fad08b 112 //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
mdavis30 0:381a84fad08b 113
mdavis30 0:381a84fad08b 114 /* *************************** Potentiometer *************************** */
mdavis30 0:381a84fad08b 115 PRESSURE_ticker.attach(&PRESSURE_ticking, 2.0);
mdavis30 0:381a84fad08b 116 /* *************************** Potentiometer *************************** */
mdavis30 0:381a84fad08b 117
mdavis30 0:381a84fad08b 118 /* *************************** MOTOR CONTROLLER *************************** */
mdavis30 0:381a84fad08b 119 Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
mdavis30 0:381a84fad08b 120 /* *************************** MOTOR CONTROLLER *************************** */
mdavis30 0:381a84fad08b 121
mdavis30 0:381a84fad08b 122 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 123 IMU_code IMU_object; //create the IMU object from the imported class
mdavis30 0:381a84fad08b 124 IMU_ticker.attach(&IMU_ticking, 2.0);
mdavis30 0:381a84fad08b 125 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 126
mdavis30 0:381a84fad08b 127 /* *************************** BCE *************************** */
mdavis30 0:381a84fad08b 128 float previous_positionCmd = -1;
mdavis30 0:381a84fad08b 129 BCE_ticker.attach(&BCE_ticking, 2.0);
mdavis30 0:381a84fad08b 130 /* *************************** BCE *************************** */
mdavis30 0:381a84fad08b 131
mdavis30 0:381a84fad08b 132 while(1)
mdavis30 0:381a84fad08b 133 {
mdavis30 0:381a84fad08b 134 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 135 IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
mdavis30 0:381a84fad08b 136 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 137
mdavis30 0:381a84fad08b 138 /* Buoyancy Engine */
mdavis30 0:381a84fad08b 139 // update the position velocity filter
mdavis30 0:381a84fad08b 140 pvf().update();
mdavis30 0:381a84fad08b 141
mdavis30 0:381a84fad08b 142 //update the controller with the current numbers in the position guesser
mdavis30 0:381a84fad08b 143 posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
mdavis30 0:381a84fad08b 144 hBridge().run(posCon().getOutput());
mdavis30 0:381a84fad08b 145
mdavis30 0:381a84fad08b 146 /* Buoyancy Engine */
mdavis30 0:381a84fad08b 147
mdavis30 0:381a84fad08b 148 //FOR DEBUGGING
mdavis30 0:381a84fad08b 149 //PC.printf("BE_pos: %3.0f mm BE_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);
mdavis30 0:381a84fad08b 150
mdavis30 0:381a84fad08b 151 //PC.printf("WHILE LOOP\n"); //DEBUG
mdavis30 0:381a84fad08b 152 if (PC.readable())
mdavis30 0:381a84fad08b 153 {
mdavis30 0:381a84fad08b 154 //PC.printf("PC IS READABLE\n"); //DEBUG
mdavis30 0:381a84fad08b 155 Key=PC.getc();
mdavis30 0:381a84fad08b 156 if(Key=='a' or Key =='A')
mdavis30 0:381a84fad08b 157 {
mdavis30 0:381a84fad08b 158 PC.printf("%s\n", BLA_object.Keyboard_A());
mdavis30 0:381a84fad08b 159 }
mdavis30 0:381a84fad08b 160
mdavis30 0:381a84fad08b 161 else if(Key=='d' or Key =='D')
mdavis30 0:381a84fad08b 162 {
mdavis30 0:381a84fad08b 163 PC.printf("%s\n", BLA_object.Keyboard_D());
mdavis30 0:381a84fad08b 164 }
mdavis30 0:381a84fad08b 165
mdavis30 0:381a84fad08b 166 else if(Key=='w' or Key =='W')
mdavis30 0:381a84fad08b 167 {
mdavis30 0:381a84fad08b 168 PC.printf("\nW received!\n");
mdavis30 0:381a84fad08b 169
mdavis30 0:381a84fad08b 170 posCon().writeSetPoint(positionCmd);
mdavis30 0:381a84fad08b 171 //posCon().setPgain(P);
mdavis30 0:381a84fad08b 172 //posCon().setIgain(I);
mdavis30 0:381a84fad08b 173 //posCon().setDgain(D);
mdavis30 0:381a84fad08b 174 hBridge().run(posCon().getOutput());
mdavis30 0:381a84fad08b 175
mdavis30 0:381a84fad08b 176 hBridge().reset();
mdavis30 0:381a84fad08b 177
mdavis30 0:381a84fad08b 178 count = 0;
mdavis30 0:381a84fad08b 179
mdavis30 0:381a84fad08b 180 }
mdavis30 0:381a84fad08b 181
mdavis30 0:381a84fad08b 182 else if(Key =='Q') //motor disabled
mdavis30 0:381a84fad08b 183 {
mdavis30 0:381a84fad08b 184 PC.printf("%s\n", BLA_object.Keyboard_Q());
mdavis30 0:381a84fad08b 185 }
mdavis30 0:381a84fad08b 186
mdavis30 0:381a84fad08b 187 else if(Key =='E') //motor enabled
mdavis30 0:381a84fad08b 188 {
mdavis30 0:381a84fad08b 189 PC.printf("%s\n", BLA_object.Keyboard_E());
mdavis30 0:381a84fad08b 190 }
mdavis30 0:381a84fad08b 191
mdavis30 0:381a84fad08b 192 else if(Key=='-' or Key =='_')
mdavis30 0:381a84fad08b 193 {
mdavis30 0:381a84fad08b 194 PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY()); //decrease step size
mdavis30 0:381a84fad08b 195 }
mdavis30 0:381a84fad08b 196
mdavis30 0:381a84fad08b 197 else if(Key=='+' or Key=='=')
mdavis30 0:381a84fad08b 198 {
mdavis30 0:381a84fad08b 199 PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY()); //increase step size
mdavis30 0:381a84fad08b 200 }
mdavis30 0:381a84fad08b 201
mdavis30 0:381a84fad08b 202 else if(Key==']') //100 to 30000, INCREASE (100 increments)
mdavis30 0:381a84fad08b 203 {
mdavis30 0:381a84fad08b 204 PC.printf("%s\n", BLA_object.Keyboard_RIGHT_BRACKET()); //motor speed
mdavis30 0:381a84fad08b 205 }
mdavis30 0:381a84fad08b 206
mdavis30 0:381a84fad08b 207 else if(Key=='[') //30000 to 100, DECREASE (100 increments)
mdavis30 0:381a84fad08b 208 {
mdavis30 0:381a84fad08b 209 PC.printf("%s\n", BLA_object.Keyboard_LEFT_BRACKET()); //motor speed
mdavis30 0:381a84fad08b 210 }
mdavis30 0:381a84fad08b 211
mdavis30 0:381a84fad08b 212 else if(Key=='!') //RESET THE MBED
mdavis30 0:381a84fad08b 213 {
mdavis30 0:381a84fad08b 214 PC.printf("MBED RESET KEY (!) PRESSED\n");
mdavis30 0:381a84fad08b 215 PC.printf("Linear Actuator Motor disabled! Please re-enable.\n");
mdavis30 0:381a84fad08b 216 //disable the motor
mdavis30 0:381a84fad08b 217 BLA_object.Keyboard_Q(); //DISABLE THE MOTOR
mdavis30 0:381a84fad08b 218 wait(0.5); //500 milliseconds
mdavis30 0:381a84fad08b 219 mbed_reset(); //reset the mbed!
mdavis30 0:381a84fad08b 220 }
mdavis30 0:381a84fad08b 221
mdavis30 0:381a84fad08b 222 else if(Key =='H') //homing sequence
mdavis30 0:381a84fad08b 223 {
mdavis30 0:381a84fad08b 224 PC.printf("%s\n", BLA_object.Keyboard_H()); //home linear actuator manually
mdavis30 0:381a84fad08b 225 }
mdavis30 0:381a84fad08b 226 //check command against desired control buttons
mdavis30 0:381a84fad08b 227 else if (Key == 'z' or Key =='Z') {
mdavis30 0:381a84fad08b 228 //increment the duty cycle
mdavis30 0:381a84fad08b 229 positionCmd += 10.0 ;
mdavis30 0:381a84fad08b 230 PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
mdavis30 0:381a84fad08b 231 }
mdavis30 0:381a84fad08b 232 else if (Key == 'x' or Key == 'X') {
mdavis30 0:381a84fad08b 233 //decrement the duty cycle
mdavis30 0:381a84fad08b 234 positionCmd -= 10.0 ;
mdavis30 0:381a84fad08b 235 PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
mdavis30 0:381a84fad08b 236 }
mdavis30 0:381a84fad08b 237
mdavis30 0:381a84fad08b 238 else
mdavis30 0:381a84fad08b 239 {
mdavis30 0:381a84fad08b 240 PC.printf("\n%c received!\n", Key);
mdavis30 0:381a84fad08b 241 PC.printf("\nDoing nothing.\n");
mdavis30 0:381a84fad08b 242 }
mdavis30 0:381a84fad08b 243
mdavis30 0:381a84fad08b 244 wait_us(100); //for PC readable
mdavis30 0:381a84fad08b 245 }
mdavis30 0:381a84fad08b 246
mdavis30 0:381a84fad08b 247 wait_us(100); //for PC readable (0.1 ms)
mdavis30 0:381a84fad08b 248
mdavis30 0:381a84fad08b 249 if(BLA_object.MC_readable()) //if you can read the motor controller do this...
mdavis30 0:381a84fad08b 250 {
mdavis30 0:381a84fad08b 251 //PC.printf("BATTERY LINEAR ACTUATOR");
mdavis30 0:381a84fad08b 252
mdavis30 0:381a84fad08b 253
mdavis30 0:381a84fad08b 254 //PC.printf("Motor Controller response:\n");
mdavis30 0:381a84fad08b 255 // while(MC.readable())
mdavis30 0:381a84fad08b 256 // {
mdavis30 0:381a84fad08b 257 // PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc)
mdavis30 0:381a84fad08b 258 // wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
mdavis30 0:381a84fad08b 259 // }
mdavis30 0:381a84fad08b 260 }
mdavis30 0:381a84fad08b 261 if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
mdavis30 0:381a84fad08b 262 {
mdavis30 0:381a84fad08b 263 count ++;
mdavis30 0:381a84fad08b 264 //pc().printf("We have a small issue\n");
mdavis30 0:381a84fad08b 265 if(count==10)
mdavis30 0:381a84fad08b 266 {
mdavis30 0:381a84fad08b 267 pc().printf("Bad pot issue\n");
mdavis30 0:381a84fad08b 268 hBridge().stop();
mdavis30 0:381a84fad08b 269 }
mdavis30 0:381a84fad08b 270
mdavis30 0:381a84fad08b 271 }
mdavis30 0:381a84fad08b 272 //string snaps
mdavis30 0:381a84fad08b 273 else if (pvf().getVelocity() > 100)
mdavis30 0:381a84fad08b 274 {
mdavis30 0:381a84fad08b 275 //hBridge().stop();
mdavis30 0:381a84fad08b 276 pc().printf("********** String broke? *********\n");
mdavis30 0:381a84fad08b 277 }
mdavis30 0:381a84fad08b 278
mdavis30 0:381a84fad08b 279 }
mdavis30 0:381a84fad08b 280 }