<repo_name>frc3324/Metrobots2014<file_sep>/SimpleTemplate/util/gamepad.cpp
#include "GamePad.h"
#include <math.h>
#include <WPILib.h>

GamePad::GamePad(int port){
	
	joystick_ = new Joystick( port );
	dPadLeftDownEvent = false;
	dPadRightDownEvent = false;
	dPadLeftOld = false;
	dPadRightOld = false;
		
	for( int i = 0; i < MAX_NUM_BUTTONS; i++ ){
			
		oldBtnStates[i] = false;
		newBtnStates[i] = false;

		upEventStates[i] = false;
		downEventStates[i] = false;
			
	}
	
}

double GamePad::GetAxis( int axis ){

	double output;
	
	if( axis == GamePad::LEFT_X || axis == GamePad::RIGHT_X ){

	    output = joystick_->GetRawAxis( axis );

    }
    else{

        output = -joystick_->GetRawAxis( axis );

    }
	
	if( fabs(output) < GamePad::AXIS_DEADBAND ){
		output = 0.0;
	}

	return output;
		
}

void GamePad::Update(){

	for( int i = 1; i < MAX_NUM_BUTTONS + 1; i++ ){
            
        newBtnStates[i] = joystick_->GetRawButton(i);
            
        upEventStates[i] = !newBtnStates[i] && oldBtnStates[i];
            
        downEventStates[i] = newBtnStates[i] && !oldBtnStates[i];
        
        oldBtnStates[i] = newBtnStates[i];
            
    }

	dPadRightDownEvent = (joystick_->GetRawAxis(GamePad::DPAD_X) > 0.0) && !dPadRightOld;
	dPadLeftDownEvent = (joystick_->GetRawAxis(GamePad::DPAD_X) < 0.0) && !dPadLeftOld;
	dPadRightOld = joystick_->GetRawAxis(GamePad::DPAD_X) > 0.0;
	dPadLeftOld = joystick_->GetRawAxis(GamePad::DPAD_X) < 0.0;
	
}

bool GamePad::GetButton( int button ){

	return newBtnStates[ button ];

}

bool GamePad::GetButtonDown( int button ){

	return downEventStates[ button ];

}

bool GamePad::GetButtonUp( int button ){

	return upEventStates[ button ];

}

bool GamePad::GetDPadLeftDown(){
	return dPadLeftDownEvent;
}

bool GamePad::GetDPadRightDown(){
	return dPadRightDownEvent;
}


<file_sep>/SimpleTemplate/util/gamepad.h
#ifndef GAME_PAD_H
#define GAME_PAD_H

#include "WPILib.h"
#define MAX_NUM_BUTTONS 12

class GamePad {

	public:
		GamePad( int port );
		double GetAxis( int axis );
		void Update();
		bool GetButton( int button );
		bool GetButtonDown( int button );
		bool GetButtonUp( int button );
		bool GetDPadLeftDown();
		bool GetDPadRightDown();

		static const double AXIS_DEADBAND = 0.1;
		
		static const int A = 1;
		static const int B = 2;
		static const int X = 3;
		static const int Y = 4;
		static const int LB = 5;
		static const int RB = 6;
		static const int BACK = 7;
		static const int START = 8;
		static const int LEFT_JS = 9;
		static const int RIGHT_JS = 10;
		
		static const int LEFT_X = 1;
		static const int LEFT_Y = 2;
		static const int TRIGGER = 3;
		static const int RIGHT_X = 4;
		static const int RIGHT_Y = 5;
		static const int DPAD_X = 6;
		static const int DPAD_Y = 7;
		Joystick *joystick_;
		
	private:
				
		bool oldBtnStates[ MAX_NUM_BUTTONS ];
		bool newBtnStates[ MAX_NUM_BUTTONS ];
		bool upEventStates[ MAX_NUM_BUTTONS ];
		bool downEventStates[ MAX_NUM_BUTTONS ];
		bool dPadLeftDownEvent;
		bool dPadRightDownEvent;
		bool dPadLeftOld;
		bool dPadRightOld;

};

#endif
<file_sep>/SimpleTemplate/kicker.h
#ifndef KICKER_H
#define KICKER_H

#include "WPILib.h"
#include <Math.h>
#include "util/metropidcontroller.h"

class Kicker {
	
	public:
		Kicker( SpeedController *kicker1, SpeedController *kicker2, AnalogChannel *kickerPot);
		~Kicker(){};
		void KickBall();
		void Actuate();
		void Disable();
		void KickBallN();
		void PullUp();
		void RaiseLeg();
		void StopRaise();
		bool State();
		void KickerPotVal(int x);
		int GetAdjustedPotValue();
		Timer* t;
		
		enum {Nothing, PullingBack, Kicking, Sitting, Retracting, Pull, Raising} state;
		//int state; // 0 = Nothing, 1 = Pulling Back, 2 = Kicking
		
		SpeedController *kicker1;
		SpeedController *kicker2;
		AnalogChannel *kickerPot;
		
		static const int kickRotationMin = 0;	//dummy value
		static const int kickRotationMax = 10;	//dummy value3
		
		bool isKicking, isPullingBack, isSitting, isRetracting;
		int kickerPotVal;
};

#endif
<file_sep>/SimpleTemplate/pickup.h
#ifndef PICKUP_H
#define PICKUP_H

#include "WPILib.h"
#include <Math.h>
#include "util/metropidcontroller.h"
#include "util/dualrelay.h"

class Pickup {

	public:
		Pickup( SpeedController *motor, DualRelay *angleMotor, DigitalInput *bottomSwitch, DigitalInput *topSwitch );
		~Pickup(){};
		void Actuate();
		void RunIntake(double wheelSpeed);
		void Raise();
		void Lower();
		void ToTop();
		void ToBottom();
		void StopAngle();
		void Intake();
		void Expel();
		void StopIntake();
		void Disable();
		void Kick();
		bool AllWay();
		bool isKicking();
		
		enum {Pushing, Pulling, Kicking, Nothing} pickupState;
		enum {Up, Down, Stopped} angleState;
		
		SpeedController *motor;
		DualRelay *angleMotor;
		DigitalInput *bottomSwitch, *topSwitch;
		
		double motorSpeed;
		Relay::Value angleMotorBool;
		bool moveAllWay;
};

#endif
<file_sep>/SimpleTemplate/drive.cpp
#include "drive.h"
#include <Math.h>

Drive::Drive( SpeedController *flMotor_, SpeedController *blMotor_, SpeedController *frMotor_, SpeedController *brMotor_, 
                                Encoder *flEncoder_, Encoder *blEncoder_, Encoder *frEncoder_, Encoder *brEncoder_, 
                                Gyro *gyro_ ){

        flMotor = flMotor_;
        blMotor = blMotor_;
        frMotor = frMotor_;
        brMotor = brMotor_;
        
        flEncoder = flEncoder_;
        blEncoder = blEncoder_;
        frEncoder = frEncoder_;
        brEncoder = brEncoder_;

        flEncoder->Start();
        blEncoder->Start();
        frEncoder->Start();
        brEncoder->Start();
        
        gyro = gyro_;
        
        flPID = new MetroPIDController( Drive::PID_P, Drive::PID_I, Drive::PID_D, MetroPIDController::PID, true );
        blPID = new MetroPIDController( Drive::PID_P, Drive::PID_I, Drive::PID_D, MetroPIDController::PID, true );
        frPID = new MetroPIDController( Drive::PID_P, Drive::PID_I, Drive::PID_D, MetroPIDController::PID, true );
        brPID = new MetroPIDController( Drive::PID_P, Drive::PID_I, Drive::PID_D, MetroPIDController::PID, true );

        xPID = new MetroPIDController( Drive::XYTURN_PID_P, Drive::XYTURN_PID_I, Drive::XYTURN_PID_D, MetroPIDController::PID, true );
        yPID = new MetroPIDController( Drive::XYTURN_PID_P, Drive::XYTURN_PID_I, Drive::XYTURN_PID_D, MetroPIDController::PID, true );
        turnPID = new MetroPIDController( Drive::XYTURN_PID_P, Drive::XYTURN_PID_I, Drive::XYTURN_PID_D, MetroPIDController::PID, true );
        
        flPID->Disable();
        blPID->Disable();
        frPID->Disable();
        brPID->Disable();
        
        motorInverters[0] = 1;
        motorInverters[1] = 1;
        motorInverters[2] = 1;
        motorInverters[3] = 1;
        
        gyroBias = 0.0;
        
        isPIDControl = false;
        isSlowDrive = false;
        isFieldOriented = true;
        isHoldAngle = false;
        
        driverX = 0.0;
        driverY = 0.0;
        driverTurn = 0.0;
}

double clamp(double range, double num) {
        return (range > fabs(num)) ? 0.0 : num;
}

void Drive::Actuate(){

        flPID->SetSource( ( 1 / flEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
        blPID->SetSource( -( 1 / blEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
        frPID->SetSource( -( 1 / frEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );
        brPID->SetSource( ( 1 / brEncoder->GetPeriod() ) / Drive::VEL_PID_MULTIPLIER );

        double flVel = 1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
        double blVel = 1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
        double frVel = -1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;
        double brVel = -1 / flEncoder->GetPeriod() / Drive::VEL_PID_MULTIPLIER;

        /*xPID->SetSource( flVel + brVel - blVel - frVel );
        yPID->SetSource( flVel + brVel + blVel + frVel );
        turnPID->SetSource( flVel - brVel + blVel - frVel );*/
        
        double x = driverX;
        double y = driverY;
        double turn = driverTurn;

		if( isFieldOriented ){
	
			double gAngle = GetGyroAngle();
			double cosA = cos( gAngle * 3.14159 / 180 );
			double sinA = sin( gAngle * 3.14159 / 180 );
	
			x = driverX*cosA - driverY*sinA;
			y = driverX*sinA + driverY*cosA;

		}
	
		if( isHoldAngle ){
	
			double gyroAngle = fmod( GetGyroAngle(), 360.0 );
			double relativeAngle = gyroAngle - targetAngle;
	
			if( relativeAngle > 180 ){
	
				relativeAngle = ( 180 - ( relativeAngle - 180 ) );
	
			}
	
			turn = Drive::ANGLE_P * relativeAngle;
	
		}

        double fl = ( y + x + turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
        double bl = ( y - x + turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
        double fr = ( y - x - turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );
        double br = ( y + x - turn ) * ( isSlowDrive ? Drive::SLOW_DRIVE_MULTIPLIER : 1.0 );

		if( isPIDControl ){
	
			flPID->SetSetpoint( fl, fl );
			blPID->SetSetpoint( bl, bl );
			frPID->SetSetpoint( fr, fr );
			brPID->SetSetpoint( br, br );
		
			fl = flPID->GetOutput();
			bl = blPID->GetOutput();
			fr = frPID->GetOutput();
			br = brPID->GetOutput();

			//fl = xPID->GetOutput() + yPID->GetOutput() + turnPID->GetOutput() ;
			//bl = -xPID->GetOutput() + yPID->GetOutput() + turnPID->GetOutput() ;
			//fr = -xPID->GetOutput() + yPID->GetOutput() - turnPID->GetOutput() ;
			//br = xPID->GetOutput() + yPID->GetOutput() - turnPID->GetOutput() ;
		
		}

        flMotor->Set( fl * motorInverters[0] );
        blMotor->Set( bl * motorInverters[1] );
        frMotor->Set( fr * motorInverters[2] );
        brMotor->Set( br * motorInverters[3] );

}

void Drive::Disable(){


        SetPIDControl( false );

        flMotor->Set( 0.0 );
        blMotor->Set( 0.0 );
        frMotor->Set( 0.0 );
        brMotor->Set( 0.0 );

}

void Drive::SetMecanumRLStrafe( double r, double l, double strafe ){

        driverX = strafe;
        driverY = (( r + l ) / 2) * (( r + l ) / 2);
        driverTurn = (( l - r ) / 2) * (( l - r ) / 2);

}

void Drive::SetMecanumXYTurn( double x, double y, double turn ){
        
        driverX = x;
        driverY = y;
        driverTurn = turn;

}


void Drive::SetFieldOriented( bool value ){

        isFieldOriented = value;

}

bool Drive::IsFieldOriented(){

        return isFieldOriented;

}

void Drive::SetHoldAngle( bool isOn ){

        isHoldAngle = isOn;

}

void Drive::SetTargetAngle( double degrees ){
        
        targetAngle = fmod( degrees, 360.0 );
        
}

void Drive::ResetEncoders(){

        flEncoder->Reset();
        blEncoder->Reset();
        frEncoder->Reset();
        brEncoder->Reset();

}

double Drive::GetDistMoved(){

        return ( flEncoder->Get() + blEncoder->Get() - frEncoder->Get() - brEncoder->Get() ) / 4;

}

void Drive::ResetGyro( double bias ){

        gyroBias = bias;
        gyro->Reset();

}

double Drive::GetGyroAngle(){

        return gyro->GetAngle() + gyroBias;

}

bool Drive::IsPIDControl(){

        return isPIDControl;

}

void Drive::SetPIDControl( bool value ){

        isPIDControl = value;
        
        if( isPIDControl ){
        
                flPID->Enable();
                blPID->Enable();
                frPID->Enable();
                brPID->Enable();

                xPID->Enable();
                yPID->Enable();
                turnPID->Enable();
        
        }
        else{
        
                flPID->Disable();
                blPID->Disable();
                frPID->Disable();
                brPID->Disable();
        
                xPID->Disable();
                yPID->Disable();
                turnPID->Disable();
                
        }

}

bool Drive::IsSlowDrive(){

        return isSlowDrive;

}

void Drive::SetSlowDrive( bool value ){

        isSlowDrive = value;

}

void Drive::SetInvertedMotors( bool fl, bool bl, bool fr, bool br ){

        motorInverters[0] = fl ? -1 : 1;
        motorInverters[1] = bl ? -1 : 1;
        motorInverters[2] = fr ? -1 : 1;
        motorInverters[3] = br ? -1 : 1;

}

void Drive::NormalizeMotorSpeeds(){
        
        /*double maxValue = 1.0;
                
        if(fabs( flSpeed ) > maxValue ){
                maxValue = fabs( flSpeed );
        }
        if( fabs( blSpeed ) > maxValue ){
                maxValue = fabs( blSpeed );
        }
        if( fabs( frSpeed ) > maxValue ){
                maxValue = fabs( frSpeed );
        }
        if( fabs( brSpeed ) > maxValue ){
                maxValue = fabs( brSpeed );
        }
        
        flSpeed = flSpeed / maxValue;
        blSpeed = blSpeed / maxValue;
        frSpeed = frSpeed / maxValue;
        brSpeed = brSpeed / maxValue;
        */
}
<file_sep>/SimpleTemplate/kicker.cpp
#include "kicker.h"
#include <Math.h>

Kicker::Kicker( SpeedController *kicker1_, SpeedController *kicker2_, AnalogChannel *kickerPot_) {

                kicker1 = kicker1_;
                kicker2 = kicker2_;
				
                //TODO: add GetAdjustedPotValue() method
				kickerPot = kickerPot_;
				
				state = Nothing;
				
				kicker1->Set(0.0);
				kicker2->Set(0.0);
				
				isKicking = false;
				isPullingBack = false;
				isSitting = false;
				
				t = new Timer();
				t->Start();

}
	
void Kicker::Actuate() {
	if (state == PullingBack) {
		kicker1->Set(1.0);
		kicker2->Set(1.0);
		if (t->Get() >= 0.4) {
			t->Reset();
			state = Kicking;
		}
	}else if (state == Kicking) {
		kicker1->Set(-1.0);
		kicker2->Set(-1.0);
		if (kickerPot->GetValue() > (kickerPotVal + 670)) {
			state = Retracting;
			kicker1->Set(0);
			kicker2->Set(0);
			t->Reset();
		}
	}else if (state == Retracting){
		kicker1->Set(0.4);
		kicker2->Set(0.4);
		if (kickerPot->GetValue() < (kickerPotVal + 90)) {
			state = Nothing;
			kicker1->Set(0);
			kicker2->Set(0);
			t->Reset();
		}
	}else if (state == Pull){
		kicker1->Set(1);
		kicker2->Set(1);
		if (kickerPot->GetValue() >= 1) {
			kicker1->Set(-0.5);
			kicker2->Set(-0.5);
			if (kickerPot->GetValue() >= 1) {
				kicker1->Set(0);
				kicker2->Set(0);
			}
		}
	}else if (state == Raising){
		kicker1->Set(1);
		kicker2->Set(1);
	}else if (state == Nothing && kickerPot->GetValue() > (kickerPotVal + 15)){
		kicker1->Set(0.8);
		kicker2->Set(0.8);
	}
	else {
		kicker1->Set(0.0);
		kicker2->Set(0.0);
	}
}

void Kicker::KickBall(){
	if (state == Nothing) {
		t->Reset();
		state = PullingBack;
	}
}

void Kicker::KickBallN() {
	if (state == Nothing) {
		//encoder->Reset();
		t->Reset();
		state = Kicking;
	}
}

void Kicker::PullUp() {
	if (state == Nothing) {
		state = Pull;
	}
}

void Kicker::RaiseLeg() {
	if (state == Nothing) {
		state = Raising;
	}
}

void Kicker::StopRaise() {
	if (state == Raising) state = Nothing;
}

void Kicker::Disable() {
	kicker1->Set(0);
	kicker2->Set(0);
	state = Nothing;
}

bool Kicker::State() {
	if (state == Nothing) return false;
	else return true;
}

void Kicker::KickerPotVal(int x) {
	kickerPotVal = x;
}

int Kicker::GetAdjustedPotValue() {
	return kickerPot->GetValue() - kickerPotVal;
}
<file_sep>/SimpleTemplate/drive.h
#ifndef DRIVE_H
#define DRIVE_H

#include "WPILib.h"
#include <Math.h>
#include "util/metropidcontroller.h"

class Drive {

	public:
		Drive( SpeedController *flMotor_, SpeedController *blMotor_, SpeedController *frMotor_, SpeedController *brMotor_, 
				Encoder *flEncoder_, Encoder *blEncoder_, Encoder *frEncoder_, Encoder *brEncoder_, 
				Gyro *gyro_ );
		~Drive(){};
		void Actuate();
		void Disable();
		void SetMecanumXYTurn( double x, double y, double turn );
		void SetMecanumRLStrafe( double leftSide, double rightSide, double strafe );
		void SetHoldAngle( bool isOn );
		void SetTargetAngle( double degrees );
		double GetDistMoved();
		void ResetEncoders();
		void ResetGyro( double bias = 0.0 );
		double GetGyroAngle();
		bool IsFieldOriented();
		void SetFieldOriented( bool value );
		void SetPIDControl( bool value );
		bool IsPIDControl();
		bool IsSlowDrive();
		void SetSlowDrive( bool value );
		void SetInvertedMotors( bool fl, bool bl, bool fr, bool br );
		
		static const double PID_P = 0.75;
		static const double PID_I = 0.0;
		static const double PID_D = 0.1;
		
		static const double XYTURN_PID_P = 0.75;
		static const double XYTURN_PID_I = 0.0;
		static const double XYTURN_PID_D = 0.1;
		
		static const double VEL_PID_MULTIPLIER = 1600;
		
		static const double ANGLE_P = -1.0/45.0; 
		static const double AIM_BIAS_INCREMENT = 20.0;
		
		static const double SLOW_DRIVE_MULTIPLIER = 0.4;
		Encoder *flEncoder, *blEncoder, *frEncoder, *brEncoder;

	//private:
		void NormalizeMotorSpeeds();
	
		SpeedController *flMotor, *blMotor, *frMotor, *brMotor;
		Gyro *gyro;
		MetroPIDController *flPID, *blPID, *frPID, *brPID, *xPID, *yPID, *turnPID;
		
		double driverX, driverY, driverTurn, targetAngle;
		int motorInverters[4];
		double gyroBias;
		
		bool isPIDControl;
		bool isSlowDrive;
		bool isHoldAngle;
		bool isFieldOriented;

};

#endif
<file_sep>/SimpleTemplate/util/dualrelay.h
#ifndef DUAL_RELAY_H
#define DUAL_RELAY_H

#include "WPILib.h"

class DualRelay {
	public:
		DualRelay( int forwardPort, int reversePort );
		~DualRelay(){};
		
		void Set( Relay::Value value );
	private:
		Relay *forward;
		Relay *reverse;
};


#endif
<file_sep>/SimpleTemplate/pickup.cpp
#include "pickup.h"
#include <Math.h>

Pickup::Pickup( SpeedController *motor_, DualRelay *angleMotor_, DigitalInput *bottomSwitch_, DigitalInput *topSwitch_ ) {

	motor = motor_;
	angleMotor = angleMotor_;
	bottomSwitch = bottomSwitch_;
	topSwitch = topSwitch_;
	angleMotorBool = Relay::kOff;
	motorSpeed = 0.0;
	
	pickupState = Nothing;
	angleState = Stopped;
                
}

void Pickup::Actuate() {
	//motor->Set(motorSpeed);
	/*if ( !topSwitch->Get() || !bottomSwitch->Get() ){
		angleMotor->Set(Relay::kOff);
	}else{
		angleMotor->Set(angleMotorBool);
	}*/
	
	if( pickupState == Pushing ){
		motor->Set(1);
	}else if( pickupState == Pulling ){
		motor->Set(-1);
	}else{
		motor->Set(0);
	}
	
	if(( angleState == Up  || pickupState == Kicking ) && topSwitch->Get() ) {
		angleMotor->Set(Relay::kReverse);
	}else if ( angleState == Down && bottomSwitch->Get() ) {
		angleMotor->Set(Relay::kForward);
	}else{
		angleMotor->Set(Relay::kOff);
		angleState = Stopped;
		moveAllWay = false;
	}
	
	
}

void Pickup::Disable() {
	motorSpeed = 0.0;
	angleMotor->Set(Relay::kOff);
	motor->Set(0.0);
	angleState = Stopped;
}

void Pickup::RunIntake(double wheelSpeed) {
	motorSpeed = wheelSpeed * 0.85;
}


void Pickup::Raise() {
	angleMotorBool = Relay::kReverse;
	angleState = Up;
	moveAllWay = false;
}

void Pickup::Lower() {
	angleMotorBool = Relay::kForward;
	angleState = Down;
	moveAllWay = false;
}

void Pickup::ToTop() {
	angleMotorBool = Relay::kReverse;
	angleState = Up;
	moveAllWay = true;
}

void Pickup::ToBottom() {
	angleMotorBool = Relay::kForward;
	angleState = Down;
	moveAllWay = true;
}

void Pickup::StopAngle() {
	angleMotorBool = Relay::kOff;
	angleState = Stopped;
}

void Pickup::Intake() {
	pickupState = Pulling;
}

void Pickup::Expel() {
	pickupState = Pushing;
}

void Pickup::Kick() {
	pickupState = Kicking;
	moveAllWay = true;
}

void Pickup::StopIntake() {
	pickupState = Nothing;
}

bool Pickup::AllWay() {
	if ( moveAllWay ) return 1;
	else return 0;
}

bool Pickup::isKicking() {
	if ( pickupState == Kicking ) return 1;
	else return 0;
}
<file_sep>/SimpleTemplate/util/dualrelay.cpp
#include "dualrelay.h"
#include "WPILib.h"

DualRelay::DualRelay( int forwardPort, int reversePort ){
	
	forward = new Relay( forwardPort, Relay::kForwardOnly );
	reverse = new Relay( reversePort, Relay::kReverseOnly );

}

void DualRelay::Set( Relay::Value value )
{
	if( value == Relay::kForward )
	{
		forward->Set( Relay::kOn );
		reverse->Set( Relay::kOff );
	}
	else if( value == Relay::kReverse )
	{
		forward->Set( Relay::kOff );
		reverse->Set( Relay::kOn );
	}
	else if( value == Relay::kOff )
	{
		forward->Set( Relay::kOff );
		reverse->Set( Relay::kOff );
	}
}
<file_sep>/SimpleTemplate/util/metropidcontroller.h
#ifndef METRO_PID_CONTROLLER_H
#define METRO_PID_CONTROLLER_H

#include "WPILib.h"

class MetroPIDController {

	public:
		enum Modes {
			PID,
			FEED_FORWARD_PID,
			TAKE_BACK_HALF_PID,
			BANG_BANG
		};
		MetroPIDController( double p_, double i_, double d_, Modes mode_, bool isIntegratedOutput_ = false, double period_ = 0.05 );
		~MetroPIDController(){};
		void SetSource( double value );
		double GetOutput();
		void SetSetpoint( double value );
		void SetSetpoint( double value, double ffValue );
		void Enable();
		void Disable();
		bool IsEnabled();
		void Reset();
		
	private:
		void Init( double p_, double i_, double d_, Modes mode_, bool isIntegratedOutput_, double period_ );
		void Run();
		static void CallRun( void *controller );
		double Limit( double input );
		
		MetroPIDController::Modes mode;
		bool isIntegratedOutput;
		
		double p;
		double i;
		double d;
		
		double period;
		bool isEnabled;
		
		double source;
		double result;
		double output;
		double setPoint;
		
		double error;
		double prevError;
		double totalError;
		
		double feedForwardValue;
		
		Notifier *notifier;

};

#endif
<file_sep>/SimpleTemplate/MyRobot.cpp
#include "WPILib.h"

#include "util/gamepad.h"
#include "util/metropidcontroller.h"
#include "util/dualrelay.h"
#include "drive.h"
#include "kicker.h"
#include "pickup.h"
#include <stdio.h>
#include <Math.h>

class CommandBasedRobot : public IterativeRobot {
	private:
        
        typedef enum {Kick, DoubleKick, Forward, NoScript} AutonScript;
        AutonScript script;
        int step;
        Timer *timer, *freshness, *lightTimer;
        
        Talon *flMotor, *blMotor, *frMotor, *brMotor, *kicker1, *kicker2, *pickupMotor;
        Servo *kickerHolder;
        DualRelay *angleMotor, *LED;
        DigitalInput *limitSwitchb, *limitSwitcht, *PiInput;
        DigitalOutput *PiAutonSignal;
        Encoder *flEncoder, *blEncoder, *frEncoder, *brEncoder, *kickerEncoder;
        AnalogChannel *kickerPot;
        Gyro *gyro;
        GamePad *driverGamePad, *kickerGamePad;
        
        Drive *drive;
		Kicker *kicker;
		Pickup *pickup;
		Kinect *kinect;
		Skeleton *skeleton;
        
        DriverStationLCD *ds;
        //NetworkTable *table;
        
        int state;
        
        virtual void RobotInit() {
                
                flMotor = new Talon( 1 );
                blMotor = new Talon( 2 );
                frMotor = new Talon( 3 );
                brMotor = new Talon( 4 );
				kicker1 = new Talon( 5 );
				kicker2 = new Talon( 6 );
				pickupMotor = new Talon( 7 );
				angleMotor = new DualRelay( 1, 1 );
							
                timer = new Timer();
                freshness = new Timer();
                
                limitSwitcht = new DigitalInput( 1 );
                limitSwitchb = new DigitalInput( 2 );
                PiInput = new DigitalInput( 12 );
                
                flEncoder = new Encoder( 13, 14 );
                blEncoder = new Encoder( 5, 6 );
                frEncoder = new Encoder( 7, 8 );
                brEncoder = new Encoder( 9, 10 );
                //kickerEncoder = new Encoder( 11, 12 );
                
                kickerPot = new AnalogChannel(2);
                
                gyro = new Gyro( 1 );
                gyro->Reset();
                
                LED = new DualRelay( 2, 2 );
                
                PiAutonSignal = new DigitalOutput( 11 );
                
                skeleton = new Skeleton();
                
                driverGamePad = new GamePad( 1 );
                kickerGamePad = new GamePad( 2 );
                
                
                drive = new Drive( flMotor, blMotor, frMotor, brMotor, flEncoder, blEncoder, frEncoder, brEncoder, gyro );
                drive->SetInvertedMotors( false, false, true, true );
                
                kicker = new Kicker( kicker1, kicker2, kickerPot );
                pickup = new Pickup( pickupMotor, angleMotor, limitSwitchb, limitSwitcht );
                

                
                ds = DriverStationLCD::GetInstance();

                //table = NetworkTable::GetTable("net");
                ////net->PutNumber("angle", 0.0);
                ////net->PutBoolean("hasAngle", 0.0);
                
                script = Kick;
                step = 0;
                timer->Reset();
                timer->Start();
                
                freshness->Reset();
                freshness->Start();
                
        }
        
        virtual void AutonomousInit() {
                Disable();
                step = 0;
                timer->Reset();
                drive->ResetGyro();
    			
    			drive->SetTargetAngle( drive->GetGyroAngle() );
    			
    			drive->SetPIDControl(false);
    			drive->SetFieldOriented(false);
    						        
    			flEncoder->Reset();
    			blEncoder->Reset();
    			frEncoder->Reset();
    			brEncoder->Reset();
    			
    			kicker->KickerPotVal(kickerPot->GetValue());
    			
    			PiAutonSignal->Set(1);
        }

		void AdvanceStep() {
			step++;
			timer->Reset();
		}
        
        virtual void AutonomousPeriodic() {
			
			if (script == Kick) {
					switch (step) {
					case 0:
						if (kinect->GetSkeleton().GetWristLeft().y > kinect->GetSkeleton().GetShoulderLeft().y || kinect->GetSkeleton().GetWristRight().y > kinect->GetSkeleton().GetShoulderRight().y || timer->Get() >= 6.0) AdvanceStep();
						break;
					case 1:
						drive->SetMecanumXYTurn(0.0, 2.0/3.0, 0.0);
						if (timer->Get() >= 0.7) AdvanceStep();
						break;
					/*case 0:
						drive->SetMecanumXYTurn(0.0, 2.0/3.0, 0.0);
						PiAutonSignal->Set(1);
						break;*/
					/*case 1:
						drive->SetMecanumXYTurn(0.0, 0.0, 0.0);
						
						break;*/
					case 2:
						drive->SetPIDControl(false);
						drive->SetMecanumXYTurn(0.0, 0.0, 0.0);
						kicker->KickBallN();
						if (kicker->State()) AdvanceStep();
						break;
					case 3:
						//PiAutonSignal->Set(0);
						break;
				}
				//drive->SetPIDControl( true );
			}else if (script == DoubleKick) {
				switch (step) {
				case 0:
					kicker->KickBallN();
					if (timer->Get() >= 1) AdvanceStep();
					break;
				case 1:
					drive->SetMecanumXYTurn(0.0, 2.0/3.0, 0.0);
					pickup->RunIntake(-1);
					if (timer->Get() >= 0.5) AdvanceStep();
					break;
				case 2:
					drive->SetMecanumXYTurn(0.0, 0.0, 0.0);
					pickup->Lower();
					if (timer->Get() >= 0.5) AdvanceStep();
					break;
				case 3:
					pickup->Disable();
					kicker->KickBallN();
					if (timer->Get() >= 2.4) AdvanceStep();
					break;
				case 4:
					break;
				}
				//drive->SetPIDControl( true );
			}else if (script == Forward) {
				switch (step) {
				case 0:
					drive->SetMecanumXYTurn(0.0, 2.0/3.0, 0.0);
					if (timer->Get() >= 1.5) AdvanceStep();
					break;
				case 1:
					drive->SetMecanumXYTurn(0.0, 0.0, 0.0);
					break;
				}
				//drive->SetPIDControl( true );
			}
			
			Actuate();
			PrintToDS();
			
		}
        
		
        
			virtual void TeleopInit() {
			                
			        /*drive->SetPIDControl(false);
			        drive->SetFieldOriented(true);
			        
			        flEncoder->Reset();
			        blEncoder->Reset();
			        frEncoder->Reset();
			        brEncoder->Reset();
			        
			        lightTimer->Start();*/
			        
			        //kicker->KickerPotVal(kickerPot->GetValue());
			                
			}
			
			virtual void TeleopPeriodic() {
 
                UpdateOI();
                
                /*
* Reset Gyro
* A: Press to reset gyro
*/
                if( driverGamePad->GetButton( GamePad::A ) ){
                        drive->ResetGyro();
                }
                
                /*
* Slow Drive
* LB: Hold to toggle ON, Release to toggle OFF
*/
                drive->SetSlowDrive( driverGamePad->GetButton( GamePad::LB ) );
                
                /*
* Field Oriented
* START: Press to toggle ON
* BACK: Press to toggle OFF
*/
                drive->SetFieldOriented( driverGamePad->GetButtonDown( GamePad::START ) ? true : ( driverGamePad->GetButtonDown( GamePad::BACK ) ? false : drive->IsFieldOriented() ) );
                
                /*
* PID Control
* X: Press to toggle ON
* Y: Press to toggle OFF
*/
                drive->SetPIDControl( driverGamePad->GetButtonDown( GamePad::X ) ? true : ( driverGamePad->GetButtonDown( GamePad::Y ) ? false : drive->IsPIDControl() ) );

                /*
* Mecanum Drive
* LEFT_X_AXIS: Strafe Left/Right
* LEFT_Y_AXIS: Forward/Reverse
* RIGHT_X_AXIS: Turn
*/

                drive->SetMecanumXYTurn( driverGamePad->GetAxis( GamePad::LEFT_X ), driverGamePad->GetAxis( GamePad::LEFT_Y ), driverGamePad->GetAxis( GamePad::RIGHT_X ) );
                //drive->SetMecanumRLStrafe( driverGamePad->GetAxis( GamePad::LEFT_Y ), driverGamePad->GetAxis( GamePad::RIGHT_Y ), 0);
                        
                /*
* Hold Angle
* RIGHT_JS_BUTTON: Hold to hold current angle
* B: Hold to hold vision target angle
*/
                if( driverGamePad->GetButtonDown( GamePad::RIGHT_JS ) ){
                        drive->SetTargetAngle( drive->GetGyroAngle() );
                }
                if( driverGamePad->GetButton/*Down*/( GamePad::B ) ){
                        drive->SetTargetAngle( HasTarget() ? GetAbsoluteAngle() : drive->GetGyroAngle() );
                }
                drive->SetHoldAngle( driverGamePad->GetButton( GamePad::RIGHT_JS ) || driverGamePad->GetButton( GamePad::B ) || driverGamePad->GetAxis(GamePad::DPAD_X) != 0.0 );

                if(driverGamePad->GetDPadLeftDown()){
                        drive->SetTargetAngle( drive->GetGyroAngle() - Drive::AIM_BIAS_INCREMENT);
                }
                if(driverGamePad->GetDPadRightDown()){
                        drive->SetTargetAngle( drive->GetGyroAngle() + Drive::AIM_BIAS_INCREMENT);
                }
                
                /*
                 * Kick Ball
                 * A: Pull back kicker and kick ball
                 * X: Raise leg
                 */
                
                if( kickerGamePad->GetButton( GamePad::A )) {
                	kicker->KickBallN();
                }
                
                /*if( kickerGamePad->GetButton( GamePad::X )) {
                	kicker->RaiseLeg();
                }else if ( !kickerGamePad->GetButton( GamePad::X )){
                	kicker->StopRaise();
                }*/
                
                
                
                //pickup->ArmAngle( kickerGamePad->GetAxis( GamePad::LEFT_Y ));

                //pickup->RunIntake( -kickerGamePad->GetAxis( GamePad::LEFT_Y ));
                
                /*
                 * Run intake
                 * LB: Take in ball
                 * RB: Spit out ball
                 */
                
                if ( kickerGamePad->GetButton( GamePad::LB )) {
                	pickup->Intake();
                }else if ( kickerGamePad->GetButton( GamePad::RB )){
                	pickup->Expel();
                }else /*if ( kickerGamePad->GetButton( GamePad::START ))*/ {
                	pickup->StopIntake();
                }
                
                /*
                 * Raise Pickup Mechanism
                 * LEFT_Y UP: Raise arm
                 * LEFT_Y DOWN: Lower arm
                 * Y: Raise arm to top
                 * B: Lower arm to bottom
                 */
                
                
                if( kickerGamePad->GetAxis( GamePad::LEFT_Y ) > 0.0 ) {
                	pickup->Raise();
                }else if( kickerGamePad->GetAxis( GamePad::LEFT_Y ) < 0.0 ) {
                	pickup->Lower();
                }else if( kickerGamePad->GetAxis( GamePad::LEFT_Y ) == 0.0 && !pickup->AllWay()){
                	pickup->StopAngle();
                }
                
                if( kickerGamePad->GetButton( GamePad::Y )) {
                   	pickup->ToTop();
                }
                if( kickerGamePad->GetButton( GamePad::B )) {
                   	pickup->ToBottom();
                }
                
                if( kickerGamePad->GetButton( GamePad::RIGHT_JS )) {
                	pickup->Kick();
                }
                
                if( pickup->isKicking() && !limitSwitcht->Get()) {
                	kicker->KickBallN();
                }
                
                if( kickerGamePad->GetButton( GamePad::RIGHT_JS ) || driverGamePad->GetButton( GamePad::RB )) {
                	if (lightTimer->Get() >= 0.25) {
                		LED->Set(Relay::kOff);
                		lightTimer->Reset();
                	}else{
                		LED->Set(Relay::kForward);
                	}
                }else if(kicker->State()){
                	LED->Set(Relay::kOff);
                }else{
                	LED->Set(Relay::kForward);
                }
                
                if( kickerGamePad->GetButton( GamePad::X )) kicker->KickerPotVal(kickerPot->GetValue());
                
                if( kickerGamePad->GetAxis( GamePad::DPAD_X ) != 0) PiAutonSignal->Set(1);
                else PiAutonSignal->Set(0);
                
                Actuate();
                PrintToDS();
                
			}
        
        virtual void DisabledInit() {
                
                Disable();
                
        }
        
        virtual void DisabledPeriodic() {
                
                UpdateOI();
					
                if (driverGamePad->GetButton( GamePad::A )) {
					script = Kick;
				}else if (driverGamePad->GetButton( GamePad::B )) {
					script = DoubleKick;
				}else if (driverGamePad->GetButton( GamePad::X )) {
					script = Forward;
				}
                
                drive->SetPIDControl( driverGamePad->GetButtonDown( GamePad::X ) ? true : ( driverGamePad->GetButtonDown( GamePad::Y ) ? false : drive->IsPIDControl() ) );
                
                drive->SetFieldOriented( driverGamePad->GetButtonDown( GamePad::START ) ? true : ( driverGamePad->GetButtonDown( GamePad::BACK ) ? false : drive->IsFieldOriented() ) );
                
                if( kickerGamePad->GetButton( GamePad::X )) kicker->KickerPotVal(kickerPot->GetValue());
                

                PrintToDS();
                
        }
        
        void Actuate(){
                
                drive->Actuate();
                kicker->Actuate();
                pickup->Actuate();
        }
        
        void Disable(){
                
                drive->Disable();
                kicker->Disable();
                pickup->Disable();
        }
        
        void UpdateOI(){

                driverGamePad->Update();
                kickerGamePad->Update();
                //IsFreshTarget();
                
        }
        
        void PrintToDS(){
                
                ds->Clear();
                ds->Printf(DriverStationLCD::kUser_Line1, 1, "Auto: %s", script == Kick ? "Single Kick" : ( script == DoubleKick ? "DoubleKick" : ( script == Forward ? "Forward" :( script == NoScript ? "None" : "YOU BROKE IT" ) ) ) );
                ds->Printf(DriverStationLCD::kUser_Line2, 1, "Dr: %s%s %f", drive->IsPIDControl() ? "PID, " : "", drive->IsFieldOriented() ? "FO, " : "", drive->gyro->GetAngle() );
                ds->Printf(DriverStationLCD::kUser_Line3, 1, "Vi: %s, Fresh: %f", HasTarget() ? "Y" : "N", freshness->Get() );
                ds->Printf(DriverStationLCD::kUser_Line4, 1, "%d", kickerPot->GetValue() );
                ds->Printf(DriverStationLCD::kUser_Line5, 1, "%d : %d : %d : %d", flEncoder->Get(), blEncoder->Get(), frEncoder->Get(), brEncoder->Get() );
                //ds->Printf(DriverStationLCD::kUser_Line6, 1, "%d, %d, %f", PiInput->Get(), step, timer->Get() );
                ds->Printf(DriverStationLCD::kUser_Line6, 1, "%f", ((kinect->GetSkeleton().GetWristLeft().y - kinect->GetSkeleton().GetShoulderLeft().y)) );
                ds->UpdateLCD();
                
        }
        
        double GetAbsoluteAngle() {
                return /*table->GetNumber("angle", 0.0) +*/ drive->GetGyroAngle();
        }
        
        double GetRelativeAngle() {
                return 0.0;//table->GetNumber("angle", 0.0);
        }
        
        bool HasTarget(){
                return false;//table->GetBoolean("hasTarget", false);
        }
        
        bool IsFreshTarget() {
                bool isFresh = false;//table->GetBoolean("isFresh", false);
                if(isFresh) {
                        freshness->Reset();
                }
                //table->PutBoolean("isFresh", false);
                return isFresh;
        }
        
        double AngleDiff( double angle1, double angle2 ){
                return fmod(angle1 - angle2 + 180.0, 360.0) - 180.0;
        }
	
};


START_ROBOT_CLASS(CommandBasedRobot);
<file_sep>/SimpleTemplate/util/metropidcontroller.cpp
#include "metropidcontroller.h"

MetroPIDController::MetroPIDController( double p_, double i_, double d_, Modes mode_, bool isIntegratedOutput_, double period_ ){

	p = p_;
	i = i_;
	d = d_;
	
	mode = mode_;
	isIntegratedOutput = isIntegratedOutput_;
	period = period_;
	
	notifier = new Notifier( MetroPIDController::CallRun , this );
		
	source = 0.0;
	result = 0.0;
	output = 0.0;
	setPoint = 0.0;
	
	error = 0.0;
	prevError = 0.0;
	totalError = 0.0;
	feedForwardValue = 0.0;

	isEnabled = false;

	notifier->StartPeriodic( period );

}

void MetroPIDController::CallRun( void *controller ){

	MetroPIDController *control = (MetroPIDController*) controller;
	control->Run();
	
}

void MetroPIDController::Run(){

	Synchronized s(semaphore);
	
	if( isEnabled ){
	
		error = setPoint - source;
		totalError += error;
		
		if ( mode == TAKE_BACK_HALF_PID && ( error * prevError < 0 || error == 0 ) ) {
                
            feedForwardValue = ( feedForwardValue + output ) / 2;
                
        }

        result = ( error * p ) + ( totalError * i ) + ( (error-prevError) * d ) + ( isIntegratedOutput ? result : 0.0 );
        result = Limit( result );    
        
    	output = result;
            
        if ( mode == FEED_FORWARD_PID || mode == TAKE_BACK_HALF_PID ) {
                
            output += feedForwardValue;
                
        }
            
        if( mode == BANG_BANG ){
                
            output = ( source < setPoint ? 1.0 : 0.0 );
                
        }
		
		prevError = error;
	
	}

}

void MetroPIDController::SetSource( double value ){

	Synchronized s(semaphore);
	source = value;

}

double MetroPIDController::GetOutput(){

	Synchronized s(semaphore);
	return output;

}

void MetroPIDController::SetSetpoint( double value ){

	Synchronized s(semaphore);
	setPoint = value;

}

void MetroPIDController::SetSetpoint( double value, double ffValue ){

	Synchronized s(semaphore);
	setPoint = value;
	feedForwardValue = ffValue;

}

bool MetroPIDController::IsEnabled(){

	Synchronized s(semaphore);
	return isEnabled;

}

void MetroPIDController::Enable(){

	Synchronized s(semaphore);
	isEnabled = true;

}

void MetroPIDController::Disable(){

	Synchronized s(semaphore);
	output = 0.0;
	isEnabled = false;
	Reset();

}

double MetroPIDController::Limit( double input ){
	
	if( input > 1.0 ){
		return 1.0;
	}
	if( input < -1.0 ){
		return -1.0;
	}
	return input;
	
}

void MetroPIDController::Reset(){

	Synchronized s(semaphore);
	output = 0.0;
	prevError = 0.0;
	totalError = 0.0;
	setPoint = 0.0;
	isEnabled = false;

}
