FSG
/
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
Diff: main.cpp
- Revision:
- 6:ce2cf7f4d7d5
- Parent:
- 5:7421776f6b08
- Child:
- 7:10d7fbac30ea
--- a/main.cpp Mon Jul 17 14:28:53 2017 +0000
+++ b/main.cpp Fri Jul 21 13:32:10 2017 +0000
@@ -8,8 +8,6 @@
#include <string>
using namespace std;
-bool debug_mode = false;
-
#include "IMU_code.h" //IMU code
#include "Battery_Linear_Actuator.h" //Battery Linear Actuator code (TROY) (FIX INCLUSION ISSUES, ports)
@@ -28,7 +26,7 @@
DigitalOut led3(LED3);
DigitalOut led4(LED4);
-AnalogIn pressure_analog_in(A5); //Initialize pin20 (read is float value)
+//Pin for limit switch for buoyancy engine
AnalogIn ain(p18);
/* ************ These tickers work independent of any while loops ********** */
@@ -39,64 +37,47 @@
Ticker BCE_ticker; //new 6/5/17
Ticker PID_ticker; //new 6/14/17
Ticker LA_ticker; //new 6/22/17
-
-float positionCmd = 200.0; //250
-
/* ************************************************************************* */
+//Set beginning position for buoyancy and linear actuator
+float positionCmd = 190.0;
float pi = 3.14159265359;
+float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint)
/* PID LOOP STUFF */
-float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint)
-
float la_P_gain = 1.0;
float la_I_gain = 0.00;
float la_D_gain = 0.00;
/* PID LOOP STUFF */
-float IMU_pitch_angle = 0.00;
+float IMU_pitch_angle;
+double double_actual_position = 0.00;
-bool motor_retracting = false;
-bool motor_extending = false;
-
-// 7/10/17
-string actual_position_string = "";
-double double_actual_position = 0.00;
+string MC_readable_string = "";
void IMU_ticking()
{
- led1 = !led1; //flash the IMU LED
-
- if (debug_mode)
- PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
+ //led1 = !led1; //flash the IMU LED
+
+ //PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
+ PC.printf("%s\n", IMU_STRING.c_str());
}
void PRESSURE_ticking()
{
- if (debug_mode)
- PC.printf("ressure: %f psi \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin
+ PC.printf("pressure: %f psi \r", (0.00122*(adc().ch1_filt)*9.9542)-0.0845); //read the analog pin
//this voltage has been checked and scaled properly (6/28/2017)
}
void BCE_ticking() //new 6/5/17
{
- if (debug_mode)
- PC.printf("Buoyancy_Engine_POS: %3.0f mm BE_vel: %2.2f mm/s Set Point %3.0f posCon.getOutput: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
-
- //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());
+ 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());
}
int main()
{
- PC.baud(9600); //mbed to PC serial connection speed
- //PC.baud(230400);
- //got screwy when i changed it
- hBridge().stop();
-
- PC.printf("* * * * * * * * * * * * * * * * *\n");
- PC.printf("PV TEST PROGRAM STARTED 7/13/2017\n");
- PC.printf("* * * * * * * * * * * * * * * * *\n");
-
+ PC.baud(9600); //mbed to PC serial connection speed
+ hBridge().stop(); //Stop the buoyancy engine from moving
systemTime().start(); //start the timer, needed for PID loop
/* **************** Linear Actuator MOTOR CONTROLLER **************** */
@@ -117,61 +98,54 @@
//the filter to converge
pvf().init();
-////CHANGED TO GLOBAL VARIABLES
+ //NEW 7/21 Homing the motor
+ PC.printf("### homing the device ###");
+ BLA_object.Keyboard_H();
+ wait(10); //for debugging
+
+
float motor_cmd = 0.0;
-// float positionCmd = 250.0;
float P = 0.10;
float I = 0.00;
float D = 0.00;
float count = 0.0;
- //char userInput; //from Trent's code?
+ float positionCmd_cm;
float la_step = 1.0;
float la_setPoint_temp = 0.0;
- //Start off in manual mode for testing.
- bool BCE_auto = false;
+ bool BCE_auto = true;
bool LA_auto = false;
- float bce_auto_step_raw = 1.0;
- float bce_auto_step_l;
- //float convert = 10000;
- float convert = 1;
- float bce_auto_step_ml = bce_auto_step_raw * convert;
- int bce_manual_step = 10;
- //float volume_bce = 90.0*convert; //Troy: Not sure I get the conversion
- float volume_bce = 0;
+ float bce_auto_step = 0.1;
+ float volume_bce = 1.0;
+ int bce_man_step = 50;
float positionCmd_temp;
- //float ml_to_l= 0.000000001; //Is this a milliliter? TROY: 7/10/17
- float ml_to_l= 0.001; //milliliter???? TROY: 7/10/17
-
- hBridge().run(motor_cmd);
//set the intial gains for the position controller
posCon().setPgain(P);
posCon().setIgain(I);
posCon().setDgain(D);
- posCon().writeSetPoint(positionCmd); //7/13/17 initialize the position of the motor to 200
-
- PC.printf("BCE Test Program Started!\n");
- wait(1);
+ posCon().writeSetPoint(positionCmd);
+ hBridge().reset();
+ hBridge().run(motor_cmd);
/* *************************** LED *************************** */
led1 = 1; //initial values
led2 = 1;
- led3 = 0;
+ led3 = 1;
led4 = 1;
/* *************************** LED *************************** */
-
- int la_cases = 0;
+
+ //PC.printf("Program Started 6/5/17\n");
int count_while = 0;
//hBridge().reset();
- PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n");
+ //PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n");
//PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
- /* ************************** Pressure Sensor ************************** */
- PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
- /* ************************** Pressure Sensor ************************** */
+ /* *************************** Potentiometer *************************** */
+ //PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
+ /* *************************** Potentiometer *************************** */
/* *************************** MOTOR CONTROLLER *************************** */
//Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
@@ -179,30 +153,20 @@
/* *************************** IMU *************************** */
IMU_code IMU_object; //create the IMU object from the imported class
- IMU_ticker.attach(&IMU_ticking, 3.0);
+ IMU_ticker.attach(&IMU_ticking, 5.0);
/* *************************** IMU *************************** */
/* *************************** BCE *************************** */
//float previous_positionCmd = -1;
- //BCE_ticker.attach(&BCE_ticking, 3.0);
+ //BCE_ticker.attach(&BCE_ticking, 10.0);
/* *************************** BCE *************************** */
while(1)
{
- //PC.printf("DEBUG: POT position: %f velocity: %f adc_count: %d VS conv_distance: %f adc_ch0_filter: %f\n ", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().getVelocity(), adc().get_ch0_filt); //DEBUG TROY
- if (debug_mode)
- {
- PC.printf("DEBUG: POT position: %6.3f velocity: %6.3f adc_count: %d VS conv_distance: %6.3f adc_ch0_filter: %6.3f\n", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().get_conv_distance(), adc().get_ch0_filt()); //DEBUG TROY
- PC.printf("DEBUG: dt: %f current_time: %f last_time: %f\n", pvf().getDt(), pvf().get_curr_time(), pvf().get_last_time()); //DEBUG TROY
- PC.printf("DEBUG: x1: %f x2: %f x1_dot: %f x2_dot: %f\n", pvf().get_x1(), pvf().get_x2(), pvf().get_x1_dot(), pvf().get_x2_dot()); //DEBUG TROY
- }
-
/* *************************** IMU *************************** */
IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
- IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?
-
- if (debug_mode)
- PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint);
+ IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?
+ //PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_yaw_angle, la_setPoint);
/* *************************** IMU *************************** */
/* Buoyancy Engine */
@@ -218,13 +182,15 @@
//FOR DEBUGGING
//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);
+ //PC.printf("WHILE LOOP\n"); //DEBUG
if (PC.readable())
{
- led4 = !led4; //read indicator changes
- Key=PC.getc();
+ //led4 != led4;
+ //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG
+ Key=PC.getc();
//Universal MBED Controls
- if(Key == '!') //RESET THE MBED
+ if(Key=='!') //RESET THE MBED
{
PC.printf("MBED RESET KEY (!) PRESSED\n");
PC.printf("Linear Actuator Motor disabled!\n");
@@ -233,49 +199,50 @@
wait(0.5); //500 milliseconds
mbed_reset(); //reset the mbed!
}
-
- else if(Key == '~') // (shift + '`')
- {
- debug_mode = !debug_mode; //shift it back and forth
- PC.printf(" # # # # # # DEBUG MODE: %d # # # # # # \n", debug_mode);
- }
-
- else if(Key == 'H') //homing sequence
+ else if(Key =='H') //homing sequence
{
PC.printf("### homing the device ###");
BLA_object.Keyboard_H();
- wait(5); //for debugging
+ wait(10); //for debugging
- ////TEST THIS
- //PC.printf("BE_pos: 0\n");
- //PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str()); //flip this back and forth
-
- //TROY: TEST THIS 7/11/2017
const char *char_actual_position = BLA_object.get_pos().c_str();
//actual_position_string = BLA_object.get_pos().c_str();
sscanf(char_actual_position, "%lf", &double_actual_position);
// 7/10/17
- PC.printf("### position is\nBEP: %lf ###\n", double_actual_position);
+ PC.printf("LA actual position: %lf \n", double_actual_position);
+
+ //TROY NOTES on Linear Actuator commands:
+ //"pos" returns integer value "66813 for example"
+ //"hosp-100" is the setting of the motor, it retracts at 100 rpm
+ //"gohoseq" to home it, it will not give you feedback
+ //print the position after you do this to see where the LA is
+ }
+ else if(Key=='p' or Key == 'P')
+ {
+ PC.printf("BCE piston position: %f velocity? %f (BCE active? %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
- wait(1); //for debugging
- }
- else if(Key == 'p' or Key == 'P')
- {
// 7/10/17
//actual_position_string = BLA_object.get_pos();
//actual_position_string = BLA_object.get_pos().c_str();
//const char *char_actual_position = string_actual_position.c_str();
+
const char *char_actual_position = BLA_object.get_pos().c_str();
//actual_position_string = BLA_object.get_pos().c_str();
sscanf(char_actual_position, "%lf", &double_actual_position);
// 7/10/17
- PC.printf("### position is\nBEP: %lf ###\n", double_actual_position); //flip this back and forth
+ PC.printf("### L.A. position is\nLA_AP: %lf ###\n", double_actual_position); //flip this back and forth
+
+ if (double_actual_position > 180000)
+ PC.printf(" <<<< REACHED MAXIMUM L.A. POSITION! >>>>\n");
+ else if (double_actual_position < 0)
+ PC.printf(" <<<< REACHED MINIMUM L.A. POSITION! >>>>\n");
+
wait(1); //for debugging
- // "-999999" means it is not working
+ // "-999999" means it is not working
}
//Buoyancy Engine Controls
@@ -304,57 +271,12 @@
}
}
//BCE Automatic Controls
- else if (Key == 's' or Key == 'S')
+ else if(Key =='d' or Key == 'D')
{
if (BCE_auto == true)
{
- //PC.printf("BCE Automatic Step Size Change\n");
- if (bce_auto_step_raw == 1.0)
- {
- bce_auto_step_raw = 5.0;
- }
- else if (bce_auto_step_raw == 5.0)
- {
- bce_auto_step_raw = 10.0;
- }
- else if (bce_auto_step_raw == 10.0)
- {
- bce_auto_step_raw = 50.0;
- }
- else if (bce_auto_step_raw == 50.0)
- {
- bce_auto_step_raw = 100.0;
- }
- else if (bce_auto_step_raw == 100.0)
- {
- bce_auto_step_raw = 1.0;
- }
- bce_auto_step_ml = bce_auto_step_raw * convert;
- PC.printf("BCE Auto Step Size Now\n BE_ST_ML: %7.0f milliliters\n", bce_auto_step_ml);
- }
- else
- {
- PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
-
- }
- }
- else if(Key == 'd' or Key == 'D') //0 mm = 0 mL, 350 mm = 1816 mL
- {
- PC.printf("(d) volume_bce: %f\n", volume_bce);
- if (BCE_auto == true)
- {
- PC.printf("(d) BCE_auto: %d\n", BCE_auto);
- if (volume_bce >= 1) //350 mm retracted from end = 1816 mL in buyoancy engine tube
- {
- volume_bce -= bce_auto_step_ml;
- float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
- PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB
- }
- else if (volume_bce < 1)
- {
- PC.printf("Volume reset to 1 mL!\n"); //keep the volume at zero mL
- volume_bce = 1;
- }
+ volume_bce -= bce_auto_step;
+ PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB
}
else
{
@@ -363,21 +285,10 @@
}
else if(Key == 'f' or Key == 'F')
{
- PC.printf("(f) volume_bce: %f\n", volume_bce);
if (BCE_auto == true)
{
- PC.printf("(f) BCE_auto: %d\n", BCE_auto);
- if (volume_bce <= 1816) //350 mm retracted from end = 1816 mL in buyoancy engine tube
- {
- volume_bce += bce_auto_step_ml;
- float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
- PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB
- }
- else if (volume_bce > 1816)
- {
- PC.printf("Volume reset to 1816 mL (max volume)!\n"); //keep the volume at 1816 mL (max)
- volume_bce = 1816;
- }
+ volume_bce += bce_auto_step;
+ PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB
}
else
{
@@ -389,19 +300,19 @@
if (BCE_auto == true)
{
PC.printf("\nR received!\n");
-
- //Troy equation (volume 1 mL = 1000 mm^3)
- positionCmd = (volume_bce * 1000) / (pi*40.64*40.64); //volume / (pi * r^2)
- PC.printf("\nBCE volume sent was %d mL (distance: %f mm)\n", volume_bce, positionCmd);
- PC.printf("(BCE Distance) VBE_SENT: %3.0f\n", positionCmd);
-
- posCon().writeSetPoint(positionCmd); //write the setPoint (target)
-
- hBridge().run(posCon().getOutput()); //run the h-bridge until it reaches the target
+ positionCmd_cm=(1000/(16*pi))*volume_bce;
+ positionCmd = positionCmd_cm*10;
+ //positionCmd= positionCmd_temp*0.000000001;
+ //PC.printf("BCE engine going to position: %3.2f\n", positionCmd);
+ PC.printf("\nBASETP: %3.0f\n", positionCmd);
+ posCon().writeSetPoint(positionCmd);
+ //posCon().setPgain(P);
+ //posCon().setIgain(I);
+ //posCon().setDgain(D);
+ hBridge().run(posCon().getOutput());
+
- hBridge().reset(); //reset to start this process of moving the h-bridge
-
- count = 0; //not sure...
+ count = 0;
}
else
{
@@ -414,88 +325,73 @@
if (BCE_auto == false)
{
PC.printf("BCE Manual Step Size Change\n");
- if (bce_manual_step == 1)
+ if (bce_man_step == 50)
{
- bce_manual_step = 10;
+ bce_man_step = 25;
}
- else if (bce_manual_step == 10)
+ else if (bce_man_step == 25)
{
- bce_manual_step = 25;
+ bce_man_step = 10;
}
- else if (bce_manual_step == 25)
+ else if (bce_man_step == 10)
{
- bce_manual_step = 50;
+ bce_man_step = 1;
}
- else if (bce_manual_step == 50)
+ else if (bce_man_step == 1)
{
- bce_manual_step = 1;
+ bce_man_step = 50;
}
- PC.printf("BCE Manual Step Size Now\nBEM_ST: %d\n", bce_manual_step);
- }
- else
- {
- PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
- }
- }
- else if (Key == 'z' or Key == 'Z')
- {
- if (BCE_auto == false)
- {
- positionCmd -= bce_manual_step;
- //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)
-
- //decrement the duty cycle
- if (positionCmd >= 50 && positionCmd <=350) //limit buoyancy engine position 25 to 375
- {
- PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
- }
- else if (positionCmd < 50)
- {
- PC.printf("BCE past limits! Reset to 50\n"); //to read in MATLAB
- positionCmd = 50;
- }
- }
- else
- {
- PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
- }
- }
- else if (Key == 'l' or Key == 'L')
- PC.printf("DEBUG: String position? %f velocity? %f (BCE active: %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
-
- else if (Key == 'x' or Key == 'X')
- {
- if (BCE_auto == false)
- {
- positionCmd += bce_manual_step;
- //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)
-
- //decrement the duty cycle
- if (positionCmd >= 50 && positionCmd <=350) //limit buoyancy engine position 25 to 375
- {
- PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
- }
- else if (positionCmd >350)
- {
- PC.printf("BCE past limits! Reset to 350\n"); //to read in MATLAB
- positionCmd = 350;
- }
+ PC.printf("BCE Manual Step Size Now %d\n", bce_man_step);
}
else
{
PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
}
}
- else if(Key == 'w' or Key == 'W')
+ else if (Key == 'z' or Key =='Z')
+ {
+ if (BCE_auto == false and positionCmd < 395)
+ {
+ //increment the duty cycle
+ positionCmd -= bce_man_step;
+ PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
+ }
+ else if (positionCmd > 395)
+ {
+ PC.printf("ERROR: Cannot move past 395 mm\n");
+ positionCmd = 370;
+ }
+ else
+ {
+ PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
+ }
+ }
+ else if (Key == 'x' or Key == 'X')
+ {
+ if (BCE_auto == false and positionCmd > -25)
+ {
+ //decrement the duty cycle
+ positionCmd += bce_man_step;
+ PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
+ }
+ else if (positionCmd < -25)
+ {
+ PC.printf("ERROR: Cannot move past -5 mm\n");
+ positionCmd = -5;
+ }
+ else
+ {
+ PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
+ }
+ }
+ else if(Key=='w' or Key =='W')
{
if (BCE_auto == false)
{
PC.printf("\nW received!\n");
- PC.printf("BEM_SND: %3.0f\n", positionCmd);
-
- posCon().writeSetPoint(positionCmd); //writing once doesn't work sometimes
-
+ PC.printf("BASETP: %3.0f\n", positionCmd);
+ posCon().writeSetPoint(positionCmd);
//posCon().setPgain(P);
//posCon().setIgain(I);
//posCon().setDgain(D);
@@ -511,6 +407,8 @@
PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
}
}
+ else if (Key == 'l' or Key == 'L')
+ PC.printf("DEBUG: String position? %f velocity? %f (BCE active: %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
//Linear Actuator Controls
else if(Key == 'c' or Key == 'C')
@@ -522,8 +420,7 @@
else
{
LA_auto = true;
- PC.printf("```````````LA now in IMU (Auto) Controlled Mode```````````````\n");
- la_cases = 0;
+ PC.printf("```````````Now in IMU Controlled Mode```````````````\n");
count_while = 0;
}
}
@@ -532,55 +429,46 @@
if (LA_auto == true)
{
LA_auto = false;
- //Change cases: go from imu controlled to manual
- PC.printf("```````````LA now in Manual Mode````````````````````\n");
- la_cases = 1;
+ //go from imu controlled to manual
+ PC.printf("```````````Now in Manual Mode````````````````````\n");
count_while = 0;
}
else
{
PC.printf("ERROR: LA already in manual mode\n");
- PC.printf("LA_auto ==> %d\n", LA_auto); //should show "0" (false)
}
}
else if (Key == '0' or Key == ')')
{
- if (LA_auto == true)
+ PC.printf(") recieved\n");
+ if (la_step == 0.5)
+ {
+ la_step = 1.0;
+ }
+ else if (la_step == 1.0)
+ {
+ la_step = 5.0;
+ }
+ else if (la_step == 5.0)
{
- PC.printf(") recieved\n");
- if (la_step == 0.5)
- {
- la_step = 1.0;
- }
- else if (la_step == 1.0)
- {
- la_step = 5.0;
- }
- else if (la_step == 5.0)
- {
- la_step = 10.0;
- }
- else if (la_step == 10.0)
- {
- la_step = 15.0;
- }
- else if (la_step == 15.0)
- {
- la_step = 0.5;
- }
- PC.printf("LA Step Size Now\nLA_ST_SZ: %f\n", la_step);
+ la_step = 10.0;
+ }
+ else if (la_step == 10.0)
+ {
+ la_step = 15.0;
}
- else
- {
- PC.printf("ERROR: LA in manual mode!\n");
+ else if (la_step == 15.0)
+ {
+ la_step = 0.5;
}
+ PC.printf("LA Step Size Now %f\n", la_step);
}
-
- else if (Key == '-' or Key == '_')
+
+ else if (Key=='-' or Key == '_')
{
if (LA_auto == true)
{
- la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
+ la_setPoint_temp -= la_step; //IMU_yaw_angle -= 1.0;
PC.printf("- recieved\n");
PC.printf("LA auto step size: %f\n", la_step);
PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
@@ -590,11 +478,12 @@
PC.printf("ERROR: LA in manual mode!\n");
}
}
- else if (Key == '=' or Key == '+')
+
+ else if (Key =='=' or Key == '+')
{
if (LA_auto == true)
{
- la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0;
+ la_setPoint_temp += la_step; //IMU_yaw_angle += 1.0;
PC.printf("+ recieved\n");
PC.printf("LA auto step size: %f\n", la_step);
PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
@@ -604,8 +493,9 @@
PC.printf("ERROR: LA in manual mode!\n");
}
}
+
else if (Key == 'A' or Key == 'a')
- {
+ {
if (LA_auto == true)
{
PC.printf("A recieved\n");
@@ -616,51 +506,11 @@
{
PC.printf("ERROR: LA in manual mode!\n");
}
- }
+ }
- else if (Key == '[' or Key == '{')
- {
- la_P_gain -= 0.1;
- PC.printf("[ key pressed\n");
- PC.printf("P gain is now %f\n", la_P_gain);
-
- }
- else if (Key == ']' or Key == '}')
- {
- la_P_gain += 0.1;
- PC.printf("] key pressed\n");
- PC.printf("P gain is now %f\n", la_P_gain);
-
- }
- else if (Key == ';')
- {
- la_I_gain -= 0.1;
- PC.printf("; key pressed\n");
- PC.printf("I gain is now %f\n", la_I_gain);
-
- }
- else if (Key == '\'')
- {
- la_I_gain += 0.1;
- PC.printf("\\ key pressed\n");
- PC.printf("I gain is now %f\n", la_I_gain);
- }
- else if (Key == '.')
- {
- la_D_gain -= 0.1;
- PC.printf(". key pressed\n");
- PC.printf("D gain is now %f\n", la_D_gain);
- }
- else if (Key == '/')
- {
- la_D_gain += 0.1;
- PC.printf("/ key pressed\n");
- PC.printf("D gain is now %f\n", la_D_gain);
- }
-
else if(Key == 'n' or Key == 'N')
- {
+ {
if (LA_auto == false)
{
PC.printf("N key pressed. \n");
@@ -672,6 +522,7 @@
PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
}
}
+
else if(Key == 'm' or Key == 'M')
{
if (LA_auto == false)
@@ -685,6 +536,7 @@
PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
}
}
+
else if(Key == 'j' or Key == 'J')
{
if (LA_auto == false)
@@ -698,6 +550,7 @@
PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
}
}
+
else if(Key == 'k' or Key == 'K')
{
if (LA_auto == false)
@@ -711,12 +564,6 @@
PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
}
}
-
- else if (Key == 't')
- {
- PC.printf("VELOCITY?\n%s\n",BLA_object.get_vel());
- }
-
else
{
@@ -725,16 +572,16 @@
}
wait_us(100); //for PC readable
- //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
- //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
+ //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
+ //BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
}
-
+ //end of PC readBLE
+
//MC readable
- string MC_readable_string = "";
MC_readable_string = BLA_object.MC_readable_redux();
//PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string);
- if (LA_auto == false)
+ /* if (LA_auto == false)
{
if (!MC_readable_string.empty()) //if this string is empty
{
@@ -745,65 +592,71 @@
;
//PC.printf("NOTHING?\n");
}
- }
-
- //change between automatic and manual mode of linear actuator? Troy: 7/11/2017
-
- if (la_cases==0)
+ }*/
+ if (LA_auto==true)
{
- if (debug_mode) //debug mode true
- PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
- else //debug mode false
- BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
+ //PC.printf("LA_auto true\n");
+ //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
+ BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
+ //wait_us(100); //for PC readable (0.1 ms)
}
- else if (la_cases==1)
- {
+ else if (LA_auto == false)
+ {
+ //PC.printf("LA_auto false\n");
+// if (!MC_readable_string.empty()) //if this string is empty
+// {
+// PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller
+// }
+// else
+// {
+// ;
+// //PC.printf("NOTHING?\n");
+// }
- while (count_while==0)
- {
- PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
- PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
- wait(1);
- PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
- wait(1);
- PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
- count_while++;
- }
+// while (count_while==0)
+// {
+//// PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
+//// PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
+//// wait(1);
+//// PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
+//// wait(1);
+//// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
+//// count_while++;
+//
+// BLA_object.Keyboard_E(); //turn on motor
+// BLA_object.velocity_only(0); //set the velocity to zero just in case
+// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
+// count_while++;
+// }
}
+
if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
{
count ++;
//pc().printf("We have a small issue\n");
- //PC.printf("posCon().getOutput() %f\n", posCon().getOutput()); //what is this again?
- //always 1?
- if(count==10)
- {
- //PC.printf("> > > Bad pot issue?\n");
- //hBridge().stop();
- count = 0; //reset counter
- }
+ if(count==10)
+ {
+ pc().printf("Bad pot issue\n");
+ //hBridge().stop();
+ count = 0;
+ }
}
else if ((5.0*ain.read())<1.0)
{
- PC.printf("Hit the limit switch??\n");
+ pc().printf("Hit the limit switch??\n");
hBridge().stop();
}
-
-
- /* buoyancy engine potentiometer string snaps */
+ //string snaps
else if (pvf().getVelocity() > 100)
{
PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY
//hBridge().stop();
//PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity());
- PC.printf("********** String broke? *********\n");
+ PC.printf("********** String broke? *********\n");
}
-
- if (debug_mode)
- PC.printf("+");
- //PC.printf("DEBUG: End of while loop?\n");
- } //end of while loop
+
+ }
}
\ No newline at end of file