7_26_17_FSG

FSG » Code » 7_26_17_FSG

Dependencies: BCEmotor Battery_Linear_Actuator Controller_ IMU_code_ LTC1298_7_14 MODSERIAL PosVelFilter System mbed

Fork of 7_21_17_FSG by FSG

main.cpp@11:b5cc98f154a4, 2017-07-27 (annotated)

Committer:: mdavis30
Date:: Thu Jul 27 16:31:21 2017 +0000
Revision:: 11:b5cc98f154a4
Parent:: 10:68c3b3f9dc50
Child:: 12:fa9f84f2967f

Who changed what in which revision?

User	Revision	Line number	New 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	10:68c3b3f9dc50	5	//new line to get the publish working
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	8:9b9431404db7	20	float input_num;
mdavis30	0:381a84fad08b	21
mdavis30	0:381a84fad08b	22	string IMU_STRING = "";
mdavis30	0:381a84fad08b	23
mdavis30	0:381a84fad08b	24
mdavis30	0:381a84fad08b	25	DigitalOut led1(LED1);
mdavis30	0:381a84fad08b	26	DigitalOut led2(LED2);
mdavis30	0:381a84fad08b	27	DigitalOut led3(LED3);
mdavis30	0:381a84fad08b	28	DigitalOut led4(LED4);
mdavis30	0:381a84fad08b	29
mdavis30	6:ce2cf7f4d7d5	30	//Pin for limit switch for buoyancy engine
mdavis30	2:c3cb3ea3c9fa	31	AnalogIn ain(p18);
mdavis30	0:381a84fad08b	32
mdavis30	0:381a84fad08b	33	/* ********** These tickers work independent of any while loops ******** */
mdavis30	0:381a84fad08b	34	Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker
mdavis30	0:381a84fad08b	35	Ticker BE_position_ticker; //probably delete soon
mdavis30	0:381a84fad08b	36	Ticker PRESSURE_ticker;
mdavis30	0:381a84fad08b	37
mdavis30	0:381a84fad08b	38	Ticker BCE_ticker; //new 6/5/17
mdavis30	3:1257a7d2eb3a	39	Ticker PID_ticker; //new 6/14/17
mdavis30	3:1257a7d2eb3a	40	Ticker LA_ticker; //new 6/22/17
mdavis30	0:381a84fad08b	41	/* ************************************************************************* */
mdavis30	0:381a84fad08b	42
mdavis30	6:ce2cf7f4d7d5	43	//Set beginning position for buoyancy and linear actuator
mdavis30	6:ce2cf7f4d7d5	44	float positionCmd = 190.0;
mdavis30	4:3c22d85a94a8	45	float pi = 3.14159265359;
mdavis30	6:ce2cf7f4d7d5	46	float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint)
mdavis30	4:3c22d85a94a8	47
mdavis30	3:1257a7d2eb3a	48	/* PID LOOP STUFF */
mdavis30	3:1257a7d2eb3a	49	float la_P_gain = 1.0;
mdavis30	3:1257a7d2eb3a	50	float la_I_gain = 0.00;
mdavis30	3:1257a7d2eb3a	51	float la_D_gain = 0.00;
mdavis30	3:1257a7d2eb3a	52	/* PID LOOP STUFF */
mdavis30	3:1257a7d2eb3a	53
mdavis30	8:9b9431404db7	54	int count_while = 0;
mdavis30	8:9b9431404db7	55
mdavis30	6:ce2cf7f4d7d5	56	float IMU_pitch_angle;
mdavis30	6:ce2cf7f4d7d5	57	double double_actual_position = 0.00;
mdavis30	3:1257a7d2eb3a	58
mdavis30	6:ce2cf7f4d7d5	59	string MC_readable_string = "";
tnhnrl	5:7421776f6b08	60
mdavis30	8:9b9431404db7	61	int IMU_count = 0;
mdavis30	8:9b9431404db7	62	int Pr_count = 0;
mdavis30	8:9b9431404db7	63	int BCE_count = 0;
mdavis30	8:9b9431404db7	64
mdavis30	0:381a84fad08b	65	void IMU_ticking()
mdavis30	0:381a84fad08b	66	{
mdavis30	6:ce2cf7f4d7d5	67	//led1 = !led1; //flash the IMU LED
mdavis30	6:ce2cf7f4d7d5	68
mdavis30	6:ce2cf7f4d7d5	69	//PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
mdavis30	6:ce2cf7f4d7d5	70	PC.printf("%s\n", IMU_STRING.c_str());
mdavis30	0:381a84fad08b	71	}
mdavis30	0:381a84fad08b	72
mdavis30	0:381a84fad08b	73	void PRESSURE_ticking()
mdavis30	0:381a84fad08b	74	{
mdavis30	8:9b9431404db7	75	PC.printf("pressure: %f psi \n", (0.00122(adc().ch1_filt)9.9542)-0.0845); //read the analog pin
mdavis30	3:1257a7d2eb3a	76	//this voltage has been checked and scaled properly (6/28/2017)
mdavis30	0:381a84fad08b	77	}
mdavis30	0:381a84fad08b	78
mdavis30	0:381a84fad08b	79	void BCE_ticking() //new 6/5/17
mdavis30	0:381a84fad08b	80	{
mdavis30	6:ce2cf7f4d7d5	81	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	82	}
mdavis30	0:381a84fad08b	83
mdavis30	8:9b9431404db7	84
mdavis30	0:381a84fad08b	85	int main()
mdavis30	0:381a84fad08b	86	{
mdavis30	6:ce2cf7f4d7d5	87	PC.baud(9600); //mbed to PC serial connection speed
mdavis30	6:ce2cf7f4d7d5	88	hBridge().stop(); //Stop the buoyancy engine from moving
mdavis30	3:1257a7d2eb3a	89	systemTime().start(); //start the timer, needed for PID loop
mdavis30	3:1257a7d2eb3a	90
mdavis30	3:1257a7d2eb3a	91	/* ************** Linear Actuator MOTOR CONTROLLER ************** */
mdavis30	3:1257a7d2eb3a	92	Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
mdavis30	3:1257a7d2eb3a	93
mdavis30	3:1257a7d2eb3a	94	PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
mdavis30	3:1257a7d2eb3a	95
mdavis30	3:1257a7d2eb3a	96	PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
mdavis30	3:1257a7d2eb3a	97	wait(1);
mdavis30	3:1257a7d2eb3a	98	PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
mdavis30	3:1257a7d2eb3a	99	wait(1);
mdavis30	3:1257a7d2eb3a	100
mdavis30	0:381a84fad08b	101	//setup and start the adc. This runs on a fixed interval and is interrupt driven
mdavis30	0:381a84fad08b	102	adc().initialize();
mdavis30	0:381a84fad08b	103	adc().start();
mdavis30	0:381a84fad08b	104
mdavis30	0:381a84fad08b	105	//Initialize the position velocity filter. This will consume a couple of seconds for
mdavis30	0:381a84fad08b	106	//the filter to converge
mdavis30	0:381a84fad08b	107	pvf().init();
mdavis30	0:381a84fad08b	108
mdavis30	6:ce2cf7f4d7d5	109	//NEW 7/21 Homing the motor
mdavis30	6:ce2cf7f4d7d5	110	PC.printf("### homing the device ###");
mdavis30	6:ce2cf7f4d7d5	111	BLA_object.Keyboard_H();
mdavis30	6:ce2cf7f4d7d5	112	wait(10); //for debugging
mdavis30	6:ce2cf7f4d7d5	113
mdavis30	6:ce2cf7f4d7d5	114
mdavis30	0:381a84fad08b	115	float P = 0.10;
mdavis30	0:381a84fad08b	116	float I = 0.00;
mdavis30	0:381a84fad08b	117	float D = 0.00;
mdavis30	0:381a84fad08b	118	float count = 0.0;
mdavis30	6:ce2cf7f4d7d5	119	float positionCmd_cm;
mdavis30	0:381a84fad08b	120
mdavis30	4:3c22d85a94a8	121	float la_step = 1.0;
mdavis30	4:3c22d85a94a8	122	float la_setPoint_temp = 0.0;
mdavis30	4:3c22d85a94a8	123
mdavis30	6:ce2cf7f4d7d5	124	bool BCE_auto = true;
tnhnrl	5:7421776f6b08	125	bool LA_auto = false;
mdavis30	4:3c22d85a94a8	126
mdavis30	6:ce2cf7f4d7d5	127	float bce_auto_step = 0.1;
mdavis30	6:ce2cf7f4d7d5	128	float volume_bce = 1.0;
mdavis30	6:ce2cf7f4d7d5	129	int bce_man_step = 50;
mdavis30	8:9b9431404db7	130
mdavis30	8:9b9431404db7	131	float ticker_step_size = 1.0;
mdavis30	0:381a84fad08b	132
mdavis30	0:381a84fad08b	133	//set the intial gains for the position controller
mdavis30	8:9b9431404db7	134	posCon().setPgain(P);
mdavis30	0:381a84fad08b	135	posCon().setIgain(I);
mdavis30	0:381a84fad08b	136	posCon().setDgain(D);
mdavis30	6:ce2cf7f4d7d5	137	posCon().writeSetPoint(positionCmd);
mdavis30	6:ce2cf7f4d7d5	138	hBridge().reset();
mdavis30	8:9b9431404db7	139	//hBridge().run(motor_cmd);
mdavis30	8:9b9431404db7	140
mdavis30	0:381a84fad08b	141	/* ************************* LED ************************* */
mdavis30	0:381a84fad08b	142	led1 = 1; //initial values
mdavis30	0:381a84fad08b	143	led2 = 1;
mdavis30	6:ce2cf7f4d7d5	144	led3 = 1;
mdavis30	0:381a84fad08b	145	led4 = 1;
mdavis30	0:381a84fad08b	146	/* ************************* LED ************************* */
mdavis30	6:ce2cf7f4d7d5	147
mdavis30	8:9b9431404db7	148	IMU_code IMU_object;
mdavis30	0:381a84fad08b	149
mdavis30	0:381a84fad08b	150	while(1)
mdavis30	0:381a84fad08b	151	{
mdavis30	0:381a84fad08b	152	/* ************************* IMU ************************* */
mdavis30	0:381a84fad08b	153	IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
mdavis30	6:ce2cf7f4d7d5	154	IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?
mdavis30	6:ce2cf7f4d7d5	155	//PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_yaw_angle, la_setPoint);
mdavis30	0:381a84fad08b	156	/* ************************* IMU ************************* */
mdavis30	0:381a84fad08b	157
mdavis30	0:381a84fad08b	158	/* Buoyancy Engine */
mdavis30	0:381a84fad08b	159	// update the position velocity filter
mdavis30	0:381a84fad08b	160	pvf().update();
mdavis30	0:381a84fad08b	161
mdavis30	0:381a84fad08b	162	//update the controller with the current numbers in the position guesser
mdavis30	0:381a84fad08b	163	posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
mdavis30	0:381a84fad08b	164	hBridge().run(posCon().getOutput());
mdavis30	0:381a84fad08b	165
mdavis30	0:381a84fad08b	166	/* Buoyancy Engine */
mdavis30	0:381a84fad08b	167
mdavis30	0:381a84fad08b	168	//FOR DEBUGGING
mdavis30	0:381a84fad08b	169	//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	170
mdavis30	6:ce2cf7f4d7d5	171	//PC.printf("WHILE LOOP\n"); //DEBUG
mdavis30	0:381a84fad08b	172	if (PC.readable())
mdavis30	0:381a84fad08b	173	{
mdavis30	6:ce2cf7f4d7d5	174	//led4 != led4;
mdavis30	6:ce2cf7f4d7d5	175	//PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG
mdavis30	0:381a84fad08b	176
mdavis30	6:ce2cf7f4d7d5	177	Key=PC.getc();
mdavis30	4:3c22d85a94a8	178	//Universal MBED Controls
mdavis30	6:ce2cf7f4d7d5	179	if(Key=='!') //RESET THE MBED
mdavis30	0:381a84fad08b	180	{
mdavis30	4:3c22d85a94a8	181	PC.printf("MBED RESET KEY (!) PRESSED\n");
mdavis30	4:3c22d85a94a8	182	PC.printf("Linear Actuator Motor disabled!\n");
mdavis30	4:3c22d85a94a8	183	//disable the motor
mdavis30	4:3c22d85a94a8	184	BLA_object.Keyboard_Q(); //DISABLE THE MOTOR
mdavis30	4:3c22d85a94a8	185	wait(0.5); //500 milliseconds
mdavis30	4:3c22d85a94a8	186	mbed_reset(); //reset the mbed!
mdavis30	0:381a84fad08b	187	}
mdavis30	9:2dcfd6ce61ea	188	/* else if(Key == '[')
mdavis30	9:2dcfd6ce61ea	189	{
mdavis30	9:2dcfd6ce61ea	190	BLA_object.velocity_only(-100); //send speed -100 command to linear actuator (retract)
mdavis30	9:2dcfd6ce61ea	191	}
mdavis30	9:2dcfd6ce61ea	192	else if(Key == ']')
mdavis30	9:2dcfd6ce61ea	193	{
mdavis30	9:2dcfd6ce61ea	194	BLA_object.velocity_only(100); //send speed +100 command to linear actuator (extend)
mdavis30	9:2dcfd6ce61ea	195	}
mdavis30	9:2dcfd6ce61ea	196	else if(Key == 'u')
mdavis30	9:2dcfd6ce61ea	197	{
mdavis30	9:2dcfd6ce61ea	198	BLA_object.Keyboard_U(); //stop linear actuator
mdavis30	9:2dcfd6ce61ea	199	} */
mdavis30	6:ce2cf7f4d7d5	200	else if(Key =='H') //homing sequence
mdavis30	0:381a84fad08b	201	{
mdavis30	4:3c22d85a94a8	202	PC.printf("### homing the device ###");
mdavis30	4:3c22d85a94a8	203	BLA_object.Keyboard_H();
mdavis30	6:ce2cf7f4d7d5	204	wait(10); //for debugging
mdavis30	4:3c22d85a94a8	205
tnhnrl	5:7421776f6b08	206	const char *char_actual_position = BLA_object.get_pos().c_str();
tnhnrl	5:7421776f6b08	207	//actual_position_string = BLA_object.get_pos().c_str();
tnhnrl	5:7421776f6b08	208
tnhnrl	5:7421776f6b08	209	sscanf(char_actual_position, "%lf", &double_actual_position);
tnhnrl	5:7421776f6b08	210	// 7/10/17
tnhnrl	5:7421776f6b08	211
mdavis30	6:ce2cf7f4d7d5	212	PC.printf("LA actual position: %lf \n", double_actual_position);
mdavis30	6:ce2cf7f4d7d5	213
mdavis30	6:ce2cf7f4d7d5	214	//TROY NOTES on Linear Actuator commands:
mdavis30	6:ce2cf7f4d7d5	215	//"pos" returns integer value "66813 for example"
mdavis30	6:ce2cf7f4d7d5	216	//"hosp-100" is the setting of the motor, it retracts at 100 rpm
mdavis30	6:ce2cf7f4d7d5	217	//"gohoseq" to home it, it will not give you feedback
mdavis30	6:ce2cf7f4d7d5	218	//print the position after you do this to see where the LA is
mdavis30	6:ce2cf7f4d7d5	219	}
mdavis30	6:ce2cf7f4d7d5	220	else if(Key=='p' or Key == 'P')
mdavis30	6:ce2cf7f4d7d5	221	{
mdavis30	11:b5cc98f154a4	222
tnhnrl	5:7421776f6b08	223	// 7/10/17
tnhnrl	5:7421776f6b08	224	//actual_position_string = BLA_object.get_pos();
tnhnrl	5:7421776f6b08	225	//actual_position_string = BLA_object.get_pos().c_str();
tnhnrl	5:7421776f6b08	226	//const char *char_actual_position = string_actual_position.c_str();
mdavis30	6:ce2cf7f4d7d5	227
tnhnrl	5:7421776f6b08	228	const char *char_actual_position = BLA_object.get_pos().c_str();
tnhnrl	5:7421776f6b08	229	//actual_position_string = BLA_object.get_pos().c_str();
tnhnrl	5:7421776f6b08	230
tnhnrl	5:7421776f6b08	231	sscanf(char_actual_position, "%lf", &double_actual_position);
tnhnrl	5:7421776f6b08	232	// 7/10/17
tnhnrl	5:7421776f6b08	233
mdavis30	6:ce2cf7f4d7d5	234	PC.printf("### L.A. position is\nLA_AP: %lf ###\n", double_actual_position); //flip this back and forth
mdavis30	6:ce2cf7f4d7d5	235
mdavis30	6:ce2cf7f4d7d5	236	if (double_actual_position > 180000)
mdavis30	6:ce2cf7f4d7d5	237	PC.printf(" <<<< REACHED MAXIMUM L.A. POSITION! >>>>\n");
mdavis30	6:ce2cf7f4d7d5	238	else if (double_actual_position < 0)
mdavis30	6:ce2cf7f4d7d5	239	PC.printf(" <<<< REACHED MINIMUM L.A. POSITION! >>>>\n");
mdavis30	6:ce2cf7f4d7d5	240
mdavis30	4:3c22d85a94a8	241	wait(1); //for debugging
mdavis30	6:ce2cf7f4d7d5	242	// "-999999" means it is not working
mdavis30	3:1257a7d2eb3a	243	}
mdavis30	8:9b9431404db7	244	else if (Key == '3' or Key == '#')
mdavis30	8:9b9431404db7	245	{
mdavis30	8:9b9431404db7	246	PC.printf("Ticker Sensor Step Size\n");
mdavis30	8:9b9431404db7	247	if (ticker_step_size == 1.0)
mdavis30	8:9b9431404db7	248	{
mdavis30	8:9b9431404db7	249	ticker_step_size = 5.0;
mdavis30	8:9b9431404db7	250	}
mdavis30	8:9b9431404db7	251	else if (ticker_step_size == 5.0)
mdavis30	8:9b9431404db7	252	{
mdavis30	8:9b9431404db7	253	ticker_step_size = 10.0;
mdavis30	8:9b9431404db7	254	}
mdavis30	8:9b9431404db7	255	else if (ticker_step_size == 10.0)
mdavis30	8:9b9431404db7	256	{
mdavis30	8:9b9431404db7	257	ticker_step_size = 20.0;
mdavis30	8:9b9431404db7	258	}
mdavis30	8:9b9431404db7	259	else if (ticker_step_size == 20.0)
mdavis30	8:9b9431404db7	260	{
mdavis30	8:9b9431404db7	261	ticker_step_size = 1.0;
mdavis30	8:9b9431404db7	262	}
mdavis30	8:9b9431404db7	263	PC.printf("Ticker Step Size is %f\n", ticker_step_size);
mdavis30	8:9b9431404db7	264	}
mdavis30	8:9b9431404db7	265	else if (Key =='4' or Key == '$')
mdavis30	8:9b9431404db7	266	{
mdavis30	8:9b9431404db7	267	if (IMU_count == 0)
mdavis30	8:9b9431404db7	268	{
mdavis30	8:9b9431404db7	269	PC.printf("IMU turned on! Ticker time is %f seconds\n", ticker_step_size);
mdavis30	8:9b9431404db7	270	IMU_ticker.attach(&IMU_ticking, ticker_step_size);
mdavis30	8:9b9431404db7	271	IMU_count = 1;
mdavis30	8:9b9431404db7	272	}
mdavis30	8:9b9431404db7	273	else if (IMU_count == 1)
mdavis30	8:9b9431404db7	274	{
mdavis30	8:9b9431404db7	275	PC.printf("IMU turned off!\n");
mdavis30	8:9b9431404db7	276	IMU_ticker.detach();
mdavis30	8:9b9431404db7	277	IMU_count = 0;
mdavis30	8:9b9431404db7	278	}
mdavis30	8:9b9431404db7	279
mdavis30	8:9b9431404db7	280	}
mdavis30	8:9b9431404db7	281	else if (Key == '5' or Key == '%')
mdavis30	8:9b9431404db7	282	{
mdavis30	8:9b9431404db7	283	if (Pr_count == 0)
mdavis30	8:9b9431404db7	284	{
mdavis30	8:9b9431404db7	285	PC.printf("Pressure turned on! Ticker time is %f seconds\n", ticker_step_size);
mdavis30	8:9b9431404db7	286	PRESSURE_ticker.attach(&PRESSURE_ticking, ticker_step_size);
mdavis30	8:9b9431404db7	287	Pr_count = 1;
mdavis30	8:9b9431404db7	288	}
mdavis30	8:9b9431404db7	289	else if (Pr_count == 1)
mdavis30	8:9b9431404db7	290	{
mdavis30	8:9b9431404db7	291	PC.printf("Pressure turned off!\n");
mdavis30	8:9b9431404db7	292	PRESSURE_ticker.detach();
mdavis30	8:9b9431404db7	293	Pr_count = 0;
mdavis30	8:9b9431404db7	294	}
mdavis30	8:9b9431404db7	295	}
mdavis30	8:9b9431404db7	296	else if (Key == '6' or Key == '^')
mdavis30	8:9b9431404db7	297	{
mdavis30	8:9b9431404db7	298	if (BCE_count == 0)
mdavis30	8:9b9431404db7	299	{
mdavis30	8:9b9431404db7	300	PC.printf("BCE turned on! Ticker time is %f seconds\n", ticker_step_size);
mdavis30	8:9b9431404db7	301	BCE_ticker.attach(&BCE_ticking, ticker_step_size);
mdavis30	8:9b9431404db7	302	BCE_count = 1;
mdavis30	8:9b9431404db7	303	}
mdavis30	8:9b9431404db7	304	else if (BCE_count == 1)
mdavis30	8:9b9431404db7	305	{
mdavis30	8:9b9431404db7	306	PC.printf("BCE turned off!\n");
mdavis30	8:9b9431404db7	307	BCE_ticker.detach();
mdavis30	8:9b9431404db7	308	BCE_count = 0;
mdavis30	8:9b9431404db7	309	}
mdavis30	8:9b9431404db7	310	}
mdavis30	4:3c22d85a94a8	311	//Buoyancy Engine Controls
mdavis30	4:3c22d85a94a8	312	else if (Key == ',' or Key == '<')
mdavis30	4:3c22d85a94a8	313	{
mdavis30	4:3c22d85a94a8	314	if (BCE_auto == false)
mdavis30	4:3c22d85a94a8	315	{
mdavis30	4:3c22d85a94a8	316	PC.printf("BCE: Now in Automatic Mode\n");
mdavis30	4:3c22d85a94a8	317	BCE_auto = true;
mdavis30	4:3c22d85a94a8	318	}
mdavis30	4:3c22d85a94a8	319	else
mdavis30	4:3c22d85a94a8	320	{
tnhnrl	5:7421776f6b08	321	PC.printf("BCE: Still in Automatic Mode\n");
mdavis30	4:3c22d85a94a8	322	}
mdavis30	4:3c22d85a94a8	323	}
mdavis30	4:3c22d85a94a8	324	else if (Key == '.' or Key == '>')
mdavis30	4:3c22d85a94a8	325	{
mdavis30	4:3c22d85a94a8	326	if (BCE_auto == true)
mdavis30	4:3c22d85a94a8	327	{
mdavis30	4:3c22d85a94a8	328	PC.printf("BCE: Now in Manual Mode\n");
mdavis30	4:3c22d85a94a8	329	BCE_auto = false;
mdavis30	4:3c22d85a94a8	330	}
mdavis30	4:3c22d85a94a8	331	else
mdavis30	4:3c22d85a94a8	332	{
tnhnrl	5:7421776f6b08	333	PC.printf("BCE: Still in Manual Mode\n");
mdavis30	4:3c22d85a94a8	334	}
mdavis30	4:3c22d85a94a8	335	}
mdavis30	4:3c22d85a94a8	336	//BCE Automatic Controls
mdavis30	6:ce2cf7f4d7d5	337	else if(Key =='d' or Key == 'D')
mdavis30	4:3c22d85a94a8	338	{
mdavis30	4:3c22d85a94a8	339	if (BCE_auto == true)
mdavis30	4:3c22d85a94a8	340	{
mdavis30	6:ce2cf7f4d7d5	341	volume_bce -= bce_auto_step;
mdavis30	6:ce2cf7f4d7d5	342	PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB
mdavis30	4:3c22d85a94a8	343	}
mdavis30	4:3c22d85a94a8	344	else
mdavis30	4:3c22d85a94a8	345	{
mdavis30	4:3c22d85a94a8	346	PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30	4:3c22d85a94a8	347	}
mdavis30	4:3c22d85a94a8	348	}
mdavis30	4:3c22d85a94a8	349	else if(Key == 'f' or Key == 'F')
mdavis30	4:3c22d85a94a8	350	{
mdavis30	4:3c22d85a94a8	351	if (BCE_auto == true)
mdavis30	4:3c22d85a94a8	352	{
mdavis30	6:ce2cf7f4d7d5	353	volume_bce += bce_auto_step;
mdavis30	6:ce2cf7f4d7d5	354	PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB
mdavis30	4:3c22d85a94a8	355	}
mdavis30	4:3c22d85a94a8	356	else
mdavis30	4:3c22d85a94a8	357	{
mdavis30	4:3c22d85a94a8	358	PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30	4:3c22d85a94a8	359	}
mdavis30	4:3c22d85a94a8	360	}
mdavis30	4:3c22d85a94a8	361	else if(Key == 'r' or Key == 'R')
mdavis30	4:3c22d85a94a8	362	{
mdavis30	4:3c22d85a94a8	363	if (BCE_auto == true)
mdavis30	4:3c22d85a94a8	364	{
mdavis30	4:3c22d85a94a8	365	PC.printf("\nR received!\n");
mdavis30	6:ce2cf7f4d7d5	366	positionCmd_cm=(1000/(16pi))volume_bce;
mdavis30	6:ce2cf7f4d7d5	367	positionCmd = positionCmd_cm*10;
mdavis30	6:ce2cf7f4d7d5	368	//positionCmd= positionCmd_temp*0.000000001;
mdavis30	6:ce2cf7f4d7d5	369	//PC.printf("BCE engine going to position: %3.2f\n", positionCmd);
mdavis30	6:ce2cf7f4d7d5	370	PC.printf("\nBASETP: %3.0f\n", positionCmd);
mdavis30	6:ce2cf7f4d7d5	371	posCon().writeSetPoint(positionCmd);
mdavis30	6:ce2cf7f4d7d5	372	//posCon().setPgain(P);
mdavis30	6:ce2cf7f4d7d5	373	//posCon().setIgain(I);
mdavis30	6:ce2cf7f4d7d5	374	//posCon().setDgain(D);
mdavis30	6:ce2cf7f4d7d5	375	hBridge().run(posCon().getOutput());
mdavis30	6:ce2cf7f4d7d5	376
mdavis30	4:3c22d85a94a8	377
mdavis30	6:ce2cf7f4d7d5	378	count = 0;
mdavis30	4:3c22d85a94a8	379	}
mdavis30	4:3c22d85a94a8	380	else
mdavis30	4:3c22d85a94a8	381	{
mdavis30	4:3c22d85a94a8	382	PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30	4:3c22d85a94a8	383	}
mdavis30	4:3c22d85a94a8	384	}
mdavis30	4:3c22d85a94a8	385	//BCE Manual Controls
mdavis30	4:3c22d85a94a8	386	else if (Key == '2' or Key == '@')
mdavis30	4:3c22d85a94a8	387	{
mdavis30	4:3c22d85a94a8	388	if (BCE_auto == false)
mdavis30	4:3c22d85a94a8	389	{
mdavis30	4:3c22d85a94a8	390	PC.printf("BCE Manual Step Size Change\n");
mdavis30	6:ce2cf7f4d7d5	391	if (bce_man_step == 50)
mdavis30	4:3c22d85a94a8	392	{
mdavis30	6:ce2cf7f4d7d5	393	bce_man_step = 25;
mdavis30	4:3c22d85a94a8	394	}
mdavis30	6:ce2cf7f4d7d5	395	else if (bce_man_step == 25)
mdavis30	4:3c22d85a94a8	396	{
mdavis30	6:ce2cf7f4d7d5	397	bce_man_step = 10;
mdavis30	4:3c22d85a94a8	398	}
mdavis30	6:ce2cf7f4d7d5	399	else if (bce_man_step == 10)
mdavis30	4:3c22d85a94a8	400	{
mdavis30	6:ce2cf7f4d7d5	401	bce_man_step = 1;
mdavis30	4:3c22d85a94a8	402	}
mdavis30	6:ce2cf7f4d7d5	403	else if (bce_man_step == 1)
mdavis30	4:3c22d85a94a8	404	{
mdavis30	6:ce2cf7f4d7d5	405	bce_man_step = 50;
mdavis30	4:3c22d85a94a8	406	}
mdavis30	4:3c22d85a94a8	407
mdavis30	6:ce2cf7f4d7d5	408	PC.printf("BCE Manual Step Size Now %d\n", bce_man_step);
mdavis30	4:3c22d85a94a8	409	}
mdavis30	4:3c22d85a94a8	410	else
mdavis30	4:3c22d85a94a8	411	{
mdavis30	4:3c22d85a94a8	412	PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30	4:3c22d85a94a8	413	}
mdavis30	4:3c22d85a94a8	414	}
mdavis30	6:ce2cf7f4d7d5	415	else if (Key == 'z' or Key =='Z')
mdavis30	6:ce2cf7f4d7d5	416	{
mdavis30	6:ce2cf7f4d7d5	417	if (BCE_auto == false and positionCmd < 395)
mdavis30	6:ce2cf7f4d7d5	418	{
mdavis30	6:ce2cf7f4d7d5	419	//increment the duty cycle
mdavis30	6:ce2cf7f4d7d5	420	positionCmd -= bce_man_step;
mdavis30	6:ce2cf7f4d7d5	421	PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
mdavis30	6:ce2cf7f4d7d5	422	}
mdavis30	6:ce2cf7f4d7d5	423	else if (positionCmd > 395)
mdavis30	6:ce2cf7f4d7d5	424	{
mdavis30	6:ce2cf7f4d7d5	425	PC.printf("ERROR: Cannot move past 395 mm\n");
mdavis30	6:ce2cf7f4d7d5	426	positionCmd = 370;
mdavis30	6:ce2cf7f4d7d5	427	}
mdavis30	6:ce2cf7f4d7d5	428	else
mdavis30	6:ce2cf7f4d7d5	429	{
mdavis30	6:ce2cf7f4d7d5	430	PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30	6:ce2cf7f4d7d5	431	}
mdavis30	6:ce2cf7f4d7d5	432	}
mdavis30	6:ce2cf7f4d7d5	433	else if (Key == 'x' or Key == 'X')
mdavis30	6:ce2cf7f4d7d5	434	{
mdavis30	6:ce2cf7f4d7d5	435	if (BCE_auto == false and positionCmd > -25)
mdavis30	6:ce2cf7f4d7d5	436	{
mdavis30	6:ce2cf7f4d7d5	437	//decrement the duty cycle
mdavis30	6:ce2cf7f4d7d5	438	positionCmd += bce_man_step;
mdavis30	6:ce2cf7f4d7d5	439	PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
mdavis30	6:ce2cf7f4d7d5	440	}
mdavis30	6:ce2cf7f4d7d5	441	else if (positionCmd < -25)
mdavis30	6:ce2cf7f4d7d5	442	{
mdavis30	6:ce2cf7f4d7d5	443	PC.printf("ERROR: Cannot move past -5 mm\n");
mdavis30	6:ce2cf7f4d7d5	444	positionCmd = -5;
mdavis30	6:ce2cf7f4d7d5	445	}
mdavis30	6:ce2cf7f4d7d5	446	else
mdavis30	6:ce2cf7f4d7d5	447	{
mdavis30	6:ce2cf7f4d7d5	448	PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30	6:ce2cf7f4d7d5	449	}
mdavis30	6:ce2cf7f4d7d5	450	}
mdavis30	6:ce2cf7f4d7d5	451	else if(Key=='w' or Key =='W')
mdavis30	4:3c22d85a94a8	452	{
mdavis30	4:3c22d85a94a8	453	if (BCE_auto == false)
mdavis30	4:3c22d85a94a8	454	{
mdavis30	4:3c22d85a94a8	455	PC.printf("\nW received!\n");
mdavis30	6:ce2cf7f4d7d5	456	PC.printf("BASETP: %3.0f\n", positionCmd);
mdavis30	6:ce2cf7f4d7d5	457	posCon().writeSetPoint(positionCmd);
mdavis30	4:3c22d85a94a8	458	//posCon().setPgain(P);
mdavis30	4:3c22d85a94a8	459	//posCon().setIgain(I);
mdavis30	4:3c22d85a94a8	460	//posCon().setDgain(D);
mdavis30	4:3c22d85a94a8	461	hBridge().run(posCon().getOutput());
mdavis30	4:3c22d85a94a8	462
mdavis30	4:3c22d85a94a8	463	hBridge().reset();
mdavis30	4:3c22d85a94a8	464
mdavis30	4:3c22d85a94a8	465	count = 0;
mdavis30	4:3c22d85a94a8	466
mdavis30	4:3c22d85a94a8	467	}
mdavis30	4:3c22d85a94a8	468	else
mdavis30	4:3c22d85a94a8	469	{
mdavis30	4:3c22d85a94a8	470	PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30	4:3c22d85a94a8	471	}
mdavis30	4:3c22d85a94a8	472	}
mdavis30	6:ce2cf7f4d7d5	473	else if (Key == 'l' or Key == 'L')
mdavis30	8:9b9431404db7	474	PC.printf("DEBUG: String position? %f set position? %f velocity? %f (BCE active: %f)\n", pvf().getPosition(), positionCmd, pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
mdavis30	4:3c22d85a94a8	475
mdavis30	4:3c22d85a94a8	476	//Linear Actuator Controls
mdavis30	4:3c22d85a94a8	477	else if(Key == 'c' or Key == 'C')
mdavis30	4:3c22d85a94a8	478	{
mdavis30	4:3c22d85a94a8	479	if (LA_auto == true)
mdavis30	4:3c22d85a94a8	480	{
mdavis30	4:3c22d85a94a8	481	PC.printf("ERROR: LA already in auto mode\n");
mdavis30	4:3c22d85a94a8	482	}
mdavis30	4:3c22d85a94a8	483	else
mdavis30	4:3c22d85a94a8	484	{
mdavis30	4:3c22d85a94a8	485	LA_auto = true;
mdavis30	6:ce2cf7f4d7d5	486	PC.printf("```````````Now in IMU Controlled Mode```````````````\n");
mdavis30	4:3c22d85a94a8	487	count_while = 0;
mdavis30	4:3c22d85a94a8	488	}
mdavis30	4:3c22d85a94a8	489	}
mdavis30	4:3c22d85a94a8	490	else if (Key == 'v' or Key == 'V')
mdavis30	4:3c22d85a94a8	491	{
mdavis30	4:3c22d85a94a8	492	if (LA_auto == true)
mdavis30	4:3c22d85a94a8	493	{
mdavis30	4:3c22d85a94a8	494	LA_auto = false;
mdavis30	6:ce2cf7f4d7d5	495	//go from imu controlled to manual
mdavis30	6:ce2cf7f4d7d5	496	PC.printf("```````````Now in Manual Mode````````````````````\n");
mdavis30	4:3c22d85a94a8	497	count_while = 0;
mdavis30	4:3c22d85a94a8	498	}
mdavis30	4:3c22d85a94a8	499	else
mdavis30	4:3c22d85a94a8	500	{
mdavis30	4:3c22d85a94a8	501	PC.printf("ERROR: LA already in manual mode\n");
mdavis30	4:3c22d85a94a8	502	}
mdavis30	4:3c22d85a94a8	503	}
mdavis30	4:3c22d85a94a8	504	else if (Key == '0' or Key == ')')
mdavis30	4:3c22d85a94a8	505	{
mdavis30	6:ce2cf7f4d7d5	506	PC.printf(") recieved\n");
mdavis30	6:ce2cf7f4d7d5	507	if (la_step == 0.5)
mdavis30	6:ce2cf7f4d7d5	508	{
mdavis30	6:ce2cf7f4d7d5	509	la_step = 1.0;
mdavis30	6:ce2cf7f4d7d5	510	}
mdavis30	6:ce2cf7f4d7d5	511	else if (la_step == 1.0)
mdavis30	6:ce2cf7f4d7d5	512	{
mdavis30	6:ce2cf7f4d7d5	513	la_step = 5.0;
mdavis30	6:ce2cf7f4d7d5	514	}
mdavis30	6:ce2cf7f4d7d5	515	else if (la_step == 5.0)
mdavis30	4:3c22d85a94a8	516	{
mdavis30	6:ce2cf7f4d7d5	517	la_step = 10.0;
mdavis30	6:ce2cf7f4d7d5	518	}
mdavis30	6:ce2cf7f4d7d5	519	else if (la_step == 10.0)
mdavis30	6:ce2cf7f4d7d5	520	{
mdavis30	6:ce2cf7f4d7d5	521	la_step = 15.0;
mdavis30	4:3c22d85a94a8	522	}
mdavis30	6:ce2cf7f4d7d5	523	else if (la_step == 15.0)
mdavis30	6:ce2cf7f4d7d5	524	{
mdavis30	6:ce2cf7f4d7d5	525	la_step = 0.5;
mdavis30	4:3c22d85a94a8	526	}
mdavis30	6:ce2cf7f4d7d5	527	PC.printf("LA Step Size Now %f\n", la_step);
tnhnrl	5:7421776f6b08	528	}
mdavis30	6:ce2cf7f4d7d5	529
mdavis30	6:ce2cf7f4d7d5	530	else if (Key=='-' or Key == '_')
tnhnrl	5:7421776f6b08	531	{
tnhnrl	5:7421776f6b08	532	if (LA_auto == true)
mdavis30	4:3c22d85a94a8	533	{
mdavis30	6:ce2cf7f4d7d5	534	la_setPoint_temp -= la_step; //IMU_yaw_angle -= 1.0;
tnhnrl	5:7421776f6b08	535	PC.printf("- recieved\n");
tnhnrl	5:7421776f6b08	536	PC.printf("LA auto step size: %f\n", la_step);
tnhnrl	5:7421776f6b08	537	PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
mdavis30	4:3c22d85a94a8	538	}
tnhnrl	5:7421776f6b08	539	else
mdavis30	4:3c22d85a94a8	540	{
tnhnrl	5:7421776f6b08	541	PC.printf("ERROR: LA in manual mode!\n");
mdavis30	4:3c22d85a94a8	542	}
mdavis30	4:3c22d85a94a8	543	}
mdavis30	6:ce2cf7f4d7d5	544
mdavis30	6:ce2cf7f4d7d5	545	else if (Key =='=' or Key == '+')
tnhnrl	5:7421776f6b08	546	{
tnhnrl	5:7421776f6b08	547	if (LA_auto == true)
tnhnrl	5:7421776f6b08	548	{
mdavis30	6:ce2cf7f4d7d5	549	la_setPoint_temp += la_step; //IMU_yaw_angle += 1.0;
tnhnrl	5:7421776f6b08	550	PC.printf("+ recieved\n");
tnhnrl	5:7421776f6b08	551	PC.printf("LA auto step size: %f\n", la_step);
tnhnrl	5:7421776f6b08	552	PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
tnhnrl	5:7421776f6b08	553	}
tnhnrl	5:7421776f6b08	554	else
tnhnrl	5:7421776f6b08	555	{
tnhnrl	5:7421776f6b08	556	PC.printf("ERROR: LA in manual mode!\n");
tnhnrl	5:7421776f6b08	557	}
tnhnrl	5:7421776f6b08	558	}
mdavis30	6:ce2cf7f4d7d5	559
mdavis30	3:1257a7d2eb3a	560	else if (Key == 'A' or Key == 'a')
mdavis30	6:ce2cf7f4d7d5	561	{
tnhnrl	5:7421776f6b08	562	if (LA_auto == true)
tnhnrl	5:7421776f6b08	563	{
tnhnrl	5:7421776f6b08	564	PC.printf("A recieved\n");
tnhnrl	5:7421776f6b08	565	la_setPoint=la_setPoint_temp;
tnhnrl	5:7421776f6b08	566	PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint);
tnhnrl	5:7421776f6b08	567	}
tnhnrl	5:7421776f6b08	568	else
tnhnrl	5:7421776f6b08	569	{
tnhnrl	5:7421776f6b08	570	PC.printf("ERROR: LA in manual mode!\n");
tnhnrl	5:7421776f6b08	571	}
mdavis30	6:ce2cf7f4d7d5	572	}
mdavis30	4:3c22d85a94a8	573
mdavis30	0:381a84fad08b	574
mdavis30	4:3c22d85a94a8	575	else if(Key == 'n' or Key == 'N')
mdavis30	6:ce2cf7f4d7d5	576	{
tnhnrl	5:7421776f6b08	577	if (LA_auto == false)
tnhnrl	5:7421776f6b08	578	{
tnhnrl	5:7421776f6b08	579	PC.printf("N key pressed. \n");
mdavis30	9:2dcfd6ce61ea	580	PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY().c_str());
tnhnrl	5:7421776f6b08	581	}
tnhnrl	5:7421776f6b08	582
tnhnrl	5:7421776f6b08	583	else
tnhnrl	5:7421776f6b08	584	{
tnhnrl	5:7421776f6b08	585	PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl	5:7421776f6b08	586	}
mdavis30	4:3c22d85a94a8	587	}
mdavis30	6:ce2cf7f4d7d5	588
mdavis30	4:3c22d85a94a8	589	else if(Key == 'm' or Key == 'M')
mdavis30	4:3c22d85a94a8	590	{
tnhnrl	5:7421776f6b08	591	if (LA_auto == false)
tnhnrl	5:7421776f6b08	592	{
tnhnrl	5:7421776f6b08	593	PC.printf("M key pressed. \n");
mdavis30	9:2dcfd6ce61ea	594	PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY().c_str());
tnhnrl	5:7421776f6b08	595	}
tnhnrl	5:7421776f6b08	596
tnhnrl	5:7421776f6b08	597	else
tnhnrl	5:7421776f6b08	598	{
tnhnrl	5:7421776f6b08	599	PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl	5:7421776f6b08	600	}
mdavis30	0:381a84fad08b	601	}
mdavis30	6:ce2cf7f4d7d5	602
mdavis30	4:3c22d85a94a8	603	else if(Key == 'j' or Key == 'J')
mdavis30	4:3c22d85a94a8	604	{
tnhnrl	5:7421776f6b08	605	if (LA_auto == false)
tnhnrl	5:7421776f6b08	606	{
tnhnrl	5:7421776f6b08	607	PC.printf("J key pressed. \n");
mdavis30	9:2dcfd6ce61ea	608	PC.printf("%s\n", BLA_object.Keyboard_A().c_str());
tnhnrl	5:7421776f6b08	609	}
tnhnrl	5:7421776f6b08	610
tnhnrl	5:7421776f6b08	611	else
tnhnrl	5:7421776f6b08	612	{
tnhnrl	5:7421776f6b08	613	PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl	5:7421776f6b08	614	}
mdavis30	0:381a84fad08b	615	}
mdavis30	6:ce2cf7f4d7d5	616
mdavis30	4:3c22d85a94a8	617	else if(Key == 'k' or Key == 'K')
mdavis30	4:3c22d85a94a8	618	{
tnhnrl	5:7421776f6b08	619	if (LA_auto == false)
tnhnrl	5:7421776f6b08	620	{
tnhnrl	5:7421776f6b08	621	PC.printf("K key pressed. \n");
mdavis30	9:2dcfd6ce61ea	622	PC.printf("%s\n", BLA_object.Keyboard_D().c_str());
tnhnrl	5:7421776f6b08	623	}
tnhnrl	5:7421776f6b08	624
tnhnrl	5:7421776f6b08	625	else
tnhnrl	5:7421776f6b08	626	{
tnhnrl	5:7421776f6b08	627	PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl	5:7421776f6b08	628	}
tnhnrl	5:7421776f6b08	629	}
mdavis30	0:381a84fad08b	630
mdavis30	0:381a84fad08b	631	else
mdavis30	0:381a84fad08b	632	{
mdavis30	0:381a84fad08b	633	PC.printf("\n%c received!\n", Key);
mdavis30	0:381a84fad08b	634	PC.printf("\nDoing nothing.\n");
mdavis30	0:381a84fad08b	635	}
mdavis30	0:381a84fad08b	636
mdavis30	0:381a84fad08b	637	wait_us(100); //for PC readable
mdavis30	6:ce2cf7f4d7d5	638	//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
mdavis30	6:ce2cf7f4d7d5	639	//BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
tnhnrl	5:7421776f6b08	640	}
mdavis30	6:ce2cf7f4d7d5	641	//end of PC readBLE
mdavis30	6:ce2cf7f4d7d5	642
tnhnrl	5:7421776f6b08	643	//MC readable
tnhnrl	5:7421776f6b08	644	MC_readable_string = BLA_object.MC_readable_redux();
tnhnrl	5:7421776f6b08	645	//PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string);
mdavis30	0:381a84fad08b	646
mdavis30	6:ce2cf7f4d7d5	647	/* if (LA_auto == false)
mdavis30	4:3c22d85a94a8	648	{
tnhnrl	5:7421776f6b08	649	if (!MC_readable_string.empty()) //if this string is empty
tnhnrl	5:7421776f6b08	650	{
tnhnrl	5:7421776f6b08	651	PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller
tnhnrl	5:7421776f6b08	652	}
tnhnrl	5:7421776f6b08	653	else
tnhnrl	5:7421776f6b08	654	{
tnhnrl	5:7421776f6b08	655	;
tnhnrl	5:7421776f6b08	656	//PC.printf("NOTHING?\n");
tnhnrl	5:7421776f6b08	657	}
mdavis30	6:ce2cf7f4d7d5	658	}*/
mdavis30	6:ce2cf7f4d7d5	659	if (LA_auto==true)
tnhnrl	5:7421776f6b08	660	{
mdavis30	6:ce2cf7f4d7d5	661	//PC.printf("LA_auto true\n");
mdavis30	6:ce2cf7f4d7d5	662	//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
mdavis30	6:ce2cf7f4d7d5	663	BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
mdavis30	6:ce2cf7f4d7d5	664	//wait_us(100); //for PC readable (0.1 ms)
tnhnrl	5:7421776f6b08	665	}
mdavis30	6:ce2cf7f4d7d5	666	else if (LA_auto == false)
mdavis30	6:ce2cf7f4d7d5	667	{
mdavis30	6:ce2cf7f4d7d5	668	//PC.printf("LA_auto false\n");
mdavis30	6:ce2cf7f4d7d5	669	// if (!MC_readable_string.empty()) //if this string is empty
mdavis30	6:ce2cf7f4d7d5	670	// {
mdavis30	6:ce2cf7f4d7d5	671	// PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller
mdavis30	6:ce2cf7f4d7d5	672	// }
mdavis30	6:ce2cf7f4d7d5	673	// else
mdavis30	6:ce2cf7f4d7d5	674	// {
mdavis30	6:ce2cf7f4d7d5	675	// ;
mdavis30	6:ce2cf7f4d7d5	676	// //PC.printf("NOTHING?\n");
mdavis30	6:ce2cf7f4d7d5	677	// }
mdavis30	4:3c22d85a94a8	678
mdavis30	6:ce2cf7f4d7d5	679	// while (count_while==0)
mdavis30	6:ce2cf7f4d7d5	680	// {
mdavis30	6:ce2cf7f4d7d5	681	//// PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
mdavis30	6:ce2cf7f4d7d5	682	//// PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
mdavis30	6:ce2cf7f4d7d5	683	//// wait(1);
mdavis30	6:ce2cf7f4d7d5	684	//// PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
mdavis30	6:ce2cf7f4d7d5	685	//// wait(1);
mdavis30	6:ce2cf7f4d7d5	686	//// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
mdavis30	6:ce2cf7f4d7d5	687	//// count_while++;
mdavis30	6:ce2cf7f4d7d5	688	//
mdavis30	6:ce2cf7f4d7d5	689	// BLA_object.Keyboard_E(); //turn on motor
mdavis30	6:ce2cf7f4d7d5	690	// BLA_object.velocity_only(0); //set the velocity to zero just in case
mdavis30	6:ce2cf7f4d7d5	691	// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
mdavis30	6:ce2cf7f4d7d5	692	// count_while++;
mdavis30	6:ce2cf7f4d7d5	693	// }
mdavis30	4:3c22d85a94a8	694	}
mdavis30	4:3c22d85a94a8	695
mdavis30	6:ce2cf7f4d7d5	696
mdavis30	0:381a84fad08b	697	if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
mdavis30	0:381a84fad08b	698	{
mdavis30	0:381a84fad08b	699	count ++;
mdavis30	0:381a84fad08b	700	//pc().printf("We have a small issue\n");
mdavis30	6:ce2cf7f4d7d5	701	if(count==10)
mdavis30	6:ce2cf7f4d7d5	702	{
mdavis30	6:ce2cf7f4d7d5	703	pc().printf("Bad pot issue\n");
mdavis30	6:ce2cf7f4d7d5	704	//hBridge().stop();
mdavis30	6:ce2cf7f4d7d5	705	count = 0;
mdavis30	7:10d7fbac30ea	706
mdavis30	6:ce2cf7f4d7d5	707	}
mdavis30	0:381a84fad08b	708
mdavis30	0:381a84fad08b	709	}
mdavis30	2:c3cb3ea3c9fa	710	else if ((5.0*ain.read())<1.0)
mdavis30	2:c3cb3ea3c9fa	711	{
mdavis30	6:ce2cf7f4d7d5	712	pc().printf("Hit the limit switch??\n");
mdavis30	2:c3cb3ea3c9fa	713	hBridge().stop();
mdavis30	7:10d7fbac30ea	714	wait(1);
mdavis30	7:10d7fbac30ea	715	hBridge().reset();
mdavis30	7:10d7fbac30ea	716	positionCmd= 375;
mdavis30	7:10d7fbac30ea	717	posCon().writeSetPoint(positionCmd);
mdavis30	7:10d7fbac30ea	718	hBridge().run(posCon().getOutput());
mdavis30	8:9b9431404db7	719	wait(2);
mdavis30	2:c3cb3ea3c9fa	720	}
mdavis30	2:c3cb3ea3c9fa	721
mdavis30	6:ce2cf7f4d7d5	722	//string snaps
mdavis30	0:381a84fad08b	723	else if (pvf().getVelocity() > 100)
mdavis30	0:381a84fad08b	724	{
tnhnrl	5:7421776f6b08	725	PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY
mdavis30	0:381a84fad08b	726	//hBridge().stop();
tnhnrl	5:7421776f6b08	727	//PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity());
mdavis30	6:ce2cf7f4d7d5	728	PC.printf("******** String broke? *******\n");
mdavis30	4:3c22d85a94a8	729	}
mdavis30	6:ce2cf7f4d7d5	730
mdavis30	6:ce2cf7f4d7d5	731	}
mdavis30	0:381a84fad08b	732	}

Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	26 Jul 2017
Imports:	3
Forks:	0
Commits:	13
Dependents:	0
Dependencies:	9
Followers:	2

Important changes to repositories hosted on mbed.com

main.cpp@11:b5cc98f154a4, 2017-07-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

main.cpp@11:b5cc98f154a4, 2017-07-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning