FSG
/
7_26_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 System mbed
Fork of
7_21_17_FSG
by 
FSG
Diff: main.cpp
- Revision:
- 5:7421776f6b08
- Parent:
- 4:3c22d85a94a8
- Child:
- 6:ce2cf7f4d7d5
diff -r 3c22d85a94a8 -r 7421776f6b08 main.cpp
--- a/main.cpp Fri Jul 07 21:23:19 2017 +0000
+++ b/main.cpp Mon Jul 17 14:28:53 2017 +0000
@@ -8,6 +8,8 @@
#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)
@@ -38,7 +40,7 @@
Ticker PID_ticker; //new 6/14/17
Ticker LA_ticker; //new 6/22/17
-float positionCmd = 250.0;
+float positionCmd = 200.0; //250
/* ************************************************************************* */
@@ -57,23 +59,31 @@
bool motor_retracting = false;
bool motor_extending = false;
+// 7/10/17
+string actual_position_string = "";
+double double_actual_position = 0.00;
+
void IMU_ticking()
{
led1 = !led1; //flash the IMU LED
-
- //PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
- // PC.printf("\t pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint);
+
+ if (debug_mode)
+ PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
}
void PRESSURE_ticking()
{
- //PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin
+ if (debug_mode)
+ PC.printf("ressure: %f psi \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin
//this voltage has been checked and scaled properly (6/28/2017)
}
void BCE_ticking() //new 6/5/17
{
- 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());
+ 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());
}
int main()
@@ -83,7 +93,9 @@
//got screwy when i changed it
hBridge().stop();
- PC.printf("Linear Actuator PID Program Started 6/22/17\n");
+ PC.printf("* * * * * * * * * * * * * * * * *\n");
+ PC.printf("PV TEST PROGRAM STARTED 7/13/2017\n");
+ PC.printf("* * * * * * * * * * * * * * * * *\n");
systemTime().start(); //start the timer, needed for PID loop
@@ -117,17 +129,21 @@
float la_step = 1.0;
float la_setPoint_temp = 0.0;
- bool BCE_auto = true;
- bool LA_auto = true;
+ //Start off in manual mode for testing.
+ bool BCE_auto = false;
+ bool LA_auto = false;
float bce_auto_step_raw = 1.0;
float bce_auto_step_l;
- float convert = 10000;
+ //float convert = 10000;
+ float convert = 1;
float bce_auto_step_ml = bce_auto_step_raw * convert;
- int bce_man_step = 1;
- float volume_bce = 90.0*convert;
+ int bce_manual_step = 10;
+ //float volume_bce = 90.0*convert; //Troy: Not sure I get the conversion
+ float volume_bce = 0;
float positionCmd_temp;
- float ml_to_l= 0.000000001;
+ //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);
@@ -135,25 +151,27 @@
posCon().setPgain(P);
posCon().setIgain(I);
posCon().setDgain(D);
- //posCon().writeSetPoint(positionCmd);
+ posCon().writeSetPoint(positionCmd); //7/13/17 initialize the position of the motor to 200
+
+ PC.printf("BCE Test Program Started!\n");
+ wait(1);
/* *************************** LED *************************** */
led1 = 1; //initial values
led2 = 1;
- led3 = 1;
+ led3 = 0;
led4 = 1;
/* *************************** LED *************************** */
-
- PC.printf("Program Started 6/5/17\n");
- int cases = 0;
+
+ int la_cases = 0;
int count_while = 0;
//hBridge().reset();
PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n");
//PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
- /* *************************** Potentiometer *************************** */
+ /* ************************** Pressure Sensor ************************** */
PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
- /* *************************** Potentiometer *************************** */
+ /* ************************** Pressure Sensor ************************** */
/* *************************** MOTOR CONTROLLER *************************** */
//Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
@@ -166,15 +184,25 @@
/* *************************** BCE *************************** */
//float previous_positionCmd = -1;
- BCE_ticker.attach(&BCE_ticking, 3.0);
+ //BCE_ticker.attach(&BCE_ticking, 3.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?
- //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?
+
+ if (debug_mode)
+ PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint);
/* *************************** IMU *************************** */
/* Buoyancy Engine */
@@ -190,15 +218,13 @@
//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;
- //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG
+ led4 = !led4; //read indicator changes
+ Key=PC.getc();
- 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");
@@ -207,18 +233,47 @@
wait(0.5); //500 milliseconds
mbed_reset(); //reset the mbed!
}
- else if(Key =='H') //homing sequence
+
+ 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
{
PC.printf("### homing the device ###");
BLA_object.Keyboard_H();
wait(5); //for debugging
- PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str()); //flip this back and forth
+ ////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);
+
wait(1); //for debugging
}
- else if(Key=='p' or Key == 'P')
+ else if(Key == 'p' or Key == 'P')
{
- PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str()); //flip this back and forth
+ // 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
wait(1); //for debugging
// "-999999" means it is not working
}
@@ -233,7 +288,7 @@
}
else
{
- PC.printf("BCE: Still in Manual Mode\n");
+ PC.printf("BCE: Still in Automatic Mode\n");
}
}
else if (Key == '.' or Key == '>')
@@ -245,7 +300,7 @@
}
else
{
- PC.printf("BCE: Still in Automatic Mode\n");
+ PC.printf("BCE: Still in Manual Mode\n");
}
}
//BCE Automatic Controls
@@ -264,10 +319,18 @@
}
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 %7.0f milliliters\n", bce_auto_step_ml);
+ PC.printf("BCE Auto Step Size Now\n BE_ST_ML: %7.0f milliliters\n", bce_auto_step_ml);
}
else
{
@@ -275,12 +338,23 @@
}
}
- else if(Key =='d' or Key == 'D')
+ 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)
{
- volume_bce -= bce_auto_step_ml;
- PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB
+ 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;
+ }
}
else
{
@@ -289,10 +363,21 @@
}
else if(Key == 'f' or Key == 'F')
{
+ PC.printf("(f) volume_bce: %f\n", volume_bce);
if (BCE_auto == true)
{
- volume_bce += bce_auto_step_ml;
- PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB
+ 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;
+ }
}
else
{
@@ -304,19 +389,19 @@
if (BCE_auto == true)
{
PC.printf("\nR received!\n");
- positionCmd=(volume_bce-(pi*40.64*40.64*377))/(-1*pi*40.64*40.64);
- //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());
+
+ //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
- hBridge().reset();
+ hBridge().reset(); //reset to start this process of moving the h-bridge
- count = 0;
+ count = 0; //not sure...
}
else
{
@@ -329,63 +414,88 @@
if (BCE_auto == false)
{
PC.printf("BCE Manual Step Size Change\n");
- if (bce_man_step == 1)
+ if (bce_manual_step == 1)
{
- bce_man_step = 10;
+ bce_manual_step = 10;
}
- else if (bce_man_step == 10)
+ else if (bce_manual_step == 10)
{
- bce_man_step = 25;
+ bce_manual_step = 25;
}
- else if (bce_man_step == 25)
+ else if (bce_manual_step == 25)
{
- bce_man_step = 50;
+ bce_manual_step = 50;
}
- else if (bce_man_step == 50)
+ else if (bce_manual_step == 50)
{
- bce_man_step = 1;
+ bce_manual_step = 1;
}
- PC.printf("BCE Manual Step Size Now %d\n", bce_man_step);
+ 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;
+ }
}
else
{
PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
}
}
- else if (Key == 'z' or Key =='Z')
- {
- if (BCE_auto == false)
- {
- //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
- {
- PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
- }
- }
- else if (Key == 'x' or Key == 'X')
- {
- if (BCE_auto == false)
- {
- //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
- {
- PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
- }
- }
- else if(Key=='w' or Key =='W')
+ else if(Key == 'w' or Key == 'W')
{
if (BCE_auto == false)
{
PC.printf("\nW received!\n");
- PC.printf("BASETP: %3.0f\n", positionCmd);
- posCon().writeSetPoint(positionCmd);
+ PC.printf("BEM_SND: %3.0f\n", positionCmd);
+
+ posCon().writeSetPoint(positionCmd); //writing once doesn't work sometimes
+
//posCon().setPgain(P);
//posCon().setIgain(I);
//posCon().setDgain(D);
@@ -412,8 +522,8 @@
else
{
LA_auto = true;
- PC.printf("```````````Now in IMU Controlled Mode```````````````\n");
- cases = 0;
+ PC.printf("```````````LA now in IMU (Auto) Controlled Mode```````````````\n");
+ la_cases = 0;
count_while = 0;
}
}
@@ -423,76 +533,106 @@
{
LA_auto = false;
//Change cases: go from imu controlled to manual
- PC.printf("```````````Now in Manual Mode````````````````````\n");
- cases = 1;
+ PC.printf("```````````LA now in Manual Mode````````````````````\n");
+ la_cases = 1;
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 == ')')
{
- PC.printf(") recieved\n");
- if (la_step == 0.5)
+ if (LA_auto == true)
{
- la_step = 1.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);
}
- else if (la_step == 1.0)
- {
- la_step = 5.0;
- }
- else if (la_step == 5.0)
- {
- la_step = 10.0;
+ else
+ {
+ PC.printf("ERROR: LA in manual mode!\n");
}
- else if (la_step == 10.0)
+ }
+
+ else if (Key == '-' or Key == '_')
+ {
+ if (LA_auto == true)
{
- la_step = 15.0;
+ la_setPoint_temp -= la_step; //IMU_pitch_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);
}
- else if (la_step == 15.0)
+ else
{
- la_step = 0.5;
+ PC.printf("ERROR: LA in manual mode!\n");
}
- PC.printf("LA Step Size Now %f\n", la_step);
}
-
-
+ else if (Key == '=' or Key == '+')
+ {
+ if (LA_auto == true)
+ {
+ la_setPoint_temp += la_step; //IMU_pitch_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);
+ }
+ else
+ {
+ PC.printf("ERROR: LA in manual mode!\n");
+ }
+ }
else if (Key == 'A' or Key == 'a')
{
- PC.printf("A recieved\n");
- la_setPoint=la_setPoint_temp;
- PC.printf("LA angle now set to %f\n", la_setPoint);
- }
- else if (Key=='-' or Key == '_')
- {
- la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
- PC.printf("- recieved\n");
- PC.printf("LA angle changed to: %d la_step: %f\n", la_setPoint_temp, la_step);
- }
- else if (Key =='=' or Key == '+')
- {
- la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0;
- PC.printf("+ recieved\n");
- PC.printf("LA angle changed to: %f\n", la_setPoint_temp);
+ if (LA_auto == true)
+ {
+ PC.printf("A recieved\n");
+ la_setPoint=la_setPoint_temp;
+ PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint);
+ }
+ else
+ {
+ PC.printf("ERROR: LA in manual mode!\n");
+ }
}
- else if (Key=='[' or Key == '{')
+ 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 == '}')
+ 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==';')
+ else if (Key == ';')
{
la_I_gain -= 0.1;
PC.printf("; key pressed\n");
@@ -500,19 +640,19 @@
}
- else if (Key=='\'')
+ else if (Key == '\'')
{
la_I_gain += 0.1;
- PC.printf("\ key pressed\n");
+ PC.printf("\\ key pressed\n");
PC.printf("I gain is now %f\n", la_I_gain);
}
- else if (Key=='.')
+ 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=='/')
+ else if (Key == '/')
{
la_D_gain += 0.1;
PC.printf("/ key pressed\n");
@@ -521,23 +661,60 @@
else if(Key == 'n' or Key == 'N')
{
- PC.printf("N key pressed. \n");
- PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
+ if (LA_auto == false)
+ {
+ PC.printf("N key pressed. \n");
+ PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
+ }
+
+ else
+ {
+ PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
+ }
}
else if(Key == 'm' or Key == 'M')
{
- PC.printf("M key pressed. \n");
- PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());
+ if (LA_auto == false)
+ {
+ PC.printf("M key pressed. \n");
+ PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());
+ }
+
+ else
+ {
+ PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
+ }
}
else if(Key == 'j' or Key == 'J')
{
- PC.printf("J key pressed. \n");
- PC.printf("%s\n", BLA_object.Keyboard_A());
+ if (LA_auto == false)
+ {
+ PC.printf("J key pressed. \n");
+ PC.printf("%s\n", BLA_object.Keyboard_A());
+ }
+
+ else
+ {
+ PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
+ }
}
else if(Key == 'k' or Key == 'K')
{
- PC.printf("K key pressed. \n");
- PC.printf("%s\n", BLA_object.Keyboard_D());
+ if (LA_auto == false)
+ {
+ PC.printf("K key pressed. \n");
+ PC.printf("%s\n", BLA_object.Keyboard_D());
+ }
+
+ else
+ {
+ 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());
}
@@ -550,16 +727,36 @@
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();
- }
+ }
+
+ //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 (cases==0)
+ if (LA_auto == false)
{
- //PC.printf("Case is 0; IMU control\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)
+ 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");
+ }
}
- else if (cases==1)
+
+ //change between automatic and manual mode of linear actuator? Troy: 7/11/2017
+
+ if (la_cases==0)
+ {
+ 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();
+ }
+ else if (la_cases==1)
{
while (count_while==0)
@@ -574,41 +771,39 @@
}
}
- /*if(BLA_object.MC_readable()) //if you can read the motor controller do this...
- {
- //PC.printf("BATTERY LINEAR ACTUATOR");
-
-
- //PC.printf("Motor Controller response:\n");
-// while(MC.readable())
-// {
-// PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc)
-// wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
-// }
- }*/
if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
{
count ++;
//pc().printf("We have a small issue\n");
- if(count==10)
- {
- pc().printf("Bad pot issue\n");
- //hBridge().stop();
- }
+ //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
+ }
}
else if ((5.0*ain.read())<1.0)
{
- pc().printf("Hit the limit switch??\n");
+ PC.printf("Hit the limit switch??\n");
hBridge().stop();
}
+
+
- //string snaps
+ /* buoyancy engine potentiometer 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("********** String broke? *********\n");
+ //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity());
+ 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