7_20_17_FSG_

FSG » Code » 7_20_17_FSG_

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@6:ce2cf7f4d7d5, 2017-07-21 (annotated)

Committer:: mdavis30
Date:: Fri Jul 21 13:32:10 2017 +0000
Revision:: 6:ce2cf7f4d7d5
Parent:: 5:7421776f6b08
Child:: 7:10d7fbac30ea

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

Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	21 Jul 2017
Imports:	0
Forks:	1
Commits:	8
Dependents:	0
Dependencies:	9
Followers:	2

Important changes to repositories hosted on mbed.com

main.cpp@6:ce2cf7f4d7d5, 2017-07-21 (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@6:ce2cf7f4d7d5, 2017-07-21 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning