/********************************************************************************

*  Project   : FIRST Motor Controller

*  File Name   : BTRobot.cpp

*  Contributors : ELF

*  Creation Date : Jan 3, 2009

*  Revision History : Source code & revision history maintained at sourceforge.WPI.edu

*  File Description : Demo program showing color tracking using servos

*/

/*----------------------------------------------------------------------------*/

/*        Copyright (c) FIRST 2008.  All Rights Reserved.                     */

/*  Open Source Software - may be modified and shared by FRC teams. The code  */

/*  must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */

/*----------------------------------------------------------------------------*/

#include <iostream.h>

#include "math.h"

#include "AxisCamera.h"

#include "BaeUtilities.h"

#include "FrcError.h"

#include "TrackAPI.h"

#include "VisionAPI.h"

#include "WPILib.h"

#include "BTDrive.h"

#include "BTRobot.h"

#include "BTCollector.h"

#include "BTShooter.h"

#include "ControlBoard.h"

#include "Sensors.h"

#include "Autonomous.h"

#include "DashboardDataFormat.h"

/**

* This is a demo program showing the use of the color tracking API to track two colors.

* It uses the SimpleRobot class as a base of a robot application that will automatically call your

* Autonomous and OperatorControl methods at the right time as controlled by the switches on

* the driver station or the field controls. Autonomous mode tracks color assuming camera is

* mounted on a gimbal with two servos.

*/

class BTRobot : public SimpleRobot

{

Sensors* sensors;

BTShooter* shooter;

ControlBoard* control_board;

BTDrive* drivetrain;

BTCollector* collector;

BTAutonomous* autonomous;

DashboardDataFormat* dashboard;

public:

/**

* Constructor for this robot subclass.

* Create an instance of a RobotDrive with left and right motors plugged into PWM

* ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.

*/

BTRobot(void)

{

// printf("Starting BTRobot Constructor\n");

// Wait(5.0);

control_board = new ControlBoard();

sensors = new Sensors();

drivetrain = new BTDrive(control_board);

shooter = new BTShooter(sensors, control_board);

collector = new BTCollector();

autonomous = new BTAutonomous();

dashboard = new DashboardDataFormat();

SetDebugFlag(DEBUG_OFF);

GetWatchdog().SetExpiration(0.5);

}

void Autonomous(void)

{

//char funcName[]="Autonomous";

//dprintf(LOG_INFO, "Begin %s, time = %f", funcName, GetClock());

printf("Starting Autonomous.\n");

// GetWatchdog().Feed(); // turn watchdog off while debugging

GetWatchdog().SetEnabled(false);

collector->Enable(autonomous);

sensors->autonomousMode = true;

sensors->StartSensors();

shooter->Enable(1);

shooter->arbitrator->Enable();

shooter->turret->turretEnc->Start();

collector->lastLimitChangeTime = GetClock();

while( IsAutonomous() )

{

// GetWatchdog().Feed();

autonomous->Switch();

collector->DetectJam();

dprintf(LOG_INFO, "Starting Autonomous Algorithm &d", autonomous->position);

autonomous->RunAutonomous(drivetrain, sensors, control_board, shooter);

}  // end while

collector->Disable();

shooter->Disable();

shooter->arbitrator->Disable();

//dprintf(LOG_INFO, "End %s, time = &f", funcName, GetClock());

ShowActivity ("Autonomous end                                            ");

}  // end autonomous

void OperatorControl(void)

{

//char funcName[]="OperatorControl";

//dprintf(LOG_DEBUG, funcName);

printf("Starting Teleoperated.\n");

GetWatchdog().SetEnabled(true);

int selector_value;

bool prev_trigger = false;

shooter->arbitrator->Enable();

// shooter->turret->turretEnc->Start();

collector->lastLimitChangeTime = GetClock();

while (1)

{

GetWatchdog().Feed();

autonomous->Switch();

//printf("Selector position = %d", autonomous->position);

selector_value = autonomous->position;

drivetrain->DoDrive(6, false);

collector->DetectJam();

if (control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::SHOOTER_DISABLE))

{

shooter->Disable();

}

if (control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::SHOOTER_ENABLE))

{

shooter->Enable(autonomous->position);

}

if(control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::COLLECTOR_ENABLE))

collector->Enable(autonomous);

if(control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::COLLECTOR_DISABLE))

collector->Disable();

if(control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::COLLECTOR_REVERSE))

collector->Reverse();

if (control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::OVERRIDE))

shooter->turret->Override();

// printf("encoder value = %d", shooter->turret->turretEnc->Get());

else

{

if (shooter->turret->overridden)

{

shooter->turret->turretMotor->Set(0.00);

shooter->turret->overridden = false;

}

GetWatchdog().Feed();

shooter->turret->Center();

GetWatchdog().Feed();

}

if (control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::ARBITRATOR_REVERSE))

{

shooter->arbitrator->Reverse();

}

shooter->SetSpeedToThrottle();

if (control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::SHOOTER_FIRE))

{

shooter->arbitrator->Lock_N_Load(control_board, 1);

}

else if (prev_trigger)

{

shooter->arbitrator->Reverse();

}

prev_trigger = control_board->GetRawButton(ControlBoard::SHOOTER, BUTTONS_S::SHOOTER_FIRE);

UpdateDashboard();

}

// collector->Disable();

// shooter->Disable();

} // end operator control

void UpdateDashboard(void)

{

dashboard->boolUserData[0] = shooter->turret->IsColorFound();

dashboard->boolUserData[1] = (!shooter->turret->clLimitPressed);

dashboard->boolUserData[2] = (!shooter->turret->ccLimitPressed);

dashboard->boolUserData[3] = collector->collectorJammed;

dashboard->intUserData = (shooter->turret->turretEnc->Get() % (shooter->turret->TURRET_FULL_CIRCLE));

if (dashboard->intUserData < 0)

{

dashboard->intUserData += shooter->turret->TURRET_FULL_CIRCLE;

}

dashboard->PackAndSend();

}

};

START_ROBOT_CLASS(BTRobot);