2150DCode.c

// Put in Main File
#include "menu.h"
#include "auton.h"

void pre_auton()
{
	clearLCDLine(0);
	clearLCDLine(1);
	bLCDBacklight = true;
	calibrateGyro();
	startTask(Menu);

  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

task autonomous()
{
		// Turn The LCD Backlight On
	bLCDBacklight = true;

	// Naming Convention, Due to Multiple Counts Running
	int finalCount = count;

	//Stop The Menu Task
	stopTask(Menu);

	//Consult The Menu Class to Run Autonomous Mode
	switch(finalCount)
	{
	case 0:
		{
			autonomous0();
			break;
		}
	case 1:
		{
			autonomous1();
			break;
		}
	case 2:
		{
			autonomous2();
			break;
		}
	case 3:
		{
			autonomous3();
			break;
		}
	case 4:
		{
			autonomous4();
			break;
		}
	case 5:
		{
			autonomous5();
			break;
		}
	case 6:
		{
			autonomous6();
			break;
		}
	case 7:
		{
			autonomous7();
			break;
		}
	case 8:
		{
			autonomous8();
			break;
		}
	case 9:
		{
			autonomous9();
			break;
		}
	case 10:
		{
			autonomous10();
			break;
		}
	case 11:
		{
			autonomous11();
			break;
		}
	case 12:
		{
			autonomous12();
			break;
		}
	case 13:
		{
			autonomous13();
			break;
		}
	case 14:
		{
			autonomous14();
			break;
		}
	case 15:
		{
			autonomous15();
			break;
		}
	case 16:
		{
			testing();
			break;
		}
	case 17:
		{
			programming();
			break;
		}
	}
}


// Make a File Called menu.h and include it
#define MAX_AUTONS 17

int count = 0;
//Wait for button release on the LCD.
void waitForRelease()
{
	while (nLCDButtons != 0){}
}
//Top menu for LCD
int top_menu()
{
	bool picked2 = false;
	int menu = -1;
	clearLCDLine(0);
	clearLCDLine(1);
	while (!picked2)
	{
		displayLCDCenteredString(0, "<Autonomous>");
		displayLCDString(1, 0, "<Sensor | Power>");
		if (nLCDButtons == 1)
		{
			waitForRelease();
			menu = 0;
			picked2 = true;
		}
		if (nLCDButtons == 4)
		{
			waitForRelease();
			menu = 2;
			picked2 = true;
		}
		if (nLCDButtons == 2)
		{
			waitForRelease();
			menu = 1;
			picked2 = true;
		}
	}
	return menu;
}

//Autonomous Menu
int auton_menu(){
	bool picked = false;
	bool canceled = false;
	clearLCDLine(0);
	clearLCDLine(1);
	while (!picked)
	{
		displayLCDCenteredString(1, "< Enter >");
		if (nLCDButtons == 1)
		{
			waitForRelease();
			count--;
			if (count < 0)
			{
				count = MAX_AUTONS;
			}
		}
		if (nLCDButtons == 4)
		{
			waitForRelease();
			count++;
			if (count > MAX_AUTONS)
			{
				count = 0;
			}
		}
		if (nLCDButtons == 2)
		{
			waitForRelease();
			picked = true;
		}

		switch (count)
		{
			case 0: {
				displayLCDCenteredString(0, "Blue Autoloader");
				break;
			}
			case 1: {
				displayLCDCenteredString(0, "Blue Normal");
					break;
			}
			case 2: {
				displayLCDCenteredString(0, "Red Autoloader");
				break;
			}
			case 3: {
				displayLCDCenteredString(0, "Red Normal");
				break;
			}
			case 4: {
				displayLCDCenteredString(0, "Deploy Only");
				break;
			}
			case 5: {
				displayLCDCenteredString(0, "1 Point");
				break;
			}
			case 6: {
				displayLCDCenteredString(0, "Only on Red");
				break;
			}
			case 7: {
				displayLCDCenteredString(0, "Only on Blue");
				break;
			}
			case 8:{
				displayLCDCenteredString(0, "Build 2 Skyrise");
				break;
			}
			case 9:{
				displayLCDCenteredString(0, "Stack 2 Cubes");
				break;
			}
			case 10:{
				displayLCDCenteredString(0, "Troll1");
				break;
			}
			case 11:{
				displayLCDCenteredString(0, "Troll2");
				break;
			}
			case 12:{
				displayLCDCenteredString(0, "Troll3");
				break;
			}
			case 13:{
				displayLCDCenteredString(0, "Troll4");
				break;
			}
			case 14:{
				displayLCDCenteredString(0, "Troll5");
				break;
			}
			case 15:{
				displayLCDCenteredString(0, "Troll6");
				break;
			}
			case 16: {
				displayLCDCenteredString(0, "Testing");
				break;
			}
			case 17: {
				displayLCDCenteredString(0, "Programming");
				break;
			}
		}
	}
	//clearLCDLine(1);

	while (!canceled)
	{
		displayLCDCenteredString(1, " < Cancel > ");
		if (nLCDButtons == 2)
		{
			waitForRelease();
			return -1;
		}
	}

	return 0;
}

//More sensor values
int moar_sensor_menu()
{
	bool picked2 = false;
	string rEncoder = "";
	string lEncoder = "";
	int menu = -1;
	clearLCDLine(0);
	clearLCDLine(1);
	while (!picked2)
	{
		rEncoder = "";
		lEncoder = "";
		displayLCDCenteredString(1, "<More | Back>");
		displayLCDString(0, 0, "L: ");
		sprintf(rEncoder, "%0.1f", ticksToInches(nMotorEncoder[m_FR])); //Build the value to be displayed
		sprintf(lEncoder, "%0.1f", ticksToInches(nMotorEncoder[m_FL])); //Build the value to be displayed
		displayLCDString(0, 2, lEncoder);
		displayLCDString(0, 9, "R: ");
		displayLCDString(0, 11, rEncoder);
		if (nLCDButtons == 4)
		{
			waitForRelease();
			menu = -1;
			picked2 = true;
		}
		if (nLCDButtons == 1)
		{
			waitForRelease();
			menu = 0;
			picked2 = true;
		}
	}
	return menu;
}

//Sensor values
int sensor_menu()
{
	bool picked2 = false;
	string liftHeight = "";
	string limitswitch = "";
	int menu = -1;
	clearLCDLine(0);
	clearLCDLine(1);
	while (!picked2)
	{
		liftHeight = "";
		limitswitch = "";
		if (SensorValue[s_lftZero] == 1)
		{
			nMotorEncoder[m_lftLB] = 0;
		}
		displayLCDCenteredString(1, "<Moar | Back>");
		displayLCDString(0, 0, "lift: ");
		sprintf(liftHeight, "%d", nMotorEncoder[m_lftLB]); //Build the value to be displayed
		displayLCDString(0, 4, liftHeight);
		sprintf(limitswitch, "%d", SensorValue[s_lftZero]); //Build the value to be displayed
		displayLCDString(0, 10, " Lmt:");
		displayNextLCDString(limitswitch);
		if (nLCDButtons == 4)
		{
			waitForRelease();
			menu = -1;
			picked2 = true;
		}
		if (nLCDButtons == 1)
		{
			waitForRelease();
			menu = 3;
			picked2 = true;
		}
	}
	return menu;
}

//Power values
int power_menu()
{
	bool picked2 = false;
	string mainBattery = "";
	string secondary = "";
	int menu = -1;
	clearLCDLine(0);
	clearLCDLine(1);
	while (!picked2)
	{
		displayLCDString(0, 0, "P:");
		sprintf(mainBattery, "%1.2f%c", (float)nImmediateBatteryLevel / 1000.0, 'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);
		sprintf(secondary, "%1.2f%c", (float)BackupBatteryLevel / 1000.0, 'V');
		displayLCDString(0, 9, "B:");
		displayNextLCDString(secondary);
		displayLCDCenteredString(1, "<Back>");
		if (nLCDButtons == 2)
		{
			waitForRelease();
			menu = -1;
			picked2 = true;
		}
	}
	return menu;
}

//Menu task
task Menu()
{
	int whichMenu = -1;
	while (true)
	{
		if (whichMenu == -1)
		{
			whichMenu = top_menu();
		}
		if (whichMenu == 0)
		{
			whichMenu = sensor_menu();
		}
		if (whichMenu == 1)
		{
			whichMenu = auton_menu();
		}
		if (whichMenu == 2)
		{
			whichMenu = power_menu();
		}
		if (whichMenu == 3)
		{
			whichMenu = moar_sensor_menu();
		}
		wait1Msec(15);
	}
}

// Make a File Called auton.h and include it
void resetAll()
{
	// Set The Intake to Open
	SensorValue[p_intClaw] = 1;
	startTask(handleEncoders);
	startTask(trackXYT);
	startTask(ArmPID);
	calibrateGyro();
}

void autonomous0()
{
	resetAll();
}

void autonomous1()
{

}

void autonomous2()
{

}
void autonomous3()
{

}

void autonomous4()
{

}
void autonomous5()
{

}
void autonomous6()
{

}
void autonomous7()
{

}

void autonomous8()
{

}

void autonomous9()
{

}

void autonomous10()
{

}

void autonomous11()
{

}

void autonomous12()
{

}

void autonomous13()
{

}

void autonomous14()
{

}

void autonomous15()
{

}
void testing()
{

}
void programming()
{

}