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@2:c3cb3ea3c9fa, 2017-06-13 (annotated)

Committer:: mdavis30
Date:: Tue Jun 13 17:23:06 2017 +0000
Revision:: 2:c3cb3ea3c9fa
Parent:: 0:381a84fad08b
Child:: 3:1257a7d2eb3a

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

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@2:c3cb3ea3c9fa, 2017-06-13 (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@2:c3cb3ea3c9fa, 2017-06-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning