RobotMain.cpp

#include "RobotMain.h"

FoxFire::FoxFire(void) :
	m_oscillo_signal_toggle(NULL),
	m_oscillo_signal_high_low(NULL),
	m_drive_train(NULL), 
	m_compressor(NULL), 
	m_last_auto(false), 
	m_first_boot(true) {
	
}

Compressor * FoxFire::GetCompressor() {
	return m_compressor;
}

void FoxFire::RobotInit() {
	RobotFinder::Init(this);
	printf("Initializing FoxFire...\n");
	printf("Code compiled on %s at %s\n",__DATE__ , __TIME__);
	this->SetPeriod(0.1);
	printf("\tPeriod set to %2.2f seconds\n", this->GetPeriod());
	printf("Initializing configuration:\n");
	RobotConfiguration::Init();
	printf("Configuration complete.\n");
	printf("\tInitializing Oscilloscope signal...");
	m_oscillo_signal_toggle = new OscilloSignal(13);
	m_oscillo_signal_high_low = new OscilloSignal(12);
	printf("done.\n");
	printf("\tInitializing DriverInput...");
	DriverInput::Init();
	printf("done.\n");
	//printf("\tInitializing PIDTester...");
	//PIDtester::Init(); 
	//printf("done.\n");
	printf("\tInitializing DriveTrain...");
	m_drive_train = new DriveTrain();
	m_drive_train->Init();
	printf("done.\n");
	printf("\tInitializing LineManager...");
	LineManager::Init();
	printf("done.\n");
	printf("\tInitializing DashboardInterface...");
	DashboardInterface::Init();
	printf("done.\n");
	printf("\tInitializing Camera...");
	RobotCamera2011::Init();
	printf("done.\n");
	printf("\tInitializing Personality...");
	Personality::Init();
	printf("done.\n");
	printf("\tInitializing Shuttle...");
	Shuttle::Init();
	printf("done.\n");
	printf("\tInit Arm...");
	RobotArm::Init();
	printf("done.\n");
	printf("\tInitializing Grabber...");
	RobotGrabber::Init();
	printf("done.\n");
	printf("\tInitializing Deployer...");
	Deployer::Init();
	printf("done.\n");
	printf("\tInitizliaing signal light...");
	SignalLight::Init();
	printf("done.\n");

	printf("\tInitializing Air Compressor...");
	m_compressor = new Compressor(4,11, 4,1);
	m_compressor->Start();
	printf("done.\n");
	printf("\tInitializing SensorBox...");
	SensorBox::Init();
	printf("done.\n");

	printf("Initialization complete.\n");
	
	printf("\n\nHello, student. Would you like to play a game?\n");
}

void FoxFire::TeleopInit() {
	printf("Initializing Teleop mode...\n");
	printf("Teleop mode initialized.\n");
	if (m_last_auto) {
		RobotGrabber::Stop();
		Shuttle::Reset();
		m_last_auto = false;
	}
}

void FoxFire::TeleopPeriodic() {
	m_oscillo_signal_high_low->SetHigh();
	m_oscillo_signal_toggle->FlipSig();

	// START HERE

	// This should go first.
	RobotConfiguration::Process();
	DriverInput::Process();
	SensorBox::Process();
	PoseControl::Process();
	
	LineManager::Process();
	Deployer::Process();

	RobotCamera2011::Process();
	m_drive_train->SetDriveMode(DriveTrain::DRIVE_TELEOP);
	m_drive_train->Process();
	Shuttle::Process();
	RobotArm::Process();
	RobotGrabber::Process();
	SignalLight::Process();
	

	RobotConfiguration::ClearUpdated();
	// This should  go last.
	DashboardInterface::Process();
	// END HERE

	m_oscillo_signal_high_low->SetLow();
}

void FoxFire::TeleopContinuous() {

}

void FoxFire::AutonomousInit() {
	printf("Going into autonomous!\n");
	Autonomous::Init();
	m_last_auto = true;
}

void FoxFire::AutonomousPeriodic() {

	m_oscillo_signal_high_low->SetHigh();
	m_oscillo_signal_toggle->FlipSig();
	
	 // START HERE
	 DriverInput::Process();
	 SensorBox::Process();
	 LineManager::Process();
	 Autonomous::Process();
	 PoseControl::Process();
	 
	 // Now that autonomous control has generated the proper signals,
	 // distribute the instructions to the various components.
	 
	 m_drive_train->SetDriveMode(DriveTrain::DRIVE_AUTONOMOUS);
	 m_drive_train->Process();
	 
	 // This will cause the robot arm assembly to move and do things.
	 // DANGER WILL ROBENSON!
	 Shuttle::Process();
	 RobotArm::Process();
	 RobotGrabber::Process();
	 
	 // END HERE
	 
	 m_oscillo_signal_high_low->SetLow();
	 
	DashboardInterface::Process();

}

void FoxFire::AutonomousContinuous() {

}

void FoxFire::DisabledInit() {
	if (m_first_boot) {
		m_first_boot = false; 
	} else {
		RobotConfiguration::Init();
	}
	
}

void FoxFire::DisabledPeriodic() {
	DriverInput::Process();
	PoseControl::Process();
	SensorBox::Process();
	LineManager::Process();
	
	RobotConfiguration::Process();
	Personality::Process();
	
	DashboardInterface::Process();
}

void FoxFire::DisabledContinuous() {

}
START_ROBOT_CLASS(FoxFire);