<repo_name>Robotics-Team-5268/RobotCode2019<file_sep>/src/main/include/commands/MoveElevator.h
#pragma once

#include "CommandBase.h"

class MoveElevator : public CommandBase {
 private:
  double speed;
 public:
  MoveElevator(double spd);
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
};
<file_sep>/src/main/include/commands/Autonomous.h
#pragma once

#include <frc/commands/Command.h>
#include "CommandBase.h"

class Autonomous : public CommandBase {
 public:
  Autonomous(int selection);
};
<file_sep>/src/main/cpp/commands/DriveWithJoystick.cpp
#include "commands/DriveWithJoystick.h"

DriveWithJoystick::DriveWithJoystick() : CommandBase("DriveWithJoystick") {
  Requires(drive.get());
}

void DriveWithJoystick::Initialize() {}

void DriveWithJoystick::Execute() {
  drive->setMotors(
    -CommandBase::oi->getDriverJoystick()->GetRawAxis(1) ,
	  -CommandBase::oi->getDriverJoystick()->GetRawAxis(5)
    // 0.0,0.0
  );
}

bool DriveWithJoystick::IsFinished() { return true; }

void DriveWithJoystick::End() {}

void DriveWithJoystick::Interrupted() {}
<file_sep>/src/main/include/subsystems/MB1013Ultrasonic.h
#pragma once

#include <frc/commands/Subsystem.h>

#include "RobotMap.h"

class MB1013Ultrasonic : public frc::Subsystem {
 private:
  frc::AnalogInput ultrasonic{ULTRASONIC};
 public:
  MB1013Ultrasonic();
  double getVoltage();
};

/*
.29-.30 = 1 ft
.41-.42 = 1-1/2 ft
.58-.60 = 2 ft
.72-.72 = 2-1/2 ft
.86-.87 = 3 ft
1.09-1.10 = 4 ft
*/<file_sep>/src/main/cpp/commands/GrabberCommand.cpp
#include "commands/GrabberCommand.h"
#include "CommandBase.h"

GrabberCommand::GrabberCommand(std::string drct) {
  // Use Requires() here to declare subsystem dependencies
  // eg. Requires(Robot::chassis.get());
  Requires(CommandBase::grabber.get());
  direction = drct;
}

// Called just before this Command runs the first time
void GrabberCommand::Initialize() {}

// Called repeatedly when this Command is scheduled to run
void GrabberCommand::Execute() {
  if(direction == "Output"){
    CommandBase::grabber->GrabberOutput();
  }
  if(direction == "Intake"){
    CommandBase::grabber->GrabberIntake();
  }
}

// Make this return true when this Command no longer needs to run execute()
bool GrabberCommand::IsFinished() { return false; }

// Called once after isFinished returns true
void GrabberCommand::End() {
  CommandBase::grabber->GrabberOff();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void GrabberCommand::Interrupted() {}
<file_sep>/src/main/include/commands/Move.h
#pragma once

#include <frc/commands/Command.h>
#include <frc/Timer.h>
#include <frc/WPILib.h>

#include "CommandBase.h"

class Move : public CommandBase {
 public:
  Move(float tm, float spd);
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
  float speed;
  float seconds;
  std::shared_ptr<frc::Timer> timer;
  frc::PIDController* pid;
};

class MovePIDOutput: public frc::PIDOutput {
  public:
    MovePIDOutput(double sp);
    virtual ~MovePIDOutput();
    void PIDWrite(double a);
    float speed;
};
<file_sep>/src/main/include/CommandBase.h
#pragma once

#include <memory>

#include <frc/commands/CommandGroup.h>
#include <string.h>

#include "subsystems/UDPReceiver.h"
#include "subsystems/Drive.h"
#include "subsystems/Sighting.h"
#include "subsystems/Encoders.h"
#include "subsystems/Elevator.h"
#include "subsystems/Grabber.h"
#include "subsystems/Arm.h"
#include "subsystems/Pneumatics.h"
#include "subsystems/MB1013Ultrasonic.h"
#include "OI.h"

class CommandBase : public frc::CommandGroup {
 public:
  CommandBase(const std::string &name);
  CommandBase();
  static void init();

  
  static std::unique_ptr<Drive> drive;
  static std::unique_ptr<Encoders> leftEncoder;
  static std::unique_ptr<Encoders> rightEncoder;
  static std::unique_ptr<Encoders> elevatorEncoder;
  static std::unique_ptr<MB1013Ultrasonic> ultrasonic;
  static std::unique_ptr<Elevator> elevator;
  static std::unique_ptr<Grabber> grabber;
  static std::unique_ptr<Pneumatics> pneumatics;
  static std::unique_ptr<Arm> arm;
  static std::unique_ptr<OI> oi;
  static std::unique_ptr<UDPReceiver> udp;
  static std::unique_ptr<Sighting> sight;
};
<file_sep>/src/main/cpp/commands/MovePneumatics.cpp
#include "commands/MovePneumatics.h"
#include "CommandBase.h"

MovePneumatics::MovePneumatics(frc::DoubleSolenoid::Value v) : CommandBase("MovePneumatics") {
    Requires(CommandBase::pneumatics.get());
    value = v;
}

void MovePneumatics::Initialize() {}

void MovePneumatics::Execute() {
    if (value == frc::DoubleSolenoid::kForward) {
        CommandBase::pneumatics->Forward();
    } else if (value == frc::DoubleSolenoid::kReverse){
        CommandBase::pneumatics->Reverse();
    } else {
        CommandBase::pneumatics->Off();
    }
}

bool MovePneumatics::IsFinished() { 
    return true; 
}

void MovePneumatics::End() {}

void MovePneumatics::Interrupted() {}
<file_sep>/src/main/include/subsystems/Drive.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/Ultrasonic.h>

#include "RobotMap.h"

class Drive : public frc::Subsystem {
 private:
    frc::Talon speedControllerFL{DRIVE_SPEED_CONTROLLER_FL_CHANNEL};
	frc::Talon speedControllerBL{DRIVE_SPEED_CONTROLLER_BL_CHANNEL};
	frc::SpeedControllerGroup leftSC{speedControllerFL, speedControllerBL};

	frc::Talon speedControllerFR{DRIVE_SPEED_CONTROLLER_FR_CHANNEL};
	frc::Talon speedControllerBR{DRIVE_SPEED_CONTROLLER_BR_CHANNEL};
	frc::SpeedControllerGroup rightSC{speedControllerFR, speedControllerBR};

	frc::DifferentialDrive diffDrive{leftSC, rightSC};
	float oldLeftSpeed, oldRightSpeed;
	const float MAX_CHANGE = .05;
	const float MAX_SPEED = 3000;//used to be 3000
	float velocityToCommandSlope[4];
	float velocityToCommandIntercept[4];

	frc::AnalogGyro gyro{GYRO_ANALOG_PORT};
 	


    //std::ofstream fout;
 public:
	Drive();
	void InitDefaultCommand() override;
	virtual void setMotors(float leftSpeed, float rightSpeed);
	virtual void safetyOff() {diffDrive.SetSafetyEnabled(false);}
	virtual void takeInput();
	frc::AnalogGyro* getGyro();
	float getGyroAngle();
	float getGyroRate();
	void resetGyro();
	void calibrateGyro();
	void SetVelocity(float left, float right);
};<file_sep>/src/main/cpp/subsystems/REVDigitBoard.cpp
#include <frc/I2C.h>
#include <frc/Timer.h>
#include <frc/DigitalInput.h>
#include <frc/AnalogInput.h>
#include "Subsystems/RevDigitBoard.h"
#include <string.h>
#include <cctype>

REVDigitBoard::REVDigitBoard() : frc::Subsystem("REVDigitBoard")
		, i2c( frc::I2C::kMXP, 0x70 )
		, buttonA( 19 )
		, buttonB( 20 )
		, pot( 3 )
	{
		byte osc;
	 	byte blink;
	 	byte bright;
	 	osc = (byte)0x21;
	 	blink = (byte)0x81;
	 	bright = (byte)0xEF;

		i2c.WriteBulk(&osc, 1);
		frc::Wait(.01);
		i2c.WriteBulk(&bright, 1);
	  frc::Wait(.01);
		i2c.WriteBulk(&blink, 1);
		frc::Wait(.01);

		charreg[0][0] = (byte)0b00111111; charreg[9][1] = (byte)0b00000000; //0
		charmap['0'] = 0;
		charreg[1][0] = (byte)0b00000110; charreg[0][1] = (byte)0b00000000; //1
		charmap['1'] = 1;
	 	charreg[2][0] = (byte)0b11011011; charreg[1][1] = (byte)0b00000000; //2
		charmap['2'] = 2;
	 	charreg[3][0] = (byte)0b11001111; charreg[2][1] = (byte)0b00000000; //3
		charmap['3'] = 3;
	 	charreg[4][0] = (byte)0b11100110; charreg[3][1] = (byte)0b00000000; //4
		charmap['4'] = 4;
	 	charreg[5][0] = (byte)0b11101101; charreg[4][1] = (byte)0b00000000; //5
		charmap['5'] = 5;
	 	charreg[6][0] = (byte)0b11111101; charreg[5][1] = (byte)0b00000000; //6
		charmap['6'] = 6;
	 	charreg[7][0] = (byte)0b00000111; charreg[6][1] = (byte)0b00000000; //7
		charmap['7'] = 7;
	 	charreg[8][0] = (byte)0b11111111; charreg[7][1] = (byte)0b00000000; //8
		charmap['8'] = 8;
	 	charreg[9][0] = (byte)0b11101111; charreg[8][1] = (byte)0b00000000; //9
		charmap['9'] = 9;

	 	charreg[10][0] = (byte)0b11110111; charreg[10][1] = (byte)0b00000000; //A
		charmap['A'] = 10;
	 	charreg[11][0] = (byte)0b10001111; charreg[11][1] = (byte)0b00010010; //B
		charmap['B'] = 11;
	 	charreg[12][0] = (byte)0b00111001; charreg[12][1] = (byte)0b00000000; //C
		charmap['C'] = 12;
	 	charreg[13][0] = (byte)0b00001111; charreg[13][1] = (byte)0b00010010; //D
		charmap['D'] = 13;
	 	charreg[14][0] = (byte)0b11111001; charreg[14][1] = (byte)0b00000000; //E
		charmap['E'] = 14;
	 	charreg[15][0] = (byte)0b11110001; charreg[15][1] = (byte)0b00000000; //F
		charmap['F'] = 15;
	 	charreg[16][0] = (byte)0b10111101; charreg[16][1] = (byte)0b00000000; //G
		charmap['G'] = 16;
	 	charreg[17][0] = (byte)0b11110110; charreg[17][1] = (byte)0b00000000; //H
		charmap['H'] = 17;
	 	charreg[18][0] = (byte)0b00001001; charreg[18][1] = (byte)0b00010010; //I
		charmap['I'] = 18;
	 	charreg[19][0] = (byte)0b00011110; charreg[19][1] = (byte)0b00000000; //J
		charmap['J'] = 19;
	 	charreg[20][0] = (byte)0b01110000; charreg[20][1] = (byte)0b00001100; //K
		charmap['K'] = 20;
	 	charreg[21][0] = (byte)0b00111000; charreg[21][1] = (byte)0b00000000; //L
		charmap['L'] = 21;
	 	charreg[22][0] = (byte)0b00110110; charreg[22][1] = (byte)0b00000101; //M
		charmap['M'] = 22;
	 	charreg[23][0] = (byte)0b00110110; charreg[23][1] = (byte)0b00001001; //N
		charmap['N'] = 23;
	 	charreg[24][0] = (byte)0b00111111; charreg[24][1] = (byte)0b00000000; //O
	 	charmap['O'] = 24;
	 	charreg[25][0] = (byte)0b11110011; charreg[25][1] = (byte)0b00000000; //P
	 	charmap['P'] = 25;
	 	charreg[26][0] = (byte)0b00111111; charreg[26][1] = (byte)0b00001000; //Q
	 	charmap['Q'] = 26;
	 	charreg[27][0] = (byte)0b11110011; charreg[27][1] = (byte)0b00001000; //R
	 	charmap['R'] = 27;
	 	charreg[28][0] = (byte)0b10001101; charreg[28][1] = (byte)0b00000001; //S
	 	charmap['S'] = 28;
	 	charreg[29][0] = (byte)0b00000001; charreg[29][1] = (byte)0b00010010; //T
	 	charmap['T'] = 29;
	 	charreg[30][0] = (byte)0b00111110; charreg[30][1] = (byte)0b00000000; //U
	 	charmap['U'] = 30;
	 	charreg[31][0] = (byte)0b00110000; charreg[31][1] = (byte)0b00100100; //V
	 	charmap['V'] = 31;
	 	charreg[32][0] = (byte)0b00110110; charreg[32][1] = (byte)0b00101000; //W
	 	charmap['W'] = 32;
	 	charreg[33][0] = (byte)0b00000000; charreg[33][1] = (byte)0b00101101; //X
	 	charmap['X'] = 33;
	 	charreg[34][0] = (byte)0b00000000; charreg[34][1] = (byte)0b00010101; //Y
	 	charmap['Y'] = 34;
	 	charreg[35][0] = (byte)0b00001001; charreg[35][1] = (byte)0b00100100; //Z
	 	charmap['Z'] = 35;
		charreg[36][0] = (byte)0b00000000; charreg[36][1] = (byte)0b00000000; //space
		charmap[' '] = 36;
	}

	void REVDigitBoard::InitDefaultCommand() {}

	void REVDigitBoard::display(std::string str) { // only displays first 4 chars
		byte charz[4] = {36,36,36,36};

		for(size_t i = 0; i < 4 && i < str.length(); i++) {
			charz[i] = charmap[std::toupper(str[i])];
		}

		_display(charz);
	}

	void REVDigitBoard::display(double batt) { // optimized for battery voltage, needs a double like 12.34
		byte charz[4] = {36,36,36,36};
		// idk how to decimal point
		int ten = (int)(batt/10);
		int one = ((int)batt)%10;
		int tenth = ((int)(batt*10)%10);
		int hundredth = ((int)(batt*100)%10);

		if (ten != 0) charz[0] = ten;
		charz[1] = one;
		charz[2] = tenth;
		charz[3] = hundredth;

		_display(charz);
	}

	 void REVDigitBoard::clear() {
		 byte charz[4] = {36,36,36,36};
		 _display(charz);
	 }

	 bool REVDigitBoard::getButtonA() {
		 return !buttonA.Get();// Not symbol because digital input is true when open
	 }
	 bool REVDigitBoard::getButtonB() {
		 return !buttonB.Get();// Not symbol because digital input is true when open
	 }
	 double REVDigitBoard::getPot() {
		 return pot.GetVoltage();
	 }

////// not supposed to be publicly used..

	void REVDigitBoard::_display( byte charz[4]) {
		byte byte1[10];
		byte1[0] = (byte)(0b00001111);
		byte1[1] = 0; //(byte)(0b00001111);
 		byte1[2] = charreg[charz[3]][0];
 		byte1[3] = charreg[charz[3]][1];
 		byte1[4] = charreg[charz[2]][0];
 		byte1[5] = charreg[charz[2]][1];
 		byte1[6] = charreg[charz[1]][0];
 		byte1[7] = charreg[charz[1]][1];
 		byte1[8] = charreg[charz[0]][0];
 		byte1[9] = charreg[charz[0]][1];
 		//send the array to the board
 		i2c.WriteBulk(byte1, sizeof( byte1 ));
 		frc::Wait(0.01);
	}


<file_sep>/src/main/cpp/commands/RampUpVelocityTest.cpp
#include "commands/RampUpVelocityTest.h"

RampUpVelocityTest::RampUpVelocityTest() 
: fout("/home/lvuser/TestValue.csv"),
pdp()
{
  driveCommand = 0.0;
  count = 0;
  fout << "Percent Power,Left Velocity,Right Velocity" << '\n';
}

void RampUpVelocityTest::Initialize() {}

void RampUpVelocityTest::Execute() {
    if (count == 10) {
      driveCommand += 0.02;
      count = 0;
    } else {
      count++;
    }
    drive->setMotors(driveCommand, driveCommand);
    double left = CommandBase::leftEncoder->getRate();
    double right = CommandBase::rightEncoder->getRate();
    fout << driveCommand << "," << left << "," << right << '\n';
}

bool RampUpVelocityTest::IsFinished() {
    return (4 <= count && driveCommand >= 1.0);
}

void RampUpVelocityTest::End() {
    fout.close();
}

void RampUpVelocityTest::Interrupted() {
    fout.close();
}<file_sep>/src/main/include/subsystems/Encoders.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/WPILib.h>

class Encoders : public frc::Subsystem {
 private:
 
 public:
  double getCount() const;
	double getRaw() const;
	double getDistance() const;
	double getRate() const; //const keyword is placed afterwards so that the accessor method will be checked to make sure it doesn't modify variable values.
	double getDirection() const;
	double getStopped() const;
	void reset() const;
	Encoders(std::string shortName, unsigned int channelA, unsigned int channelB, bool reverseDirection);
  private:
  const double MAX_PERIOD = .1;
	const double MIN_RATE = .05;
	const double PULSEINCHES = .00920; //this is the appproximate value for the magnetic encoder. gear ratio for tike is 12.75 and 10.71 on the actual robot18
	const double SAMPLES_TO_AVE = 7;
	frc::Encoder *encoder;
};
<file_sep>/src/main/cpp/commands/DriveStraightDistance.cpp
#include <frc/WPILib.h>
#include "commands/DriveStraightDistance.h"
#include "subsystems/Encoders.h"
#include "commands/Velocity.h"



void DriveStraightDistance::End(){
    velocity->done = true;
    //velocity->End();
}

void DriveStraightDistance::Initialize(){
    velocity = new Velocity();
    //velocity->Initialize();
    velocity->Start();
    //AddSequential(velocity);
    startingLeft = CommandBase::leftEncoder->getDistance();
    startingRight = CommandBase::rightEncoder->getDistance();
}

void DriveStraightDistance::Execute(){
    /*if (velocity == nullptr){
        return;
    }*/
    double currentDistance = (CommandBase::leftEncoder->getDistance() + (CommandBase::rightEncoder->getDistance() * 7)) / 2;
    double velocityPIDInput = 0;
    if (currentDistance < ACCELERATION_DISTANCE) {
        //This is when we start accelerating
        velocityPIDInput = ACCELERATION * currentDistance;
        if(velocityPIDInput < .1){ velocityPIDInput = .1;}
    } else if (currentDistance >= ACCELERATION_DISTANCE && currentDistance < distance - DECELERATION_DISTANCE) {
        //This is when we stay at a constant velocity
        velocityPIDInput = MAX_VELOCITY;
    } else {
        //This is when we to start to deaccelerate
        velocityPIDInput = -MAX_VELOCITY * ((currentDistance - distance)/DECELERATION_DISTANCE);
    }
    frc::SmartDashboard::PutNumber("Setpoint", velocityPIDInput);

    velocity->SetSetpoint(velocityPIDInput);
    //velocity->Execute();
}

bool DriveStraightDistance::IsFinished(){
    if ((CommandBase::leftEncoder->getDistance() + (CommandBase::rightEncoder->getDistance() * 7)) / 2 > distance) {
        return true;
    }
    return false;
}

DriveStraightDistance::DriveStraightDistance(double distance) : CommandBase("DriveStraightDistance") {
    this->distance = distance;
    frc::SmartDashboard::PutNumber("distance", distance);
}<file_sep>/src/main/cpp/commands/Move.cpp
#include "commands/Move.h"

Move::Move(float tm, float spd) : CommandBase() {
  Requires(drive.get());
  pid = nullptr;
  seconds = tm;
  speed = spd;
  timer.reset(new frc::Timer());
}

void Move::Initialize() {}

void Move::Execute() {
  if(pid) {
    
  }
}

bool Move::IsFinished() { 
  return timer->HasPeriodPassed(seconds); 
  }

void Move::End() {}

void Move::Interrupted() {}
<file_sep>/src/main/cpp/subsystems/Elevator.cpp
#include "subsystems/Elevator.h"
#include "CommandBase.h"

Elevator::Elevator() : Subsystem("Elevator")  {

}

void Elevator::InitDefaultCommand(){

}

void Elevator::MoveMotor(double speed) {
    elevator_SC.Set(speed);
}<file_sep>/src/main/include/subsystems/Grabber.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/drive/DifferentialDrive.h>

#include "RobotMap.h"

//I want to make take and place item commands into one class so you will see a new command called GrabberCommand
//I left the two commands you made but if you want to get rid of them go ahead
//I feel that lower level things like what direction the wheels spin should be handled at the subsystem level
//which is why I made two methods in this class, one to handle input and another for output
//the third method is just called at the end of the GrabberCommand and sets the motors to 0 so they don't run indefinitly
class Grabber : public frc::Subsystem {
 private:
  frc::Talon speedControllerL{GRABBER_SPEED_CONTROLLER_L_CHANNEL};
	frc::Talon speedControllerR{GRABBER_SPEED_CONTROLLER_R_CHANNEL};
  frc::SpeedControllerGroup GrabberSCG{speedControllerL, speedControllerR};
 public:
  Grabber();
  void GrabberIntake();
  void GrabberOutput();
  void GrabberOff();
};
<file_sep>/src/main/include/OI.h
#pragma once

#include "frc/WPILib.h"

class OI {
 private:
  std::shared_ptr<frc::Joystick> driverJoystick, mechanismsJoystick;
  std::vector<frc::JoystickButton*> driverBtns, mechanismsBtns;
 public:
  std::shared_ptr<frc::Joystick> getDriverJoystick();
  OI();
};
<file_sep>/src/main/cpp/commands/VelocityTest.cpp
#include "commands/VelocityTest.h"

VelocityTest::VelocityTest() 
: fout("/home/lvuser/TestValue.csv"),
pdp()
{
  driveCommand = 0.6;
  count = 0;
  fout << "Front Left,Front Right,Back Left,Back Right,LeftEncoderVelocity,RightEncoderVelocity" << '\n';
}

void VelocityTest::Initialize() {}

void VelocityTest::Execute() {
    count++;
    drive->SetVelocity(driveCommand, driveCommand);
    double left = CommandBase::leftEncoder->getRate();
    double right = CommandBase::rightEncoder->getRate();
    fout << pdp.GetCurrent(1) << "," << pdp.GetCurrent(2) << "," << pdp.GetCurrent(0) << "," << pdp.GetCurrent(3) << "," << left << "," << right << '\n';
}

bool VelocityTest::IsFinished() {
    return (200 <= count);
}

void VelocityTest::End() {
    fout.close();
}

void VelocityTest::Interrupted() {}

<file_sep>/src/main/include/commands/GrabberCommand.h
#pragma once

#include <frc/commands/Command.h>

class GrabberCommand : public frc::Command {
 private:
  std::string direction;
 public:
  GrabberCommand(std::string drct);
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
};
<file_sep>/src/main/cpp/Robot.cpp
#include "Robot.h"
#include "subsystems/Drive.h"
#include "CommandBase.h"

#include <frc/commands/Scheduler.h>
#include <frc/smartdashboard/SmartDashboard.h>

std::unique_ptr<Autonomous> Robot::autonomousCommand;
AutonomousChooser Robot::AutoChooser;

void Robot::RobotInit() {
    CommandBase::init();
	CommandBase::drive->calibrateGyro();
	CommandBase::drive->safetyOff();
}

void Robot::RobotPeriodic() {}

void Robot::DisabledInit() {}

void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }

void Robot::AutonomousInit() {
	autonomousCommand.reset(new Autonomous(AutoChooser.AutonomousSelection()));
	autonomousCommand->Start();
}

void Robot::AutonomousPeriodic() { 
    frc::Scheduler::GetInstance()->Run();
    AddSmartDashboardItems();
}

void Robot::TeleopInit() {
	//frc::SmartDashboard::PutNumber("breakpoint", 200);
	CommandBase::drive->resetGyro();
	CommandBase::drive->setMotors(0.0, 0.0);
}

void Robot::TeleopPeriodic() { 
    frc::Scheduler::GetInstance()->Run(); 
    AddSmartDashboardItems();
}

void Robot::TestPeriodic() {}

void Robot::AddSmartDashboardItems() {
	// frc::SmartDashboard::PutValue("Solenoid Value", CommandBase::pneumatics->getValue())
	frc::SmartDashboard::PutNumber("Gyro Angle", CommandBase::drive->getGyroAngle());
	frc::SmartDashboard::PutNumber("Gyro Rate", CommandBase::drive->getGyroRate());
	frc::SmartDashboard::PutNumber("rightcount", CommandBase::rightEncoder->getCount());
	frc::SmartDashboard::PutNumber("rightRaw Count", CommandBase::rightEncoder->getRaw());
	frc::SmartDashboard::PutNumber("rightDistance", CommandBase::rightEncoder->getDistance()*7);
	frc::SmartDashboard::PutNumber("rightRate", CommandBase::rightEncoder->getRate());
	frc::SmartDashboard::PutBoolean("rightDirection", CommandBase::rightEncoder->getDirection());
	frc::SmartDashboard::PutBoolean("rightStopped", CommandBase::rightEncoder->getStopped());
	frc::SmartDashboard::PutNumber("leftcount", CommandBase::leftEncoder->getCount());
	frc::SmartDashboard::PutNumber("leftRaw Count", CommandBase::leftEncoder->getRaw());
	frc::SmartDashboard::PutNumber("leftDistance", CommandBase::leftEncoder->getDistance());
	frc::SmartDashboard::PutNumber("leftRate", CommandBase::leftEncoder->getRate());
	frc::SmartDashboard::PutBoolean("leftDirection", CommandBase::leftEncoder->getDirection());
	frc::SmartDashboard::PutBoolean("leftStopped", CommandBase::leftEncoder->getStopped());
	frc::SmartDashboard::PutNumber("Ultrasonic", CommandBase::ultrasonic->getVoltage());
	frc::SmartDashboard::PutNumber("distance traveled with drive straight", (CommandBase::leftEncoder->getDistance() + (CommandBase::rightEncoder->getDistance() * 7)) / 2);
}

#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif
<file_sep>/src/main/include/commands/DriveWithJoystick.h
#pragma once

#include "CommandBase.h"

class DriveWithJoystick : public CommandBase {
 public:
  DriveWithJoystick();
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
};
<file_sep>/src/main/cpp/commands/MoveElevator.cpp
#include "commands/MoveElevator.h"
#include "subsystems/Elevator.h"

MoveElevator::MoveElevator(double spd) : CommandBase("MoveElevator") {
  Requires(CommandBase::elevator.get());
  speed = spd;
}

void MoveElevator::Initialize() {
}

void MoveElevator::Execute() {
  CommandBase::elevator->MoveMotor(speed);
}

bool MoveElevator::IsFinished() {
  return true;
}

void MoveElevator::End() {
  CommandBase::elevator->MoveMotor(0.0);
}

void MoveElevator::Interrupted() {}
<file_sep>/src/main/include/commands/DriveStraightDistance.h
#ifndef DRIVE_STRAIGHT_DISTANCE_H
#define DRIVE_STRAIGHT_DISTANCE_H
#define MAX_VELOCITY .3
#define ACCELERATION_DISTANCE 20
#define DECELERATION_DISTANCE 47
#define ACCELERATION (MAX_VELOCITY / ACCELERATION_DISTANCE)
#define DECELERATION (MAX_VELOCITY / DECELERATION_DISTANCE)
#include "CommandBase.h"
#include "commands/Velocity.h"

class DriveStraightDistance : public CommandBase {
private:   
    double distance, startingRight, startingLeft;
    Velocity* velocity;
public:
    virtual void End();
    virtual void Initialize();
    virtual void Execute();
    virtual bool IsFinished();
    DriveStraightDistance(double distance);
};

#endif<file_sep>/src/main/cpp/subsystems/Arm.cpp
#include "subsystems/Arm.h"

Arm::Arm() : Subsystem("Arm") {

}

void Arm::MoveArm() {
  arm_SC.Set(0.4);
}

int Arm::GetCount() {
  return hallSensor.Get();
}<file_sep>/src/main/include/subsystems/Elevator.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/drive/DifferentialDrive.h>

#include "RobotMap.h"

class Elevator : public frc::Subsystem{
 private:
  frc::Talon elevator_SC{ELEVATOR_SPEED_CONTROLLER_CHANNEL};
 
 public:
  Elevator();
  void InitDefaultCommand();
  void MoveMotor(double speed);
};<file_sep>/src/main/cpp/subsystems/Grabber.cpp
#include "subsystems/Grabber.h"

Grabber::Grabber() : Subsystem("Grabber") {
    //I have no idea if the inverts are correct here, if the intake/output go backwards then flip the trues to falses
    speedControllerL.SetInverted(false);
    speedControllerR.SetInverted(true);
}

void Grabber::GrabberIntake() {
    GrabberSCG.Set(0.6);
}
void Grabber::GrabberOff() {
    GrabberSCG.Set(0.0);
}
void Grabber::GrabberOutput() {
    GrabberSCG.Set(-0.4);
}<file_sep>/src/main/include/commands/MovePneumatics.h
#pragma once

#include "CommandBase.h"

#include <frc/DoubleSolenoid.h>

class MovePneumatics : public CommandBase {
 public:
  MovePneumatics(frc::DoubleSolenoid::Value v);
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
 private:
  frc::DoubleSolenoid::Value value;
};
<file_sep>/src/main/include/RobotMap.h
#pragma once

#include <frc/drive/DifferentialDrive.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/SpeedController.h>
#include <frc/WPILib.h>

#define DRIVE_SPEED_CONTROLLER_FL_CHANNEL 0
#define DRIVE_SPEED_CONTROLLER_FR_CHANNEL 1
#define DRIVE_SPEED_CONTROLLER_BL_CHANNEL 2
#define DRIVE_SPEED_CONTROLLER_BR_CHANNEL 3

#define SCFL_INVERTED false
#define SCBL_INVERTED false
#define SCFR_INVERTED false
#define SCBR_INVERTED false

#define GYRO_ANALOG_PORT 0
#define ULTRASONIC 1

#define ELEVATOR_SPEED_CONTROLLER_CHANNEL 5

#define GRABBER_SPEED_CONTROLLER_R_CHANNEL 6
#define GRABBER_SPEED_CONTROLLER_L_CHANNEL 7

#define ARM_SPEED_CONTROLLER_CHANNEL 8

#define HALL_SENSOR 6

#define PCM_PORT 0 //Pnuematics Control Module
#define DOUBLESOLENOID_FORWARD_CHANNEL 0
#define DOUBLESOLENOID_REVERSE_CHANNEL 1

#define TROLLEY_VICTOR_SPX_CAN_BUS_ID 0 //Needs to be changed later<file_sep>/src/main/cpp/subsystems/Drive.cpp
#include "subsystems/Drive.h"
#include "commands/DriveWithJoystick.h"

#include <frc/smartDashBoard/SmartDashBoard.h>

Drive::Drive() : Subsystem("Drive") {
	oldLeftSpeed = 0.0;
	oldRightSpeed = 0.0;
	velocityToCommandIntercept[0] = -316.89;
	velocityToCommandIntercept[1] = -378.00;
	velocityToCommandIntercept[2] = -364.78;
	velocityToCommandIntercept[3] = -331.27;
	velocityToCommandSlope[0] = 1/3493.57;
	velocityToCommandSlope[1] = 1/3493.90;
	velocityToCommandSlope[2] = 1/3386.53;
	velocityToCommandSlope[3] = 1/3744.83;
	speedControllerFL.SetInverted(SCFL_INVERTED);
	speedControllerBL.SetInverted(SCBL_INVERTED);
	speedControllerFR.SetInverted(SCFR_INVERTED);
	speedControllerBR.SetInverted(SCBR_INVERTED);
}

void Drive::InitDefaultCommand() {
  SetDefaultCommand(new DriveWithJoystick());
}

void Drive::takeInput() {
	float leftSpeed = -CommandBase::oi->getDriverJoystick()->GetRawAxis(1);
	float rightSpeed = -CommandBase::oi->getDriverJoystick()->GetRawAxis(5);

	if (leftSpeed > oldLeftSpeed + MAX_CHANGE) leftSpeed = oldLeftSpeed + MAX_CHANGE;
	else if (leftSpeed < oldLeftSpeed - MAX_CHANGE) leftSpeed = oldLeftSpeed - MAX_CHANGE;
	if (rightSpeed > oldRightSpeed + MAX_CHANGE) rightSpeed = oldRightSpeed + MAX_CHANGE;
	else if (rightSpeed < oldRightSpeed - MAX_CHANGE) rightSpeed = oldRightSpeed - MAX_CHANGE;

    if (leftSpeed >= .9 && rightSpeed >= .9) {
	  setMotors(1, 1);
    } else {
	  setMotors(leftSpeed, rightSpeed);
    }

    oldLeftSpeed = leftSpeed;
	oldRightSpeed = rightSpeed;


	//fout << getGyroRate() << '\n';
}

void Drive::setMotors(float leftSpeed, float rightSpeed) {
	diffDrive.TankDrive(leftSpeed, rightSpeed, false);

	frc::SmartDashboard::PutNumber("Speed Controller FL", speedControllerFL.Get());
	frc::SmartDashboard::PutNumber("Speed Controller FR", speedControllerFR.Get());
	frc::SmartDashboard::PutNumber("Speed Controller BL", speedControllerBL.Get());
	frc::SmartDashboard::PutNumber("Speed Controller BR", speedControllerBR.Get());
}

frc::AnalogGyro* Drive::getGyro() {
	return &gyro;
}
float Drive::getGyroAngle() {
	return gyro.GetAngle();
}
float Drive::getGyroRate() {
	return gyro.GetRate();
}
void Drive::resetGyro() {
	gyro.Reset();
}
void Drive::calibrateGyro() {
	gyro.Calibrate();
}

void Drive::SetVelocity(float left, float right) {
	float leftSpeed = left * MAX_SPEED;
	float rightSpeed = right * MAX_SPEED;
	if (leftSpeed < 0) {
		leftSpeed = velocityToCommandSlope[0]*(leftSpeed + velocityToCommandIntercept[0]);
	} else {
		leftSpeed = velocityToCommandSlope[2]*(leftSpeed + velocityToCommandIntercept[2]);
	}
	if (rightSpeed < 0) {
		rightSpeed = velocityToCommandSlope[1]*(rightSpeed + velocityToCommandIntercept[1]);
	} else {
		rightSpeed = velocityToCommandSlope[3]*(rightSpeed + velocityToCommandIntercept[3]);
	}
	setMotors(left, rightSpeed);
}<file_sep>/src/main/include/commands/LowerArm.h
#pragma once

#include "CommandBase.h"

class LowerArm : public CommandBase {
 private:
  const int STOP_POINT = 25;
 public:
  LowerArm();
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
};
<file_sep>/src/main/include/commands/VelocityTest.h
#pragma once

#include "CommandBase.h"

#include <frc/PowerDistributionPanel.h>
#include <iostream>
#include <fstream>
#include <string>

class VelocityTest : public CommandBase {
 public:
  VelocityTest();
  void Initialize() override;
  void Execute() override;
  bool IsFinished() override;
  void End() override;
  void Interrupted() override;
 private:
  frc::PowerDistributionPanel pdp;
  double driveCommand;
  int count;
  std::ofstream fout;
};
<file_sep>/src/main/cpp/CommandBase.cpp
#include "CommandBase.h"
#include "subsystems/Encoders.h"
#include "subsystems/Drive.h"
#include "subsystems/UDPReceiver.h"
#include "subsystems/Sighting.h"
#include "subsystems/Elevator.h"

#include <Subsystems/Encoders.h>

std::unique_ptr<Drive> CommandBase::drive;
std::unique_ptr<Encoders> CommandBase::leftEncoder;
std::unique_ptr<Encoders> CommandBase::rightEncoder;
std::unique_ptr<Encoders> CommandBase::elevatorEncoder;
std::unique_ptr<Elevator> CommandBase::elevator;
std::unique_ptr<Grabber> CommandBase::grabber;
std::unique_ptr<OI> CommandBase::oi;
std::unique_ptr<Arm> CommandBase::arm;
std::unique_ptr<Pneumatics> CommandBase::pneumatics;
std::unique_ptr<UDPReceiver> CommandBase::udp;
std::unique_ptr<Sighting> CommandBase::sight;
std::unique_ptr<MB1013Ultrasonic> CommandBase::ultrasonic;

CommandBase::CommandBase(const std::string &name) : frc::CommandGroup(name) {
  
}

CommandBase::CommandBase() : frc::CommandGroup() {

}

void CommandBase::init() {
    drive.reset(new Drive());
    udp.reset(new UDPReceiver());
    sight.reset(new Sighting());
    leftEncoder.reset(new Encoders("left", 3, 2, false));
	rightEncoder.reset(new Encoders("right", 1, 0, false));
    elevatorEncoder.reset(new Encoders("elevator", 4, 5, false));
    grabber.reset(new Grabber());
    arm.reset(new Arm());
    pneumatics.reset(new Pneumatics());
    ultrasonic.reset(new MB1013Ultrasonic());
    elevator.reset(new Elevator());
    // Keep at the end
	oi.reset(new OI());
}<file_sep>/src/main/include/subsystems/REVDigitBoard.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/I2C.h>
#include <frc/Timer.h>
#include <frc/DigitalInput.h>
#include <frc/AnalogInput.h>
#include <string.h>
// this class controls the REV digit board that we use to choose our autonomous mode
//without having to redeploy every match.
class REVDigitBoard: public frc::Subsystem {
	typedef unsigned char byte;

	/*
	 * DOCUMENTATION::
	 *
	 * REVDigitBoard() : constructor
	 * void display(String str) : displays the first four characters of the string (only alpha (converted to uppercase), numbers, and spaces)
	 * void display(double batt) : displays a decimal number (like battery voltage) in the form of 12.34 (ten-one-decimal-tenth-hundredth)
	 * void clear() : clears the display
	 * boolean getButtonA() : button A on the board
	 * boolean getButtonB() : button B on the board
	 * double getPot() : potentiometer value
	 */
public:
	REVDigitBoard();
	void InitDefaultCommand();
	void clear();
	bool getButtonA();
	bool getButtonB();
  double getPot();
	void display(double batt);
	void display(std::string str);

private:
	void _display( byte charz[4]);

	frc::I2C i2c;
	frc::DigitalInput buttonA, buttonB;
	frc::AnalogInput pot;

	byte charreg[37][2];
	byte charmap[256];
};
<file_sep>/src/main/cpp/subsystems/Pneumatics.cpp
#include "subsystems/Pneumatics.h"

Pneumatics::Pneumatics() : Subsystem("Pnuematics") {

}

void Pneumatics::Forward() {
    doubleSolenoid.Set(frc::DoubleSolenoid::kForward);
}

void Pneumatics::Reverse() {
    doubleSolenoid.Set(frc::DoubleSolenoid::kReverse);
}

void Pneumatics::Off() {
    doubleSolenoid.Set(frc::DoubleSolenoid::kOff);
}<file_sep>/src/main/cpp/commands/Autonomous.cpp
#include "commands/Autonomous.h"
#include "commands/Move.h"
#include "commands/VelocityTest.h"
#include "commands/RampUpVelocityTest.h"
#include "commands/DriveStraightDistance.h"

Autonomous::Autonomous(int selection) : CommandBase("Autonomous") {
  //std::string gameData;
  //gameData = frc::DriverStation::GetInstance().GetGameSpecificMessage();
  //AddSequential(new VelocityTest());
  //AddSequential(new RampUpVelocityTest());
  //AddSequential(new DriveStraightDistance(36));
  //"Wes is Stupid and dumb" -Wes

}<file_sep>/src/main/include/commands/Velocity.h
#ifndef VELOCITY_H
#define VELOCITY_H

#include "CommandBase.h"
#include <frc/PIDController.h>
#include <frc/PIDOutput.h>
#include <frc/PIDSource.h>
//NOTICE: PLEASE READ: If encoders are not already required by the command you are calling this from,
//then you must add it to that command.

class VelocityPIDOutput: public frc::PIDOutput {
    private:
        double leftOutput , rightOutput ;
        bool right;
    public:
    VelocityPIDOutput(bool right);
    virtual ~VelocityPIDOutput();
    virtual void PIDWrite(double output);
};

class VelocityPIDSource: public frc::PIDSource {
    private:
        bool right;
    public:
    VelocityPIDSource(bool right);
    virtual ~VelocityPIDSource();
    virtual double PIDGet();
};

class Velocity : public CommandBase {
    private:
    double setPointVelocity;
        frc::PIDController * leftPID = nullptr, * rightPID = nullptr;
        frc::PIDSource * rightPIDSource, * leftPIDSource;
        frc::PIDOutput * rightPIDOutput, * leftPIDOutput;
    public:
        bool done;
        Velocity();
        virtual void End();
        virtual void Initialize();
        virtual void Execute();
        virtual bool IsFinished();
        virtual void Interrupted();
        void SetSetpoint(double inputVelocity);
        bool IsInterruptible() const;
       
};
#endif<file_sep>/src/main/cpp/OI.cpp
#include <frc/WPILib.h>
#include <frc/DoubleSolenoid.h>
#include "OI.h"
#include "frc/smartDashboard/SmartDashboard.h"
#include "Robot.h"
#include "commands/DriveStraightDistance.h"
#include "commands/LowerArm.h"
#include "commands/MoveElevator.h"
#include "commands/MovePneumatics.h"

OI::OI() {
  driverJoystick.reset(new frc::Joystick(0));
  mechanismsJoystick.reset(new frc::Joystick(1));
  for (int i=1; i<=10; i++){
    driverBtns.push_back(new frc::JoystickButton(driverJoystick.get(), i));
    mechanismsBtns.push_back(new frc::JoystickButton(mechanismsJoystick.get(), i));
  }
  driverBtns[1]->WhenPressed(new DriveStraightDistance(100));
  mechanismsBtns[1]->WhileHeld(new MoveElevator(0.6)); //This should be 'b' button on the XBox controller
  mechanismsBtns[0]->WhileHeld(new MoveElevator(-0.6)); //This should be 'a' button on the XBox controller
  mechanismsBtns[2]->WhileHeld(new MovePneumatics(frc::DoubleSolenoid::kForward)); //This should be 'x' button on the XBox controller
  mechanismsBtns[2]->WhenReleased(new MovePneumatics(frc::DoubleSolenoid::kOff)); //This should be 'x' button on the XBox controller
  mechanismsBtns[3]->WhileHeld(new MovePneumatics(frc::DoubleSolenoid::kReverse)); //This should be 'y' button on the XBox controller
  mechanismsBtns[3]->WhenReleased(new MovePneumatics(frc::DoubleSolenoid::kOff)); //This should be 'y' button on the XBox controller
  mechanismsBtns[5]->WhenPressed(new LowerArm()); //Ths should be 'RB' on the XBox controller
}

std::shared_ptr<frc::Joystick> OI::getDriverJoystick() {
	return driverJoystick;
}
<file_sep>/src/main/cpp/subsystems/Encoders.cpp
#include <Subsystems/Encoders.h>
#include <frc/smartDashboard/SmartDashboard.h>
#include <String>

Encoders::Encoders(std::string shortName, unsigned int channelA, unsigned int channelB, bool reverseDirection) : frc::Subsystem("Encoders") {
	encoder = new frc::Encoder(channelA, channelB, reverseDirection, frc::Encoder::EncodingType::k4X);
	encoder->Reset();
	encoder->SetMaxPeriod(MAX_PERIOD);
	encoder->SetMinRate(MIN_RATE);
	encoder->SetReverseDirection(false);
	encoder->SetDistancePerPulse(PULSEINCHES);
	encoder->SetSamplesToAverage(SAMPLES_TO_AVE);
	frc::SmartDashboard::PutNumber(shortName + "count", encoder->Get());
	frc::SmartDashboard::PutNumber(shortName + "Raw Count", encoder->GetRaw());
	frc::SmartDashboard::PutNumber(shortName + "Distance", encoder->GetDistance());
	frc::SmartDashboard::PutNumber(shortName + "Rate", encoder->GetRate());
	frc::SmartDashboard::PutBoolean(shortName + "Direction", encoder->GetDirection());
	frc::SmartDashboard::PutBoolean(shortName + "Stopped", encoder->GetStopped());
}

double Encoders::getCount() const {
	return encoder->Get();
}

double Encoders::getRaw() const {
	return encoder->GetRaw();
}

double Encoders::getDistance() const {
  return encoder->GetDistance();
}


double Encoders::getRate() const {
  return encoder->GetRate();
}

double Encoders::getDirection() const {
	return encoder->GetDirection();
}

double Encoders::getStopped() const {
  return encoder->GetStopped();
}

void Encoders::reset() const {
  encoder->Reset();
}<file_sep>/src/main/cpp/commands/LowerArm.cpp
#include "commands/LowerArm.h"

LowerArm::LowerArm() :CommandBase("LowerArm") {
  Requires(CommandBase::arm.get());
}

void LowerArm::Initialize() {}

void LowerArm::Execute() {
  CommandBase::arm->MoveArm();
}

bool LowerArm::IsFinished() {
  return CommandBase::arm->GetCount() <= STOP_POINT;
}

void LowerArm::End() {}

void LowerArm::Interrupted() {}
<file_sep>/src/main/cpp/subsystems/MB1013Ultrasonic.cpp
#include "subsystems/MB1013Ultrasonic.h"

MB1013Ultrasonic::MB1013Ultrasonic() : Subsystem("MB1013Ultrasonic") {}

double MB1013Ultrasonic::getVoltage() {
  return ultrasonic.GetVoltage();
}<file_sep>/src/main/cpp/commands/Velocity.cpp
#include <frc/WPILib.h>
#include "CommandBase.h"
#include <frc/PIDController.h>
#include <frc/PIDOutput.h>
#include <frc/PIDSource.h>
#include "commands/Velocity.h"

Velocity::Velocity() : CommandBase("Velocity") {
    done = false;
    Requires(drive.get());
    leftPIDOutput = new VelocityPIDOutput(false);
    rightPIDOutput = new VelocityPIDOutput(true);
    leftPIDSource = new VelocityPIDSource(false);
    rightPIDSource = new VelocityPIDSource(true);
    setPointVelocity = 0;
    rightPID->SetSetpoint(0);
    leftPID->SetSetpoint(0);
}

bool Velocity::IsInterruptible() const{
    return true;
}

void Velocity::Interrupted() {
    End();
}

void Velocity::End() {
    leftPID->Disable();
    rightPID->Disable();
    delete leftPIDOutput;
    delete leftPIDSource;
    delete rightPIDOutput;
    delete rightPIDSource;
    delete leftPID;
    delete rightPID;
    drive->setMotors(0.0, 0.0);
}

void Velocity::Initialize() {
    leftPID = new frc::PIDController(
        frc::SmartDashboard::GetNumber("PLeftvelocity", .03),
        frc::SmartDashboard::GetNumber("ILeftvelocity", .000),
        frc::SmartDashboard::GetNumber("DLeftvelocity", .00),
        frc::SmartDashboard::GetNumber("FLeftvelocity", 0),
        leftPIDSource,
        leftPIDOutput
        );
    rightPID = new frc::PIDController(
        frc::SmartDashboard::GetNumber("PRightvelocity", .03),
        frc::SmartDashboard::GetNumber("IRightvelocity", .000),
        frc::SmartDashboard::GetNumber("DRightvelocity", .00),
        frc::SmartDashboard::GetNumber("FRightvelocity", 0),
        rightPIDSource,
        rightPIDOutput
        );
    
    leftPID->SetInputRange(-352.0, 352.0);
    leftPID->SetOutputRange(-1.0, 1.0);
    rightPID->SetInputRange(-352.0, 352.0);
    rightPID->SetOutputRange(-1.0, 1.0);
    leftPID->Enable();
    rightPID->Enable();  
}
    
void Velocity::Execute() {
    if(leftPID && rightPID){
        frc::SmartDashboard::PutNumber("PLeftPIDvelocity", leftPID->GetP());
        frc::SmartDashboard::PutNumber("ILeftPIDvelocity", leftPID->GetI());
        frc::SmartDashboard::PutNumber("DLeftPIDvelocity", leftPID->GetD());
        frc::SmartDashboard::PutNumber("FLeftPIDvelocity", leftPID->GetF());
        frc::SmartDashboard::PutNumber("PRightPIDvelocity", rightPID->GetP());
        frc::SmartDashboard::PutNumber("IRightPIDvelocity", rightPID->GetP());
        frc::SmartDashboard::PutNumber("DRightPIDvelocity", rightPID->GetP());
        frc::SmartDashboard::PutNumber("FRightPIDvelocity", rightPID->GetP());
    }
    //this is the hard coding way of avoiding the PID. if this is active then the line in PIDWrite should be commented or vice versa
    CommandBase::drive->setMotors(0, 0);
}
void Velocity::SetSetpoint(double inputVelocity){
    if(rightPID && leftPID){
        rightPID->SetSetpoint(inputVelocity);
        leftPID->SetSetpoint(inputVelocity);
    }
}

//this is set to false by the DriveStraightDistance End() method
bool Velocity::IsFinished() {
    return done;
}


VelocityPIDOutput::VelocityPIDOutput(bool right) {
    this->right = right;
    leftOutput = 0;
    rightOutput = 0;
}

VelocityPIDOutput::~VelocityPIDOutput() {
    
}

void VelocityPIDOutput::PIDWrite(double output) {
    if (right) {
        rightOutput = output;
    } else {
        leftOutput = output;
    }
    //CommandBase::drive->setMotors(leftOutput, rightOutput);
}

VelocityPIDSource::VelocityPIDSource(bool right) {
    this->right = right;
}

VelocityPIDSource::~VelocityPIDSource() {
    
}

double VelocityPIDSource::PIDGet() {
    if (right) {
        return (CommandBase::rightEncoder->getRate() * 7);
    } else {
        return CommandBase::leftEncoder->getRate();
    }
}<file_sep>/src/main/include/Robot.h
#pragma once

#include <frc/TimedRobot.h>
#include <frc/commands/Command.h>
#include <frc/smartdashboard/SendableChooser.h>

#include "CommandBase.h"
#include "commands/Autonomous.h"
#include "AutonomousChooser.h"



class Robot : public frc::TimedRobot {
 public:
  static std::unique_ptr<Autonomous> autonomousCommand;
  static AutonomousChooser AutoChooser;
 private:
  void RobotInit() override;
  void RobotPeriodic() override;
  void DisabledInit() override;
  void DisabledPeriodic() override;
  void AutonomousInit() override;
  void AutonomousPeriodic() override;
  void TeleopInit() override;
  void TeleopPeriodic() override;
  void TestPeriodic() override;
  void AddSmartDashboardItems();
};
<file_sep>/src/main/include/subsystems/Arm.h
#pragma once

#include <frc/commands/Subsystem.h>
#include <frc/Counter.h>

#include "RobotMap.h"

class Arm : public frc::Subsystem {
 private:
  frc::Talon arm_SC{ARM_SPEED_CONTROLLER_CHANNEL};
  frc::Counter hallSensor{HALL_SENSOR};
 public:
  Arm();
  void MoveArm();
  int GetCount();
};
