mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
Run wpiformat on merged repo (#1021)
This commit is contained in:
committed by
Peter Johnson
parent
0babbf317c
commit
6729a7d6b1
@@ -15,16 +15,16 @@
|
||||
* Runs the motors with arcade steering.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
frc::Spark m_leftMotor{0};
|
||||
frc::Spark m_rightMotor{1};
|
||||
frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
|
||||
frc::Joystick m_stick{0};
|
||||
frc::Spark m_leftMotor{0};
|
||||
frc::Spark m_rightMotor{1};
|
||||
frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
|
||||
frc::Joystick m_stick{0};
|
||||
|
||||
public:
|
||||
void TeleopPeriodic() {
|
||||
// drive with arcade style
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
public:
|
||||
void TeleopPeriodic() {
|
||||
// Drive with arcade style
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -14,55 +14,50 @@
|
||||
/**
|
||||
* This is a demo program showing the use of OpenCV to do vision processing. The
|
||||
* image is acquired from the Axis camera, then a rectangle is put on the image
|
||||
* and
|
||||
* sent to the dashboard. OpenCV has many methods for different types of
|
||||
* and sent to the dashboard. OpenCV has many methods for different types of
|
||||
* processing.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
private:
|
||||
static void VisionThread() {
|
||||
// Get the Axis camera from CameraServer
|
||||
cs::AxisCamera camera =
|
||||
CameraServer::GetInstance()->AddAxisCamera(
|
||||
"axis-camera.local");
|
||||
// Set the resolution
|
||||
camera.SetResolution(640, 480);
|
||||
private:
|
||||
static void VisionThread() {
|
||||
// Get the Axis camera from CameraServer
|
||||
cs::AxisCamera camera =
|
||||
CameraServer::GetInstance()->AddAxisCamera("axis-camera.local");
|
||||
// Set the resolution
|
||||
camera.SetResolution(640, 480);
|
||||
|
||||
// Get a CvSink. This will capture Mats from the Camera
|
||||
cs::CvSink cvSink = CameraServer::GetInstance()->GetVideo();
|
||||
// Setup a CvSource. This will send images back to the Dashboard
|
||||
cs::CvSource outputStream =
|
||||
CameraServer::GetInstance()->PutVideo(
|
||||
"Rectangle", 640, 480);
|
||||
// Get a CvSink. This will capture Mats from the Camera
|
||||
cs::CvSink cvSink = CameraServer::GetInstance()->GetVideo();
|
||||
// Setup a CvSource. This will send images back to the Dashboard
|
||||
cs::CvSource outputStream =
|
||||
CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
|
||||
|
||||
// Mats are very memory expensive. Lets reuse this Mat.
|
||||
cv::Mat mat;
|
||||
// Mats are very memory expensive. Lets reuse this Mat.
|
||||
cv::Mat mat;
|
||||
|
||||
while (true) {
|
||||
// Tell the CvSink to grab a frame from the camera and
|
||||
// put it
|
||||
// in the source mat. If there is an error notify the
|
||||
// output.
|
||||
if (cvSink.GrabFrame(mat) == 0) {
|
||||
// Send the output the error.
|
||||
outputStream.NotifyError(cvSink.GetError());
|
||||
// skip the rest of the current iteration
|
||||
continue;
|
||||
}
|
||||
// Put a rectangle on the image
|
||||
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
|
||||
cv::Scalar(255, 255, 255), 5);
|
||||
// Give the output stream a new image to display
|
||||
outputStream.PutFrame(mat);
|
||||
}
|
||||
}
|
||||
while (true) {
|
||||
// Tell the CvSink to grab a frame from the camera and put it in the
|
||||
// source mat. If there is an error notify the output.
|
||||
if (cvSink.GrabFrame(mat) == 0) {
|
||||
// Send the output the error.
|
||||
outputStream.NotifyError(cvSink.GetError());
|
||||
// skip the rest of the current iteration
|
||||
continue;
|
||||
}
|
||||
// Put a rectangle on the image
|
||||
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
|
||||
cv::Scalar(255, 255, 255), 5);
|
||||
// Give the output stream a new image to display
|
||||
outputStream.PutFrame(mat);
|
||||
}
|
||||
}
|
||||
|
||||
void RobotInit() override {
|
||||
// We need to run our vision program in a separate Thread.
|
||||
// If not, our robot program will not run
|
||||
std::thread visionThread(VisionThread);
|
||||
visionThread.detach();
|
||||
}
|
||||
void RobotInit() override {
|
||||
// We need to run our vision program in a separate thread. If not, our robot
|
||||
// program will not run.
|
||||
std::thread visionThread(VisionThread);
|
||||
visionThread.detach();
|
||||
}
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -15,29 +15,26 @@
|
||||
* through the SmartDashboard.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* Get the current going through channel 7, in Amperes. The PDP
|
||||
* returns the current in increments of 0.125A. At low currents
|
||||
* the
|
||||
* current readings tend to be less accurate.
|
||||
*/
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Current Channel 7", m_pdp.GetCurrent(7));
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* Get the current going through channel 7, in Amperes. The PDP returns the
|
||||
* current in increments of 0.125A. At low currents the current readings
|
||||
* tend to be less accurate.
|
||||
*/
|
||||
frc::SmartDashboard::PutNumber("Current Channel 7", m_pdp.GetCurrent(7));
|
||||
|
||||
/* Get the voltage going into the PDP, in Volts.
|
||||
* The PDP returns the voltage in increments of 0.05 Volts.
|
||||
*/
|
||||
frc::SmartDashboard::PutNumber("Voltage", m_pdp.GetVoltage());
|
||||
/* Get the voltage going into the PDP, in Volts. The PDP returns the voltage
|
||||
* in increments of 0.05 Volts.
|
||||
*/
|
||||
frc::SmartDashboard::PutNumber("Voltage", m_pdp.GetVoltage());
|
||||
|
||||
// Retrieves the temperature of the PDP, in degrees Celsius.
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Temperature", m_pdp.GetTemperature());
|
||||
}
|
||||
// Retrieves the temperature of the PDP, in degrees Celsius.
|
||||
frc::SmartDashboard::PutNumber("Temperature", m_pdp.GetTemperature());
|
||||
}
|
||||
|
||||
private:
|
||||
// Object for dealing with the Power Distribution Panel (PDP).
|
||||
frc::PowerDistributionPanel m_pdp;
|
||||
private:
|
||||
// Object for dealing with the Power Distribution Panel (PDP).
|
||||
frc::PowerDistributionPanel m_pdp;
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -11,84 +11,73 @@
|
||||
|
||||
/**
|
||||
* Sample program displaying the value of a quadrature encoder on the
|
||||
* SmartDashboard.
|
||||
* SmartDashboard.
|
||||
*
|
||||
* Quadrature Encoders are digital sensors which can detect the amount the
|
||||
* encoder has rotated since starting as well as the direction in which the
|
||||
* encoder shaft is rotating. However, encoders can not tell you the absolute
|
||||
* position of the encoder shaft (ie, it considers where it starts to be the
|
||||
* zero position, no matter where it starts), and so can only tell you how
|
||||
* much the encoder has rotated since starting.
|
||||
* Depending on the precision of an encoder, it will have fewer or greater
|
||||
* ticks per revolution; the number of ticks per revolution will affect the
|
||||
* conversion between ticks and distance, as specified by DistancePerPulse.
|
||||
* encoder has rotated since starting as well as the direction in which the
|
||||
* encoder shaft is rotating. However, encoders can not tell you the absolute
|
||||
* position of the encoder shaft (ie, it considers where it starts to be the
|
||||
* zero position, no matter where it starts), and so can only tell you how much
|
||||
* the encoder has rotated since starting.
|
||||
*
|
||||
* Depending on the precision of an encoder, it will have fewer or greater ticks
|
||||
* per revolution; the number of ticks per revolution will affect the conversion
|
||||
* between ticks and distance, as specified by DistancePerPulse.
|
||||
*
|
||||
* One of the most common uses of encoders is in the drivetrain, so that the
|
||||
* distance that the robot drives can be precisely controlled during the
|
||||
* autonomous mode.
|
||||
* distance that the robot drives can be precisely controlled during the
|
||||
* autonomous mode.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
Robot() {
|
||||
/* Defines the number of samples to average when determining the
|
||||
* rate.
|
||||
* On a quadrature encoder, values range from 1-255; larger
|
||||
* values
|
||||
* result in smoother but potentially less accurate rates than
|
||||
* lower
|
||||
* values.
|
||||
*/
|
||||
m_encoder.SetSamplesToAverage(5);
|
||||
public:
|
||||
Robot() {
|
||||
/* Defines the number of samples to average when determining the rate.
|
||||
* On a quadrature encoder, values range from 1-255; larger values result in
|
||||
* smoother but potentially less accurate rates than lower values.
|
||||
*/
|
||||
m_encoder.SetSamplesToAverage(5);
|
||||
|
||||
/* Defines how far the mechanism attached to the encoder moves
|
||||
* per pulse.
|
||||
* In this case, we assume that a 360 count encoder is directly
|
||||
* attached
|
||||
* to a 3 inch diameter (1.5inch radius) wheel, and that we want
|
||||
* to
|
||||
* measure distance in inches.
|
||||
*/
|
||||
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
|
||||
/* Defines how far the mechanism attached to the encoder moves per pulse. In
|
||||
* this case, we assume that a 360 count encoder is directly attached to a 3
|
||||
* inch diameter (1.5inch radius) wheel, and that we want to measure
|
||||
* distance in inches.
|
||||
*/
|
||||
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
|
||||
|
||||
/* Defines the lowest rate at which the encoder will not be
|
||||
* considered
|
||||
* stopped, for the purposes of the GetStopped() method.
|
||||
* Units are in distance / second, where distance refers to the
|
||||
* units
|
||||
* of distance that you are using, in this case inches.
|
||||
*/
|
||||
m_encoder.SetMinRate(1.0);
|
||||
}
|
||||
/* Defines the lowest rate at which the encoder will not be considered
|
||||
* stopped, for the purposes of the GetStopped() method. Units are in
|
||||
* distance / second, where distance refers to the units of distance that
|
||||
* you are using, in this case inches.
|
||||
*/
|
||||
m_encoder.SetMinRate(1.0);
|
||||
}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Retrieve the net displacement of the Encoder since the last
|
||||
// Reset.
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Encoder Distance", m_encoder.GetDistance());
|
||||
void TeleopPeriodic() override {
|
||||
// Retrieve the net displacement of the Encoder since the last Reset.
|
||||
frc::SmartDashboard::PutNumber("Encoder Distance", m_encoder.GetDistance());
|
||||
|
||||
// Retrieve the current rate of the encoder.
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Encoder Rate", m_encoder.GetRate());
|
||||
}
|
||||
// Retrieve the current rate of the encoder.
|
||||
frc::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* The Encoder object is constructed with 4 parameters, the last two
|
||||
* being
|
||||
* optional.
|
||||
* The first two parameters (1, 2 in this case) refer to the ports on
|
||||
* the
|
||||
* roboRIO which the encoder uses. Because a quadrature encoder has
|
||||
* two signal wires, the signal from two DIO ports on the roboRIO are
|
||||
* used.
|
||||
* The third (optional) parameter is a boolean which defaults to false.
|
||||
* If you set this parameter to true, the direction of the encoder
|
||||
* will
|
||||
* be reversed, in case it makes more sense mechanically.
|
||||
* The final (optional) parameter specifies encoding rate (k1X, k2X, or
|
||||
* k4X)
|
||||
* and defaults to k4X. Faster (k4X) encoding gives greater positional
|
||||
* precision but more noise in the rate.
|
||||
*/
|
||||
frc::Encoder m_encoder{1, 2, false, Encoder::k4X};
|
||||
private:
|
||||
/**
|
||||
* The Encoder object is constructed with 4 parameters, the last two being
|
||||
* optional.
|
||||
*
|
||||
* The first two parameters (1, 2 in this case) refer to the ports on the
|
||||
* roboRIO which the encoder uses. Because a quadrature encoder has two signal
|
||||
* wires, the signal from two DIO ports on the roboRIO are used.
|
||||
*
|
||||
* The third (optional) parameter is a boolean which defaults to false. If you
|
||||
* set this parameter to true, the direction of the encoder will be reversed,
|
||||
* in case it makes more sense mechanically.
|
||||
*
|
||||
* The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X)
|
||||
* and defaults to k4X. Faster (k4X) encoding gives greater positional
|
||||
* precision but more noise in the rate.
|
||||
*/
|
||||
frc::Encoder m_encoder{1, 2, false, Encoder::k4X};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -15,17 +15,16 @@
|
||||
#include "SetDistanceToBox.h"
|
||||
#include "SetWristSetpoint.h"
|
||||
|
||||
Autonomous::Autonomous()
|
||||
: frc::CommandGroup("Autonomous") {
|
||||
AddSequential(new PrepareToPickup());
|
||||
AddSequential(new Pickup());
|
||||
AddSequential(new SetDistanceToBox(0.10));
|
||||
// AddSequential(new DriveStraight(4)); // Use Encoders if ultrasonic
|
||||
// is broken
|
||||
AddSequential(new Place());
|
||||
AddSequential(new SetDistanceToBox(0.60));
|
||||
// AddSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic
|
||||
// is broken
|
||||
AddParallel(new SetWristSetpoint(-45));
|
||||
AddSequential(new CloseClaw());
|
||||
Autonomous::Autonomous() : frc::CommandGroup("Autonomous") {
|
||||
AddSequential(new PrepareToPickup());
|
||||
AddSequential(new Pickup());
|
||||
AddSequential(new SetDistanceToBox(0.10));
|
||||
// AddSequential(new DriveStraight(4)); // Use Encoders if ultrasonic
|
||||
// is broken
|
||||
AddSequential(new Place());
|
||||
AddSequential(new SetDistanceToBox(0.60));
|
||||
// AddSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic
|
||||
// is broken
|
||||
AddParallel(new SetWristSetpoint(-45));
|
||||
AddSequential(new CloseClaw());
|
||||
}
|
||||
|
||||
@@ -13,6 +13,6 @@
|
||||
* The main autonomous command to pickup and deliver the soda to the box.
|
||||
*/
|
||||
class Autonomous : public frc::CommandGroup {
|
||||
public:
|
||||
Autonomous();
|
||||
public:
|
||||
Autonomous();
|
||||
};
|
||||
|
||||
@@ -9,20 +9,13 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
CloseClaw::CloseClaw()
|
||||
: frc::Command("CloseClaw") {
|
||||
Requires(&Robot::claw);
|
||||
}
|
||||
CloseClaw::CloseClaw() : frc::Command("CloseClaw") { Requires(&Robot::claw); }
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void CloseClaw::Initialize() {
|
||||
Robot::claw.Close();
|
||||
}
|
||||
void CloseClaw::Initialize() { Robot::claw.Close(); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool CloseClaw::IsFinished() {
|
||||
return Robot::claw.IsGripping();
|
||||
}
|
||||
bool CloseClaw::IsFinished() { return Robot::claw.IsGripping(); }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void CloseClaw::End() {
|
||||
@@ -30,6 +23,6 @@ void CloseClaw::End() {
|
||||
// fall out
|
||||
// + there is no need to worry about stalling the motor or crushing the can.
|
||||
#ifndef SIMULATION
|
||||
Robot::claw.Stop();
|
||||
Robot::claw.Stop();
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -14,9 +14,9 @@
|
||||
* motors is BAD!
|
||||
*/
|
||||
class CloseClaw : public frc::Command {
|
||||
public:
|
||||
CloseClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
CloseClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
};
|
||||
|
||||
@@ -10,35 +10,33 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
DriveStraight::DriveStraight(double distance) {
|
||||
Requires(&Robot::drivetrain);
|
||||
m_pid.SetAbsoluteTolerance(0.01);
|
||||
m_pid.SetSetpoint(distance);
|
||||
Requires(&Robot::drivetrain);
|
||||
m_pid.SetAbsoluteTolerance(0.01);
|
||||
m_pid.SetSetpoint(distance);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void DriveStraight::Initialize() {
|
||||
// Get everything in a safe starting state.
|
||||
Robot::drivetrain.Reset();
|
||||
m_pid.Reset();
|
||||
m_pid.Enable();
|
||||
// Get everything in a safe starting state.
|
||||
Robot::drivetrain.Reset();
|
||||
m_pid.Reset();
|
||||
m_pid.Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool DriveStraight::IsFinished() {
|
||||
return m_pid.OnTarget();
|
||||
}
|
||||
bool DriveStraight::IsFinished() { return m_pid.OnTarget(); }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveStraight::End() {
|
||||
// Stop PID and the wheels
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
// Stop PID and the wheels
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
|
||||
double DriveStraight::DriveStraightPIDSource::PIDGet() {
|
||||
return Robot::drivetrain.GetDistance();
|
||||
return Robot::drivetrain.GetDistance();
|
||||
}
|
||||
|
||||
void DriveStraight::DriveStraightPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
}
|
||||
|
||||
@@ -19,26 +19,26 @@
|
||||
* values of the left and right encoders.
|
||||
*/
|
||||
class DriveStraight : public frc::Command {
|
||||
public:
|
||||
explicit DriveStraight(double distance);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
explicit DriveStraight(double distance);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
|
||||
class DriveStraightPIDSource : public frc::PIDSource {
|
||||
public:
|
||||
virtual ~DriveStraightPIDSource() = default;
|
||||
double PIDGet() override;
|
||||
};
|
||||
class DriveStraightPIDSource : public frc::PIDSource {
|
||||
public:
|
||||
virtual ~DriveStraightPIDSource() = default;
|
||||
double PIDGet() override;
|
||||
};
|
||||
|
||||
class DriveStraightPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
virtual ~DriveStraightPIDOutput() = default;
|
||||
void PIDWrite(double d) override;
|
||||
};
|
||||
class DriveStraightPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
virtual ~DriveStraightPIDOutput() = default;
|
||||
void PIDWrite(double d) override;
|
||||
};
|
||||
|
||||
private:
|
||||
DriveStraightPIDSource m_source;
|
||||
DriveStraightPIDOutput m_output;
|
||||
frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
|
||||
private:
|
||||
DriveStraightPIDSource m_source;
|
||||
DriveStraightPIDOutput m_output;
|
||||
frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
|
||||
};
|
||||
|
||||
@@ -9,23 +9,16 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
OpenClaw::OpenClaw()
|
||||
: frc::Command("OpenClaw") {
|
||||
Requires(&Robot::claw);
|
||||
SetTimeout(1);
|
||||
OpenClaw::OpenClaw() : frc::Command("OpenClaw") {
|
||||
Requires(&Robot::claw);
|
||||
SetTimeout(1);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void OpenClaw::Initialize() {
|
||||
Robot::claw.Open();
|
||||
}
|
||||
void OpenClaw::Initialize() { Robot::claw.Open(); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool OpenClaw::IsFinished() {
|
||||
return IsTimedOut();
|
||||
}
|
||||
bool OpenClaw::IsFinished() { return IsTimedOut(); }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void OpenClaw::End() {
|
||||
Robot::claw.Stop();
|
||||
}
|
||||
void OpenClaw::End() { Robot::claw.Stop(); }
|
||||
|
||||
@@ -14,9 +14,9 @@
|
||||
* motors is BAD!
|
||||
*/
|
||||
class OpenClaw : public frc::Command {
|
||||
public:
|
||||
OpenClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
OpenClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
};
|
||||
|
||||
@@ -11,9 +11,8 @@
|
||||
#include "SetElevatorSetpoint.h"
|
||||
#include "SetWristSetpoint.h"
|
||||
|
||||
Pickup::Pickup()
|
||||
: frc::CommandGroup("Pickup") {
|
||||
AddSequential(new CloseClaw());
|
||||
AddParallel(new SetWristSetpoint(-45));
|
||||
AddSequential(new SetElevatorSetpoint(0.25));
|
||||
Pickup::Pickup() : frc::CommandGroup("Pickup") {
|
||||
AddSequential(new CloseClaw());
|
||||
AddParallel(new SetWristSetpoint(-45));
|
||||
AddSequential(new SetElevatorSetpoint(0.25));
|
||||
}
|
||||
|
||||
@@ -14,6 +14,6 @@
|
||||
* get it in a safe state to drive around.
|
||||
*/
|
||||
class Pickup : public frc::CommandGroup {
|
||||
public:
|
||||
Pickup();
|
||||
public:
|
||||
Pickup();
|
||||
};
|
||||
|
||||
@@ -11,9 +11,8 @@
|
||||
#include "SetElevatorSetpoint.h"
|
||||
#include "SetWristSetpoint.h"
|
||||
|
||||
Place::Place()
|
||||
: frc::CommandGroup("Place") {
|
||||
AddSequential(new SetElevatorSetpoint(0.25));
|
||||
AddSequential(new SetWristSetpoint(0));
|
||||
AddSequential(new OpenClaw());
|
||||
Place::Place() : frc::CommandGroup("Place") {
|
||||
AddSequential(new SetElevatorSetpoint(0.25));
|
||||
AddSequential(new SetWristSetpoint(0));
|
||||
AddSequential(new OpenClaw());
|
||||
}
|
||||
|
||||
@@ -13,6 +13,6 @@
|
||||
* Place a held soda can onto the platform.
|
||||
*/
|
||||
class Place : public frc::CommandGroup {
|
||||
public:
|
||||
Place();
|
||||
public:
|
||||
Place();
|
||||
};
|
||||
|
||||
@@ -11,9 +11,8 @@
|
||||
#include "SetElevatorSetpoint.h"
|
||||
#include "SetWristSetpoint.h"
|
||||
|
||||
PrepareToPickup::PrepareToPickup()
|
||||
: frc::CommandGroup("PrepareToPickup") {
|
||||
AddParallel(new OpenClaw());
|
||||
AddParallel(new SetWristSetpoint(0));
|
||||
AddSequential(new SetElevatorSetpoint(0));
|
||||
PrepareToPickup::PrepareToPickup() : frc::CommandGroup("PrepareToPickup") {
|
||||
AddParallel(new OpenClaw());
|
||||
AddParallel(new SetWristSetpoint(0));
|
||||
AddSequential(new SetElevatorSetpoint(0));
|
||||
}
|
||||
|
||||
@@ -13,6 +13,6 @@
|
||||
* Make sure the robot is in a state to pickup soda cans.
|
||||
*/
|
||||
class PrepareToPickup : public frc::CommandGroup {
|
||||
public:
|
||||
PrepareToPickup();
|
||||
public:
|
||||
PrepareToPickup();
|
||||
};
|
||||
|
||||
@@ -12,35 +12,33 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetDistanceToBox::SetDistanceToBox(double distance) {
|
||||
Requires(&Robot::drivetrain);
|
||||
m_pid.SetAbsoluteTolerance(0.01);
|
||||
m_pid.SetSetpoint(distance);
|
||||
Requires(&Robot::drivetrain);
|
||||
m_pid.SetAbsoluteTolerance(0.01);
|
||||
m_pid.SetSetpoint(distance);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetDistanceToBox::Initialize() {
|
||||
// Get everything in a safe starting state.
|
||||
Robot::drivetrain.Reset();
|
||||
m_pid.Reset();
|
||||
m_pid.Enable();
|
||||
// Get everything in a safe starting state.
|
||||
Robot::drivetrain.Reset();
|
||||
m_pid.Reset();
|
||||
m_pid.Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetDistanceToBox::IsFinished() {
|
||||
return m_pid.OnTarget();
|
||||
}
|
||||
bool SetDistanceToBox::IsFinished() { return m_pid.OnTarget(); }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void SetDistanceToBox::End() {
|
||||
// Stop PID and the wheels
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
// Stop PID and the wheels
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
|
||||
double SetDistanceToBox::SetDistanceToBoxPIDSource::PIDGet() {
|
||||
return Robot::drivetrain.GetDistanceToObstacle();
|
||||
return Robot::drivetrain.GetDistanceToObstacle();
|
||||
}
|
||||
|
||||
void SetDistanceToBox::SetDistanceToBoxPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
}
|
||||
|
||||
@@ -19,26 +19,26 @@
|
||||
* encoders.
|
||||
*/
|
||||
class SetDistanceToBox : public frc::Command {
|
||||
public:
|
||||
explicit SetDistanceToBox(double distance);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
explicit SetDistanceToBox(double distance);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
|
||||
class SetDistanceToBoxPIDSource : public frc::PIDSource {
|
||||
public:
|
||||
virtual ~SetDistanceToBoxPIDSource() = default;
|
||||
double PIDGet() override;
|
||||
};
|
||||
class SetDistanceToBoxPIDSource : public frc::PIDSource {
|
||||
public:
|
||||
virtual ~SetDistanceToBoxPIDSource() = default;
|
||||
double PIDGet() override;
|
||||
};
|
||||
|
||||
class SetDistanceToBoxPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
virtual ~SetDistanceToBoxPIDOutput() = default;
|
||||
void PIDWrite(double d) override;
|
||||
};
|
||||
class SetDistanceToBoxPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
virtual ~SetDistanceToBoxPIDOutput() = default;
|
||||
void PIDWrite(double d) override;
|
||||
};
|
||||
|
||||
private:
|
||||
SetDistanceToBoxPIDSource m_source;
|
||||
SetDistanceToBoxPIDOutput m_output;
|
||||
frc::PIDController m_pid{-2, 0, 0, &m_source, &m_output};
|
||||
private:
|
||||
SetDistanceToBoxPIDSource m_source;
|
||||
SetDistanceToBoxPIDOutput m_output;
|
||||
frc::PIDController m_pid{-2, 0, 0, &m_source, &m_output};
|
||||
};
|
||||
|
||||
@@ -13,17 +13,15 @@
|
||||
|
||||
SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
|
||||
: frc::Command("SetElevatorSetpoint") {
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::elevator);
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::elevator);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetElevatorSetpoint::Initialize() {
|
||||
Robot::elevator.SetSetpoint(m_setpoint);
|
||||
Robot::elevator.Enable();
|
||||
Robot::elevator.SetSetpoint(m_setpoint);
|
||||
Robot::elevator.Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetElevatorSetpoint::IsFinished() {
|
||||
return Robot::elevator.OnTarget();
|
||||
}
|
||||
bool SetElevatorSetpoint::IsFinished() { return Robot::elevator.OnTarget(); }
|
||||
|
||||
@@ -17,11 +17,11 @@
|
||||
* commands using the elevator should make sure they disable PID!
|
||||
*/
|
||||
class SetElevatorSetpoint : public frc::Command {
|
||||
public:
|
||||
explicit SetElevatorSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
explicit SetElevatorSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double m_setpoint;
|
||||
private:
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -11,17 +11,15 @@
|
||||
|
||||
SetWristSetpoint::SetWristSetpoint(double setpoint)
|
||||
: frc::Command("SetWristSetpoint") {
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::wrist);
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::wrist);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetWristSetpoint::Initialize() {
|
||||
Robot::wrist.SetSetpoint(m_setpoint);
|
||||
Robot::wrist.Enable();
|
||||
Robot::wrist.SetSetpoint(m_setpoint);
|
||||
Robot::wrist.Enable();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetWristSetpoint::IsFinished() {
|
||||
return Robot::wrist.OnTarget();
|
||||
}
|
||||
bool SetWristSetpoint::IsFinished() { return Robot::wrist.OnTarget(); }
|
||||
|
||||
@@ -15,11 +15,11 @@
|
||||
* Other commands using the wrist should make sure they disable PID!
|
||||
*/
|
||||
class SetWristSetpoint : public frc::Command {
|
||||
public:
|
||||
explicit SetWristSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
explicit SetWristSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double m_setpoint;
|
||||
private:
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -11,21 +11,17 @@
|
||||
|
||||
TankDriveWithJoystick::TankDriveWithJoystick()
|
||||
: frc::Command("TankDriveWithJoystick") {
|
||||
Requires(&Robot::drivetrain);
|
||||
Requires(&Robot::drivetrain);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void TankDriveWithJoystick::Execute() {
|
||||
auto& joystick = Robot::oi.GetJoystick();
|
||||
Robot::drivetrain.Drive(-joystick.GetY(), -joystick.GetRawAxis(4));
|
||||
auto& joystick = Robot::oi.GetJoystick();
|
||||
Robot::drivetrain.Drive(-joystick.GetY(), -joystick.GetRawAxis(4));
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool TankDriveWithJoystick::IsFinished() {
|
||||
return false;
|
||||
}
|
||||
bool TankDriveWithJoystick::IsFinished() { return false; }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void TankDriveWithJoystick::End() {
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
void TankDriveWithJoystick::End() { Robot::drivetrain.Drive(0, 0); }
|
||||
|
||||
@@ -13,9 +13,9 @@
|
||||
* Have the robot drive tank style using the PS3 Joystick until interrupted.
|
||||
*/
|
||||
class TankDriveWithJoystick : public frc::Command {
|
||||
public:
|
||||
TankDriveWithJoystick();
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
TankDriveWithJoystick();
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
};
|
||||
|
||||
@@ -18,21 +18,19 @@
|
||||
#include "Commands/SetElevatorSetpoint.h"
|
||||
|
||||
OI::OI() {
|
||||
frc::SmartDashboard::PutData("Open Claw", new OpenClaw());
|
||||
frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
|
||||
frc::SmartDashboard::PutData("Open Claw", new OpenClaw());
|
||||
frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
|
||||
|
||||
// Connect the buttons to commands
|
||||
m_dUp.WhenPressed(new SetElevatorSetpoint(0.2));
|
||||
m_dDown.WhenPressed(new SetElevatorSetpoint(-0.2));
|
||||
m_dRight.WhenPressed(new CloseClaw());
|
||||
m_dLeft.WhenPressed(new OpenClaw());
|
||||
// Connect the buttons to commands
|
||||
m_dUp.WhenPressed(new SetElevatorSetpoint(0.2));
|
||||
m_dDown.WhenPressed(new SetElevatorSetpoint(-0.2));
|
||||
m_dRight.WhenPressed(new CloseClaw());
|
||||
m_dLeft.WhenPressed(new OpenClaw());
|
||||
|
||||
m_r1.WhenPressed(new PrepareToPickup());
|
||||
m_r2.WhenPressed(new Pickup());
|
||||
m_l1.WhenPressed(new Place());
|
||||
m_l2.WhenPressed(new Autonomous());
|
||||
m_r1.WhenPressed(new PrepareToPickup());
|
||||
m_r2.WhenPressed(new Pickup());
|
||||
m_l1.WhenPressed(new Place());
|
||||
m_l2.WhenPressed(new Autonomous());
|
||||
}
|
||||
|
||||
frc::Joystick& OI::GetJoystick() {
|
||||
return m_joy;
|
||||
}
|
||||
frc::Joystick& OI::GetJoystick() { return m_joy; }
|
||||
|
||||
@@ -11,20 +11,20 @@
|
||||
#include <Joystick.h>
|
||||
|
||||
class OI {
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick& GetJoystick();
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick& GetJoystick();
|
||||
|
||||
private:
|
||||
frc::Joystick m_joy{0};
|
||||
private:
|
||||
frc::Joystick m_joy{0};
|
||||
|
||||
// Create some buttons
|
||||
frc::JoystickButton m_dUp{&m_joy, 5};
|
||||
frc::JoystickButton m_dRight{&m_joy, 6};
|
||||
frc::JoystickButton m_dDown{&m_joy, 7};
|
||||
frc::JoystickButton m_dLeft{&m_joy, 8};
|
||||
frc::JoystickButton m_l2{&m_joy, 9};
|
||||
frc::JoystickButton m_r2{&m_joy, 10};
|
||||
frc::JoystickButton m_l1{&m_joy, 11};
|
||||
frc::JoystickButton m_r1{&m_joy, 12};
|
||||
// Create some buttons
|
||||
frc::JoystickButton m_dUp{&m_joy, 5};
|
||||
frc::JoystickButton m_dRight{&m_joy, 6};
|
||||
frc::JoystickButton m_dDown{&m_joy, 7};
|
||||
frc::JoystickButton m_dLeft{&m_joy, 8};
|
||||
frc::JoystickButton m_l2{&m_joy, 9};
|
||||
frc::JoystickButton m_r2{&m_joy, 10};
|
||||
frc::JoystickButton m_l1{&m_joy, 11};
|
||||
frc::JoystickButton m_r1{&m_joy, 12};
|
||||
};
|
||||
|
||||
@@ -16,34 +16,29 @@ Claw Robot::claw;
|
||||
OI Robot::oi;
|
||||
|
||||
void Robot::RobotInit() {
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(&drivetrain);
|
||||
frc::SmartDashboard::PutData(&elevator);
|
||||
frc::SmartDashboard::PutData(&wrist);
|
||||
frc::SmartDashboard::PutData(&claw);
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(&drivetrain);
|
||||
frc::SmartDashboard::PutData(&elevator);
|
||||
frc::SmartDashboard::PutData(&wrist);
|
||||
frc::SmartDashboard::PutData(&claw);
|
||||
}
|
||||
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand.Start();
|
||||
std::cout << "Starting Auto" << std::endl;
|
||||
m_autonomousCommand.Start();
|
||||
std::cout << "Starting Auto" << std::endl;
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
}
|
||||
void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
m_autonomousCommand.Cancel();
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
// This makes sure that the autonomous stops running when teleop starts
|
||||
// running. If you want the autonomous to continue until interrupted by
|
||||
// another command, remove this line or comment it out.
|
||||
m_autonomousCommand.Cancel();
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
}
|
||||
|
||||
void Robot::TeleopPeriodic() {
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
}
|
||||
void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
|
||||
@@ -21,21 +21,21 @@
|
||||
#include "Subsystems/Wrist.h"
|
||||
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
static DriveTrain drivetrain;
|
||||
static Elevator elevator;
|
||||
static Wrist wrist;
|
||||
static Claw claw;
|
||||
static OI oi;
|
||||
public:
|
||||
static DriveTrain drivetrain;
|
||||
static Elevator elevator;
|
||||
static Wrist wrist;
|
||||
static Claw claw;
|
||||
static OI oi;
|
||||
|
||||
private:
|
||||
Autonomous m_autonomousCommand;
|
||||
frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
|
||||
private:
|
||||
Autonomous m_autonomousCommand;
|
||||
frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
|
||||
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
};
|
||||
|
||||
@@ -7,28 +7,19 @@
|
||||
|
||||
#include "Claw.h"
|
||||
|
||||
Claw::Claw()
|
||||
: frc::Subsystem("Claw") {
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
Claw::Claw() : frc::Subsystem("Claw") {
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
}
|
||||
|
||||
void Claw::InitDefaultCommand() {}
|
||||
|
||||
void Claw::Open() {
|
||||
m_motor.Set(-1);
|
||||
}
|
||||
void Claw::Open() { m_motor.Set(-1); }
|
||||
|
||||
void Claw::Close() {
|
||||
m_motor.Set(1);
|
||||
}
|
||||
void Claw::Close() { m_motor.Set(1); }
|
||||
|
||||
void Claw::Stop() {
|
||||
m_motor.Set(0);
|
||||
}
|
||||
void Claw::Stop() { m_motor.Set(0); }
|
||||
|
||||
bool Claw::IsGripping() {
|
||||
return m_contact.Get();
|
||||
}
|
||||
bool Claw::IsGripping() { return m_contact.Get(); }
|
||||
|
||||
void Claw::Log() {}
|
||||
|
||||
@@ -17,35 +17,35 @@
|
||||
* motors don't stall.
|
||||
*/
|
||||
class Claw : public frc::Subsystem {
|
||||
public:
|
||||
Claw();
|
||||
public:
|
||||
Claw();
|
||||
|
||||
void InitDefaultCommand() override;
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* Set the claw motor to move in the open direction.
|
||||
*/
|
||||
void Open();
|
||||
/**
|
||||
* Set the claw motor to move in the open direction.
|
||||
*/
|
||||
void Open();
|
||||
|
||||
/**
|
||||
* Set the claw motor to move in the close direction.
|
||||
*/
|
||||
void Close();
|
||||
/**
|
||||
* Set the claw motor to move in the close direction.
|
||||
*/
|
||||
void Close();
|
||||
|
||||
/**
|
||||
* Stops the claw motor from moving.
|
||||
*/
|
||||
void Stop();
|
||||
/**
|
||||
* Stops the claw motor from moving.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* Return true when the robot is grabbing an object hard enough
|
||||
* to trigger the limit switch.
|
||||
*/
|
||||
bool IsGripping();
|
||||
/**
|
||||
* Return true when the robot is grabbing an object hard enough
|
||||
* to trigger the limit switch.
|
||||
*/
|
||||
bool IsGripping();
|
||||
|
||||
void Log();
|
||||
void Log();
|
||||
|
||||
private:
|
||||
frc::Spark m_motor{7};
|
||||
frc::DigitalInput m_contact{5};
|
||||
private:
|
||||
frc::Spark m_motor{7};
|
||||
frc::DigitalInput m_contact{5};
|
||||
};
|
||||
|
||||
@@ -12,33 +12,32 @@
|
||||
|
||||
#include "../Commands/TankDriveWithJoystick.h"
|
||||
|
||||
DriveTrain::DriveTrain()
|
||||
: frc::Subsystem("DriveTrain") {
|
||||
DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
|
||||
// Encoders may measure differently in the real world and in
|
||||
// simulation. In this example the robot moves 0.042 barleycorns
|
||||
// per tick in the real world, but the simulated encoders
|
||||
// simulate 360 tick encoders. This if statement allows for the
|
||||
// real robot to handle this difference in devices.
|
||||
#ifndef SIMULATION
|
||||
m_leftEncoder.SetDistancePerPulse(0.042);
|
||||
m_rightEncoder.SetDistancePerPulse(0.042);
|
||||
m_leftEncoder.SetDistancePerPulse(0.042);
|
||||
m_rightEncoder.SetDistancePerPulse(0.042);
|
||||
#else
|
||||
// Circumference in ft = 4in/12(in/ft)*PI
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
|
||||
// Circumference in ft = 4in/12(in/ft)*PI
|
||||
m_leftEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
|
||||
360.0);
|
||||
m_rightEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
|
||||
360.0);
|
||||
#endif
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
// AddChild("Front_Left Motor", m_frontLeft);
|
||||
// AddChild("Rear Left Motor", m_rearLeft);
|
||||
// AddChild("Front Right Motor", m_frontRight);
|
||||
// AddChild("Rear Right Motor", m_rearRight);
|
||||
AddChild("Left Encoder", m_leftEncoder);
|
||||
AddChild("Right Encoder", m_rightEncoder);
|
||||
AddChild("Rangefinder", m_rangefinder);
|
||||
AddChild("Gyro", m_gyro);
|
||||
// Let's show everything on the LiveWindow
|
||||
// AddChild("Front_Left Motor", m_frontLeft);
|
||||
// AddChild("Rear Left Motor", m_rearLeft);
|
||||
// AddChild("Front Right Motor", m_frontRight);
|
||||
// AddChild("Rear Right Motor", m_rearRight);
|
||||
AddChild("Left Encoder", m_leftEncoder);
|
||||
AddChild("Right Encoder", m_rightEncoder);
|
||||
AddChild("Rangefinder", m_rangefinder);
|
||||
AddChild("Gyro", m_gyro);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -46,42 +45,38 @@ DriveTrain::DriveTrain()
|
||||
* using the PS3 joystick.
|
||||
*/
|
||||
void DriveTrain::InitDefaultCommand() {
|
||||
SetDefaultCommand(new TankDriveWithJoystick());
|
||||
SetDefaultCommand(new TankDriveWithJoystick());
|
||||
}
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void DriveTrain::Log() {
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Left Distance", m_leftEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Right Distance", m_rightEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
|
||||
frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Right Distance",
|
||||
m_rightEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
|
||||
}
|
||||
|
||||
void DriveTrain::Drive(double left, double right) {
|
||||
m_robotDrive.TankDrive(left, right);
|
||||
m_robotDrive.TankDrive(left, right);
|
||||
}
|
||||
|
||||
double DriveTrain::GetHeading() {
|
||||
return m_gyro.GetAngle();
|
||||
}
|
||||
double DriveTrain::GetHeading() { return m_gyro.GetAngle(); }
|
||||
|
||||
void DriveTrain::Reset() {
|
||||
m_gyro.Reset();
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
m_gyro.Reset();
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double DriveTrain::GetDistance() {
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance())
|
||||
/ 2.0;
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
|
||||
}
|
||||
|
||||
double DriveTrain::GetDistanceToObstacle() {
|
||||
// Really meters in simulation since it's a rangefinder...
|
||||
return m_rangefinder.GetAverageVoltage();
|
||||
// Really meters in simulation since it's a rangefinder...
|
||||
return m_rangefinder.GetAverageVoltage();
|
||||
}
|
||||
|
||||
@@ -25,60 +25,60 @@ class Joystick;
|
||||
* and a gyro.
|
||||
*/
|
||||
class DriveTrain : public frc::Subsystem {
|
||||
public:
|
||||
DriveTrain();
|
||||
public:
|
||||
DriveTrain();
|
||||
|
||||
/**
|
||||
* When no other command is running let the operator drive around
|
||||
* using the PS3 joystick.
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
/**
|
||||
* When no other command is running let the operator drive around
|
||||
* using the PS3 joystick.
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Tank style driving for the DriveTrain.
|
||||
* @param left Speed in range [-1,1]
|
||||
* @param right Speed in range [-1,1]
|
||||
*/
|
||||
void Drive(double left, double right);
|
||||
/**
|
||||
* Tank style driving for the DriveTrain.
|
||||
* @param left Speed in range [-1,1]
|
||||
* @param right Speed in range [-1,1]
|
||||
*/
|
||||
void Drive(double left, double right);
|
||||
|
||||
/**
|
||||
* @return The robots heading in degrees.
|
||||
*/
|
||||
double GetHeading();
|
||||
/**
|
||||
* @return The robots heading in degrees.
|
||||
*/
|
||||
double GetHeading();
|
||||
|
||||
/**
|
||||
* Reset the robots sensors to the zero states.
|
||||
*/
|
||||
void Reset();
|
||||
/**
|
||||
* Reset the robots sensors to the zero states.
|
||||
*/
|
||||
void Reset();
|
||||
|
||||
/**
|
||||
* @return The distance driven (average of left and right encoders).
|
||||
*/
|
||||
double GetDistance();
|
||||
/**
|
||||
* @return The distance driven (average of left and right encoders).
|
||||
*/
|
||||
double GetDistance();
|
||||
|
||||
/**
|
||||
* @return The distance to the obstacle detected by the rangefinder.
|
||||
*/
|
||||
double GetDistanceToObstacle();
|
||||
/**
|
||||
* @return The distance to the obstacle detected by the rangefinder.
|
||||
*/
|
||||
double GetDistanceToObstacle();
|
||||
|
||||
private:
|
||||
frc::Spark m_frontLeft{1};
|
||||
frc::Spark m_rearLeft{2};
|
||||
frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
|
||||
private:
|
||||
frc::Spark m_frontLeft{1};
|
||||
frc::Spark m_rearLeft{2};
|
||||
frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
|
||||
|
||||
frc::Spark m_frontRight{3};
|
||||
frc::Spark m_rearRight{4};
|
||||
frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
|
||||
frc::Spark m_frontRight{3};
|
||||
frc::Spark m_rearRight{4};
|
||||
frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
|
||||
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
|
||||
frc::Encoder m_leftEncoder{1, 2};
|
||||
frc::Encoder m_rightEncoder{3, 4};
|
||||
frc::AnalogInput m_rangefinder{6};
|
||||
frc::AnalogGyro m_gyro{1};
|
||||
frc::Encoder m_leftEncoder{1, 2};
|
||||
frc::Encoder m_rightEncoder{3, 4};
|
||||
frc::AnalogInput m_rangefinder{6};
|
||||
frc::AnalogGyro m_gyro{1};
|
||||
};
|
||||
|
||||
@@ -10,28 +10,23 @@
|
||||
#include <LiveWindow/LiveWindow.h>
|
||||
#include <SmartDashboard/SmartDashboard.h>
|
||||
|
||||
Elevator::Elevator()
|
||||
: frc::PIDSubsystem("Elevator", kP_real, kI_real, 0.0) {
|
||||
Elevator::Elevator() : frc::PIDSubsystem("Elevator", kP_real, kI_real, 0.0) {
|
||||
#ifdef SIMULATION // Check for simulation and update PID values
|
||||
GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
|
||||
GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
|
||||
#endif
|
||||
SetAbsoluteTolerance(0.005);
|
||||
SetAbsoluteTolerance(0.005);
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
AddChild("Pot", &m_pot);
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
AddChild("Pot", &m_pot);
|
||||
}
|
||||
|
||||
void Elevator::InitDefaultCommand() {}
|
||||
|
||||
void Elevator::Log() {
|
||||
// frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
|
||||
// frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
|
||||
}
|
||||
|
||||
double Elevator::ReturnPIDInput() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
double Elevator::ReturnPIDInput() { return m_pot.Get(); }
|
||||
|
||||
void Elevator::UsePIDOutput(double d) {
|
||||
m_motor.Set(d);
|
||||
}
|
||||
void Elevator::UsePIDOutput(double d) { m_motor.Set(d); }
|
||||
|
||||
@@ -18,42 +18,42 @@
|
||||
* minor differences.
|
||||
*/
|
||||
class Elevator : public frc::PIDSubsystem {
|
||||
public:
|
||||
Elevator();
|
||||
public:
|
||||
Elevator();
|
||||
|
||||
void InitDefaultCommand() override;
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by
|
||||
* the subsystem.
|
||||
*/
|
||||
void UsePIDOutput(double d) override;
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by
|
||||
* the subsystem.
|
||||
*/
|
||||
void UsePIDOutput(double d) override;
|
||||
|
||||
private:
|
||||
frc::Spark m_motor{5};
|
||||
private:
|
||||
frc::Spark m_motor{5};
|
||||
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
#ifndef SIMULATION
|
||||
frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
|
||||
frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
|
||||
#else
|
||||
frc::AnalogPotentiometer m_pot{2}; // Defaults to meters
|
||||
frc::AnalogPotentiometer m_pot{2}; // Defaults to meters
|
||||
#endif
|
||||
|
||||
static constexpr double kP_real = 4;
|
||||
static constexpr double kI_real = 0.07;
|
||||
static constexpr double kP_simulation = 18;
|
||||
static constexpr double kI_simulation = 0.2;
|
||||
static constexpr double kP_real = 4;
|
||||
static constexpr double kI_real = 0.07;
|
||||
static constexpr double kP_simulation = 18;
|
||||
static constexpr double kI_simulation = 0.2;
|
||||
};
|
||||
|
||||
@@ -9,28 +9,23 @@
|
||||
|
||||
#include <SmartDashboard/SmartDashboard.h>
|
||||
|
||||
Wrist::Wrist()
|
||||
: frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
|
||||
Wrist::Wrist() : frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
|
||||
#ifdef SIMULATION // Check for simulation and update PID values
|
||||
GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
|
||||
GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
|
||||
#endif
|
||||
SetAbsoluteTolerance(2.5);
|
||||
SetAbsoluteTolerance(2.5);
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
AddChild("Pot", m_pot);
|
||||
// Let's show everything on the LiveWindow
|
||||
AddChild("Motor", m_motor);
|
||||
AddChild("Pot", m_pot);
|
||||
}
|
||||
|
||||
void Wrist::InitDefaultCommand() {}
|
||||
|
||||
void Wrist::Log() {
|
||||
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
|
||||
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
|
||||
}
|
||||
|
||||
double Wrist::ReturnPIDInput() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
double Wrist::ReturnPIDInput() { return m_pot.Get(); }
|
||||
|
||||
void Wrist::UsePIDOutput(double d) {
|
||||
m_motor.Set(d);
|
||||
}
|
||||
void Wrist::UsePIDOutput(double d) { m_motor.Set(d); }
|
||||
|
||||
@@ -16,40 +16,40 @@
|
||||
* of a linear joint.
|
||||
*/
|
||||
class Wrist : public PIDSubsystem {
|
||||
public:
|
||||
Wrist();
|
||||
public:
|
||||
Wrist();
|
||||
|
||||
void InitDefaultCommand() override;
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
/**
|
||||
* The log method puts interesting information to the SmartDashboard.
|
||||
*/
|
||||
void Log();
|
||||
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
/**
|
||||
* Use the potentiometer as the PID sensor. This method is automatically
|
||||
* called by the subsystem.
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by
|
||||
* the subsystem.
|
||||
*/
|
||||
void UsePIDOutput(double d) override;
|
||||
/**
|
||||
* Use the motor as the PID output. This method is automatically called
|
||||
* by
|
||||
* the subsystem.
|
||||
*/
|
||||
void UsePIDOutput(double d) override;
|
||||
|
||||
private:
|
||||
frc::Spark m_motor{6};
|
||||
private:
|
||||
frc::Spark m_motor{6};
|
||||
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
#ifndef SIMULATION
|
||||
frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
|
||||
frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
|
||||
#else
|
||||
frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
|
||||
frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
|
||||
#endif
|
||||
|
||||
static constexpr double kP_real = 1;
|
||||
static constexpr double kP_simulation = 0.05;
|
||||
static constexpr double kP_real = 1;
|
||||
static constexpr double kP_simulation = 0.05;
|
||||
};
|
||||
|
||||
@@ -13,46 +13,46 @@
|
||||
#include <Timer.h>
|
||||
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
Robot() {
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_timer.Start();
|
||||
}
|
||||
public:
|
||||
Robot() {
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
void AutonomousInit() override {
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
void AutonomousInit() override {
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
void AutonomousPeriodic() override {
|
||||
// Drive for 2 seconds
|
||||
if (m_timer.Get() < 2.0) {
|
||||
// Drive forwards half speed
|
||||
m_robotDrive.ArcadeDrive(-0.5, 0.0);
|
||||
} else {
|
||||
// Stop robot
|
||||
m_robotDrive.ArcadeDrive(0.0, 0.0);
|
||||
}
|
||||
}
|
||||
void AutonomousPeriodic() override {
|
||||
// Drive for 2 seconds
|
||||
if (m_timer.Get() < 2.0) {
|
||||
// Drive forwards half speed
|
||||
m_robotDrive.ArcadeDrive(-0.5, 0.0);
|
||||
} else {
|
||||
// Stop robot
|
||||
m_robotDrive.ArcadeDrive(0.0, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
void TeleopInit() override {}
|
||||
void TeleopInit() override {}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Drive with arcade style (use right stick)
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
void TeleopPeriodic() override {
|
||||
// Drive with arcade style (use right stick)
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
|
||||
void TestPeriodic() override {}
|
||||
void TestPeriodic() override {}
|
||||
|
||||
private:
|
||||
// Robot drive system
|
||||
frc::Spark m_left{0};
|
||||
frc::Spark m_right{1};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
private:
|
||||
// Robot drive system
|
||||
frc::Spark m_left{0};
|
||||
frc::Spark m_right{1};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
|
||||
frc::Joystick m_stick{0};
|
||||
frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
|
||||
frc::Timer m_timer;
|
||||
frc::Joystick m_stick{0};
|
||||
frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
|
||||
frc::Timer m_timer;
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -19,42 +19,40 @@
|
||||
* backwards while the gyro is used for direction keeping.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
}
|
||||
public:
|
||||
void RobotInit() override { m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); }
|
||||
|
||||
/**
|
||||
* The motor speed is set from the joystick while the DifferentialDrive
|
||||
* turning value is assigned from the error between the setpoint and the
|
||||
* gyro angle.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
|
||||
// Invert the direction of the turn if we are going backwards
|
||||
turningValue = std::copysign(turningValue, m_joystick.GetY());
|
||||
m_robotDrive.ArcadeDrive(m_joystick.GetY(), turningValue);
|
||||
}
|
||||
/**
|
||||
* The motor speed is set from the joystick while the DifferentialDrive
|
||||
* turning value is assigned from the error between the setpoint and the gyro
|
||||
* angle.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
|
||||
// Invert the direction of the turn if we are going backwards
|
||||
turningValue = std::copysign(turningValue, m_joystick.GetY());
|
||||
m_robotDrive.ArcadeDrive(m_joystick.GetY(), turningValue);
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr double kAngleSetpoint = 0.0;
|
||||
static constexpr double kP = 0.005; // Proportional turning constant
|
||||
private:
|
||||
static constexpr double kAngleSetpoint = 0.0;
|
||||
static constexpr double kP = 0.005; // Proportional turning constant
|
||||
|
||||
// Gyro calibration constant, may need to be adjusted
|
||||
// Gyro value of 360 is set to correspond to one full revolution
|
||||
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
|
||||
// Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
|
||||
// set to correspond to one full revolution.
|
||||
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
|
||||
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kGyroPort = 0;
|
||||
static constexpr int kJoystickPort = 0;
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kGyroPort = 0;
|
||||
static constexpr int kJoystickPort = 0;
|
||||
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
|
||||
frc::AnalogGyro m_gyro{kGyroPort};
|
||||
frc::Joystick m_joystick{kJoystickPort};
|
||||
frc::AnalogGyro m_gyro{kGyroPort};
|
||||
frc::Joystick m_joystick{kJoystickPort};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -17,46 +17,45 @@
|
||||
* (field-oriented controls).
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
// invert the left side motors
|
||||
// you may need to change or remove this to match your robot
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
public:
|
||||
void RobotInit() override {
|
||||
// Invert the left side motors. You may need to change or remove this to
|
||||
// match your robot.
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
|
||||
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
}
|
||||
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
}
|
||||
|
||||
/**
|
||||
* Mecanum drive is used with the gyro angle as an input.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
m_robotDrive.DriveCartesian(m_joystick.GetX(),
|
||||
m_joystick.GetY(), m_joystick.GetZ(),
|
||||
m_gyro.GetAngle());
|
||||
}
|
||||
/**
|
||||
* Mecanum drive is used with the gyro angle as an input.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
m_robotDrive.DriveCartesian(m_joystick.GetX(), m_joystick.GetY(),
|
||||
m_joystick.GetZ(), m_gyro.GetAngle());
|
||||
}
|
||||
|
||||
private:
|
||||
// Gyro calibration constant, may need to be adjusted
|
||||
// Gyro value of 360 is set to correspond to one full revolution
|
||||
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
|
||||
private:
|
||||
// Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
|
||||
// set to correspond to one full revolution.
|
||||
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
|
||||
|
||||
static constexpr int kFrontLeftMotorPort = 0;
|
||||
static constexpr int kRearLeftMotorPort = 1;
|
||||
static constexpr int kFrontRightMotorPort = 2;
|
||||
static constexpr int kRearRightMotorPort = 3;
|
||||
static constexpr int kGyroPort = 0;
|
||||
static constexpr int kJoystickPort = 0;
|
||||
static constexpr int kFrontLeftMotorPort = 0;
|
||||
static constexpr int kRearLeftMotorPort = 1;
|
||||
static constexpr int kFrontRightMotorPort = 2;
|
||||
static constexpr int kRearRightMotorPort = 3;
|
||||
static constexpr int kGyroPort = 0;
|
||||
static constexpr int kJoystickPort = 0;
|
||||
|
||||
frc::Spark m_frontLeft{kFrontLeftMotorPort};
|
||||
frc::Spark m_rearLeft{kRearLeftMotorPort};
|
||||
frc::Spark m_frontRight{kFrontRightMotorPort};
|
||||
frc::Spark m_rearRight{kRearRightMotorPort};
|
||||
frc::MecanumDrive m_robotDrive{
|
||||
m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
|
||||
frc::Spark m_frontLeft{kFrontLeftMotorPort};
|
||||
frc::Spark m_rearLeft{kRearLeftMotorPort};
|
||||
frc::Spark m_frontRight{kFrontRightMotorPort};
|
||||
frc::Spark m_rearRight{kRearRightMotorPort};
|
||||
frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
|
||||
m_rearRight};
|
||||
|
||||
frc::AnalogGyro m_gyro{kGyroPort};
|
||||
frc::Joystick m_joystick{kJoystickPort};
|
||||
frc::AnalogGyro m_gyro{kGyroPort};
|
||||
frc::Joystick m_joystick{kJoystickPort};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -17,62 +17,59 @@
|
||||
/**
|
||||
* This is a demo program showing the use of OpenCV to do vision processing. The
|
||||
* image is acquired from the USB camera, then a rectangle is put on the image
|
||||
* and
|
||||
* sent to the dashboard. OpenCV has many methods for different types of
|
||||
* and sent to the dashboard. OpenCV has many methods for different types of
|
||||
* processing.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
#if defined(__linux__)
|
||||
private:
|
||||
static void VisionThread() {
|
||||
// Get the USB camera from CameraServer
|
||||
cs::UsbCamera camera =
|
||||
CameraServer::GetInstance()
|
||||
->StartAutomaticCapture();
|
||||
// Set the resolution
|
||||
camera.SetResolution(640, 480);
|
||||
|
||||
// Get a CvSink. This will capture Mats from the Camera
|
||||
cs::CvSink cvSink = CameraServer::GetInstance()->GetVideo();
|
||||
// Setup a CvSource. This will send images back to the Dashboard
|
||||
cs::CvSource outputStream =
|
||||
CameraServer::GetInstance()->PutVideo(
|
||||
"Rectangle", 640, 480);
|
||||
private:
|
||||
static void VisionThread() {
|
||||
// Get the USB camera from CameraServer
|
||||
cs::UsbCamera camera = CameraServer::GetInstance()->StartAutomaticCapture();
|
||||
// Set the resolution
|
||||
camera.SetResolution(640, 480);
|
||||
|
||||
// Mats are very memory expensive. Lets reuse this Mat.
|
||||
cv::Mat mat;
|
||||
// Get a CvSink. This will capture Mats from the Camera
|
||||
cs::CvSink cvSink = CameraServer::GetInstance()->GetVideo();
|
||||
// Setup a CvSource. This will send images back to the Dashboard
|
||||
cs::CvSource outputStream =
|
||||
CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
|
||||
|
||||
while (true) {
|
||||
// Tell the CvSink to grab a frame from the camera and
|
||||
// put it
|
||||
// in the source mat. If there is an error notify the
|
||||
// output.
|
||||
if (cvSink.GrabFrame(mat) == 0) {
|
||||
// Send the output the error.
|
||||
outputStream.NotifyError(cvSink.GetError());
|
||||
// skip the rest of the current iteration
|
||||
continue;
|
||||
}
|
||||
// Put a rectangle on the image
|
||||
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
|
||||
cv::Scalar(255, 255, 255), 5);
|
||||
// Give the output stream a new image to display
|
||||
outputStream.PutFrame(mat);
|
||||
}
|
||||
}
|
||||
// Mats are very memory expensive. Lets reuse this Mat.
|
||||
cv::Mat mat;
|
||||
|
||||
while (true) {
|
||||
// Tell the CvSink to grab a frame from the camera and
|
||||
// put it
|
||||
// in the source mat. If there is an error notify the
|
||||
// output.
|
||||
if (cvSink.GrabFrame(mat) == 0) {
|
||||
// Send the output the error.
|
||||
outputStream.NotifyError(cvSink.GetError());
|
||||
// skip the rest of the current iteration
|
||||
continue;
|
||||
}
|
||||
// Put a rectangle on the image
|
||||
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
|
||||
cv::Scalar(255, 255, 255), 5);
|
||||
// Give the output stream a new image to display
|
||||
outputStream.PutFrame(mat);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void RobotInit() override {
|
||||
// We need to run our vision program in a separate Thread.
|
||||
// If not, our robot program will not run
|
||||
void RobotInit() override {
|
||||
// We need to run our vision program in a separate thread. If not, our robot
|
||||
// program will not run.
|
||||
#if defined(__linux__)
|
||||
std::thread visionThread(VisionThread);
|
||||
visionThread.detach();
|
||||
std::thread visionThread(VisionThread);
|
||||
visionThread.detach();
|
||||
#else
|
||||
wpi::errs() << "Vision only available on Linux.\n";
|
||||
wpi::errs().flush();
|
||||
wpi::errs() << "Vision only available on Linux.\n";
|
||||
wpi::errs().flush();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -15,39 +15,37 @@
|
||||
* MecanumDrive class.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
// Invert the left side motors
|
||||
// You may need to change or remove this to match your robot
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
}
|
||||
public:
|
||||
void RobotInit() override {
|
||||
// Invert the left side motors. You may need to change or remove this to
|
||||
// match your robot.
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
/* Use the joystick X axis for lateral movement, Y axis for
|
||||
* forward
|
||||
* movement, and Z axis for rotation.
|
||||
*/
|
||||
m_robotDrive.DriveCartesian(
|
||||
m_stick.GetX(), m_stick.GetY(), m_stick.GetZ());
|
||||
}
|
||||
void TeleopPeriodic() override {
|
||||
/* Use the joystick X axis for lateral movement, Y axis for forward
|
||||
* movement, and Z axis for rotation.
|
||||
*/
|
||||
m_robotDrive.DriveCartesian(m_stick.GetX(), m_stick.GetY(), m_stick.GetZ());
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr int kFrontLeftChannel = 0;
|
||||
static constexpr int kRearLeftChannel = 1;
|
||||
static constexpr int kFrontRightChannel = 2;
|
||||
static constexpr int kRearRightChannel = 3;
|
||||
private:
|
||||
static constexpr int kFrontLeftChannel = 0;
|
||||
static constexpr int kRearLeftChannel = 1;
|
||||
static constexpr int kFrontRightChannel = 2;
|
||||
static constexpr int kRearRightChannel = 3;
|
||||
|
||||
static constexpr int kJoystickChannel = 0;
|
||||
static constexpr int kJoystickChannel = 0;
|
||||
|
||||
frc::Spark m_frontLeft{kFrontLeftChannel};
|
||||
frc::Spark m_rearLeft{kRearLeftChannel};
|
||||
frc::Spark m_frontRight{kFrontRightChannel};
|
||||
frc::Spark m_rearRight{kRearRightChannel};
|
||||
frc::MecanumDrive m_robotDrive{
|
||||
m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
|
||||
frc::Spark m_frontLeft{kFrontLeftChannel};
|
||||
frc::Spark m_rearLeft{kRearLeftChannel};
|
||||
frc::Spark m_frontRight{kFrontRightChannel};
|
||||
frc::Spark m_rearRight{kRearRightChannel};
|
||||
frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
|
||||
m_rearRight};
|
||||
|
||||
frc::Joystick m_stick{kJoystickChannel};
|
||||
frc::Joystick m_stick{kJoystickChannel};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -18,12 +18,12 @@
|
||||
* range from -1 to 1 making it easy to work together.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
|
||||
public:
|
||||
void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
|
||||
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
frc::Spark m_motor{0};
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
frc::Spark m_motor{0};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -9,11 +9,9 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
CheckForHotGoal::CheckForHotGoal(double time) {
|
||||
SetTimeout(time);
|
||||
}
|
||||
CheckForHotGoal::CheckForHotGoal(double time) { SetTimeout(time); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool CheckForHotGoal::IsFinished() {
|
||||
return IsTimedOut() || Robot::shooter.GoalIsHot();
|
||||
return IsTimedOut() || Robot::shooter.GoalIsHot();
|
||||
}
|
||||
|
||||
@@ -16,7 +16,7 @@
|
||||
* the hot goal is detected or until it is timed out.
|
||||
*/
|
||||
class CheckForHotGoal : public frc::Command {
|
||||
public:
|
||||
explicit CheckForHotGoal(double time);
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
explicit CheckForHotGoal(double time);
|
||||
bool IsFinished() override;
|
||||
};
|
||||
|
||||
@@ -9,11 +9,7 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
CloseClaw::CloseClaw() {
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
CloseClaw::CloseClaw() { Requires(&Robot::collector); }
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void CloseClaw::Initialize() {
|
||||
Robot::collector.Close();
|
||||
}
|
||||
void CloseClaw::Initialize() { Robot::collector.Close(); }
|
||||
|
||||
@@ -16,7 +16,7 @@
|
||||
* detect that.
|
||||
*/
|
||||
class CloseClaw : public frc::InstantCommand {
|
||||
public:
|
||||
CloseClaw();
|
||||
void Initialize() override;
|
||||
public:
|
||||
CloseClaw();
|
||||
void Initialize() override;
|
||||
};
|
||||
|
||||
@@ -14,8 +14,8 @@
|
||||
#include "WaitForBall.h"
|
||||
|
||||
Collect::Collect() {
|
||||
AddSequential(new SetCollectionSpeed(Collector::kForward));
|
||||
AddParallel(new CloseClaw());
|
||||
AddSequential(new SetPivotSetpoint(Pivot::kCollect));
|
||||
AddSequential(new WaitForBall());
|
||||
AddSequential(new SetCollectionSpeed(Collector::kForward));
|
||||
AddParallel(new CloseClaw());
|
||||
AddSequential(new SetPivotSetpoint(Pivot::kCollect));
|
||||
AddSequential(new WaitForBall());
|
||||
}
|
||||
|
||||
@@ -13,6 +13,6 @@
|
||||
* Get the robot set to collect balls.
|
||||
*/
|
||||
class Collect : public frc::CommandGroup {
|
||||
public:
|
||||
Collect();
|
||||
public:
|
||||
Collect();
|
||||
};
|
||||
|
||||
@@ -16,13 +16,13 @@
|
||||
#include "WaitForPressure.h"
|
||||
|
||||
DriveAndShootAutonomous::DriveAndShootAutonomous() {
|
||||
AddSequential(new CloseClaw());
|
||||
AddSequential(new WaitForPressure(), 2);
|
||||
AddSequential(new CloseClaw());
|
||||
AddSequential(new WaitForPressure(), 2);
|
||||
#ifndef SIMULATION
|
||||
// NOTE: Simulation doesn't currently have the concept of hot.
|
||||
AddSequential(new CheckForHotGoal(2));
|
||||
// NOTE: Simulation doesn't currently have the concept of hot.
|
||||
AddSequential(new CheckForHotGoal(2));
|
||||
#endif
|
||||
AddSequential(new SetPivotSetpoint(45));
|
||||
AddSequential(new DriveForward(8, 0.3));
|
||||
AddSequential(new Shoot());
|
||||
AddSequential(new SetPivotSetpoint(45));
|
||||
AddSequential(new DriveForward(8, 0.3));
|
||||
AddSequential(new Shoot());
|
||||
}
|
||||
|
||||
@@ -14,6 +14,6 @@
|
||||
* it will wait briefly.
|
||||
*/
|
||||
class DriveAndShootAutonomous : public frc::CommandGroup {
|
||||
public:
|
||||
DriveAndShootAutonomous();
|
||||
public:
|
||||
DriveAndShootAutonomous();
|
||||
};
|
||||
|
||||
@@ -12,48 +12,40 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
void DriveForward::init(double dist, double maxSpeed) {
|
||||
Requires(&Robot::drivetrain);
|
||||
m_distance = dist;
|
||||
m_driveForwardSpeed = maxSpeed;
|
||||
Requires(&Robot::drivetrain);
|
||||
m_distance = dist;
|
||||
m_driveForwardSpeed = maxSpeed;
|
||||
}
|
||||
|
||||
DriveForward::DriveForward() {
|
||||
init(10, 0.5);
|
||||
}
|
||||
DriveForward::DriveForward() { init(10, 0.5); }
|
||||
|
||||
DriveForward::DriveForward(double dist) {
|
||||
init(dist, 0.5);
|
||||
}
|
||||
DriveForward::DriveForward(double dist) { init(dist, 0.5); }
|
||||
|
||||
DriveForward::DriveForward(double dist, double maxSpeed) {
|
||||
init(dist, maxSpeed);
|
||||
init(dist, maxSpeed);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void DriveForward::Initialize() {
|
||||
Robot::drivetrain.GetRightEncoder().Reset();
|
||||
SetTimeout(2);
|
||||
Robot::drivetrain.GetRightEncoder().Reset();
|
||||
SetTimeout(2);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void DriveForward::Execute() {
|
||||
m_error = (m_distance
|
||||
- Robot::drivetrain.GetRightEncoder().GetDistance());
|
||||
if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
|
||||
Robot::drivetrain.TankDrive(
|
||||
m_driveForwardSpeed, m_driveForwardSpeed);
|
||||
} else {
|
||||
Robot::drivetrain.TankDrive(m_driveForwardSpeed * kP * m_error,
|
||||
m_driveForwardSpeed * kP * m_error);
|
||||
}
|
||||
m_error = (m_distance - Robot::drivetrain.GetRightEncoder().GetDistance());
|
||||
if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
|
||||
Robot::drivetrain.TankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
|
||||
} else {
|
||||
Robot::drivetrain.TankDrive(m_driveForwardSpeed * kP * m_error,
|
||||
m_driveForwardSpeed * kP * m_error);
|
||||
}
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool DriveForward::IsFinished() {
|
||||
return (std::fabs(m_error) <= kTolerance) || IsTimedOut();
|
||||
return (std::fabs(m_error) <= kTolerance) || IsTimedOut();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveForward::End() {
|
||||
Robot::drivetrain.Stop();
|
||||
}
|
||||
void DriveForward::End() { Robot::drivetrain.Stop(); }
|
||||
|
||||
@@ -14,21 +14,21 @@
|
||||
* control This command will drive a given distance limiting to a maximum speed.
|
||||
*/
|
||||
class DriveForward : public frc::Command {
|
||||
public:
|
||||
DriveForward();
|
||||
explicit DriveForward(double dist);
|
||||
DriveForward(double dist, double maxSpeed);
|
||||
void Initialize() override;
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
DriveForward();
|
||||
explicit DriveForward(double dist);
|
||||
DriveForward(double dist, double maxSpeed);
|
||||
void Initialize() override;
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
|
||||
private:
|
||||
double m_driveForwardSpeed;
|
||||
double m_distance;
|
||||
double m_error = 0;
|
||||
static constexpr double kTolerance = 0.1;
|
||||
static constexpr double kP = -1.0 / 5.0;
|
||||
private:
|
||||
double m_driveForwardSpeed;
|
||||
double m_distance;
|
||||
double m_error = 0;
|
||||
static constexpr double kTolerance = 0.1;
|
||||
static constexpr double kP = -1.0 / 5.0;
|
||||
|
||||
void init(double dist, double maxSpeed);
|
||||
void init(double dist, double maxSpeed);
|
||||
};
|
||||
|
||||
@@ -9,22 +9,16 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
DriveWithJoystick::DriveWithJoystick() {
|
||||
Requires(&Robot::drivetrain);
|
||||
}
|
||||
DriveWithJoystick::DriveWithJoystick() { Requires(&Robot::drivetrain); }
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void DriveWithJoystick::Execute() {
|
||||
auto& joystick = Robot::oi.GetJoystick();
|
||||
Robot::drivetrain.TankDrive(joystick.GetY(), joystick.GetRawAxis(4));
|
||||
auto& joystick = Robot::oi.GetJoystick();
|
||||
Robot::drivetrain.TankDrive(joystick.GetY(), joystick.GetRawAxis(4));
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool DriveWithJoystick::IsFinished() {
|
||||
return false;
|
||||
}
|
||||
bool DriveWithJoystick::IsFinished() { return false; }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveWithJoystick::End() {
|
||||
Robot::drivetrain.Stop();
|
||||
}
|
||||
void DriveWithJoystick::End() { Robot::drivetrain.Stop(); }
|
||||
|
||||
@@ -14,9 +14,9 @@
|
||||
* except when interrupted by another command.
|
||||
*/
|
||||
class DriveWithJoystick : public frc::Command {
|
||||
public:
|
||||
DriveWithJoystick();
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
public:
|
||||
DriveWithJoystick();
|
||||
void Execute() override;
|
||||
bool IsFinished() override;
|
||||
void End() override;
|
||||
};
|
||||
|
||||
@@ -9,17 +9,12 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
ExtendShooter::ExtendShooter()
|
||||
: frc::TimedCommand(1.0) {
|
||||
Requires(&Robot::shooter);
|
||||
ExtendShooter::ExtendShooter() : frc::TimedCommand(1.0) {
|
||||
Requires(&Robot::shooter);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void ExtendShooter::Initialize() {
|
||||
Robot::shooter.ExtendBoth();
|
||||
}
|
||||
void ExtendShooter::Initialize() { Robot::shooter.ExtendBoth(); }
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void ExtendShooter::End() {
|
||||
Robot::shooter.RetractBoth();
|
||||
}
|
||||
void ExtendShooter::End() { Robot::shooter.RetractBoth(); }
|
||||
|
||||
@@ -13,8 +13,8 @@
|
||||
* Extend the shooter and then retract it after a second.
|
||||
*/
|
||||
class ExtendShooter : public frc::TimedCommand {
|
||||
public:
|
||||
ExtendShooter();
|
||||
void Initialize() override;
|
||||
void End() override;
|
||||
public:
|
||||
ExtendShooter();
|
||||
void Initialize() override;
|
||||
void End() override;
|
||||
};
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include "SetPivotSetpoint.h"
|
||||
|
||||
LowGoal::LowGoal() {
|
||||
AddSequential(new SetPivotSetpoint(Pivot::kLowGoal));
|
||||
AddSequential(new SetCollectionSpeed(Collector::kReverse));
|
||||
AddSequential(new ExtendShooter());
|
||||
AddSequential(new SetPivotSetpoint(Pivot::kLowGoal));
|
||||
AddSequential(new SetCollectionSpeed(Collector::kReverse));
|
||||
AddSequential(new ExtendShooter());
|
||||
}
|
||||
|
||||
@@ -14,6 +14,6 @@
|
||||
* it.
|
||||
*/
|
||||
class LowGoal : public frc::CommandGroup {
|
||||
public:
|
||||
LowGoal();
|
||||
public:
|
||||
LowGoal();
|
||||
};
|
||||
|
||||
@@ -9,16 +9,10 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
OpenClaw::OpenClaw() {
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
OpenClaw::OpenClaw() { Requires(&Robot::collector); }
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void OpenClaw::Initialize() {
|
||||
Robot::collector.Open();
|
||||
}
|
||||
void OpenClaw::Initialize() { Robot::collector.Open(); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool OpenClaw::IsFinished() {
|
||||
return Robot::collector.IsOpen();
|
||||
}
|
||||
bool OpenClaw::IsFinished() { return Robot::collector.IsOpen(); }
|
||||
|
||||
@@ -13,8 +13,8 @@
|
||||
* Opens the claw
|
||||
*/
|
||||
class OpenClaw : public frc::Command {
|
||||
public:
|
||||
OpenClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
OpenClaw();
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
};
|
||||
|
||||
@@ -10,11 +10,9 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetCollectionSpeed::SetCollectionSpeed(double speed) {
|
||||
Requires(&Robot::collector);
|
||||
m_speed = speed;
|
||||
Requires(&Robot::collector);
|
||||
m_speed = speed;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetCollectionSpeed::Initialize() {
|
||||
Robot::collector.SetSpeed(m_speed);
|
||||
}
|
||||
void SetCollectionSpeed::Initialize() { Robot::collector.SetSpeed(m_speed); }
|
||||
|
||||
@@ -15,10 +15,10 @@
|
||||
* the spinners may still be adjusting their speed.
|
||||
*/
|
||||
class SetCollectionSpeed : public frc::InstantCommand {
|
||||
public:
|
||||
explicit SetCollectionSpeed(double speed);
|
||||
void Initialize() override;
|
||||
public:
|
||||
explicit SetCollectionSpeed(double speed);
|
||||
void Initialize() override;
|
||||
|
||||
private:
|
||||
double m_speed;
|
||||
private:
|
||||
double m_speed;
|
||||
};
|
||||
|
||||
@@ -10,17 +10,15 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetPivotSetpoint::SetPivotSetpoint(double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::pivot);
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::pivot);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetPivotSetpoint::Initialize() {
|
||||
Robot::pivot.Enable();
|
||||
Robot::pivot.SetSetpoint(m_setpoint);
|
||||
Robot::pivot.Enable();
|
||||
Robot::pivot.SetSetpoint(m_setpoint);
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool SetPivotSetpoint::IsFinished() {
|
||||
return Robot::pivot.OnTarget();
|
||||
}
|
||||
bool SetPivotSetpoint::IsFinished() { return Robot::pivot.OnTarget(); }
|
||||
|
||||
@@ -15,11 +15,11 @@
|
||||
* Other commands using the pivot should make sure they disable PID!
|
||||
*/
|
||||
class SetPivotSetpoint : public frc::Command {
|
||||
public:
|
||||
explicit SetPivotSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
explicit SetPivotSetpoint(double setpoint);
|
||||
void Initialize() override;
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double m_setpoint;
|
||||
private:
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -14,8 +14,8 @@
|
||||
#include "WaitForPressure.h"
|
||||
|
||||
Shoot::Shoot() {
|
||||
AddSequential(new WaitForPressure());
|
||||
AddSequential(new SetCollectionSpeed(Collector::kStop));
|
||||
AddSequential(new OpenClaw());
|
||||
AddSequential(new ExtendShooter());
|
||||
AddSequential(new WaitForPressure());
|
||||
AddSequential(new SetCollectionSpeed(Collector::kStop));
|
||||
AddSequential(new OpenClaw());
|
||||
AddSequential(new ExtendShooter());
|
||||
}
|
||||
|
||||
@@ -13,6 +13,6 @@
|
||||
* Shoot the ball at the current angle.
|
||||
*/
|
||||
class Shoot : public frc::CommandGroup {
|
||||
public:
|
||||
Shoot();
|
||||
public:
|
||||
Shoot();
|
||||
};
|
||||
|
||||
@@ -9,11 +9,7 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
WaitForBall::WaitForBall() {
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
WaitForBall::WaitForBall() { Requires(&Robot::collector); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool WaitForBall::IsFinished() {
|
||||
return Robot::collector.HasBall();
|
||||
}
|
||||
bool WaitForBall::IsFinished() { return Robot::collector.HasBall(); }
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
* condition.
|
||||
*/
|
||||
class WaitForBall : public frc::Command {
|
||||
public:
|
||||
WaitForBall();
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
WaitForBall();
|
||||
bool IsFinished() override;
|
||||
};
|
||||
|
||||
@@ -9,11 +9,7 @@
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
WaitForPressure::WaitForPressure() {
|
||||
Requires(&Robot::pneumatics);
|
||||
}
|
||||
WaitForPressure::WaitForPressure() { Requires(&Robot::pneumatics); }
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool WaitForPressure::IsFinished() {
|
||||
return Robot::pneumatics.IsPressurized();
|
||||
}
|
||||
bool WaitForPressure::IsFinished() { return Robot::pneumatics.IsPressurized(); }
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
* and is intended to be used in command groups to wait for this condition.
|
||||
*/
|
||||
class WaitForPressure : public frc::Command {
|
||||
public:
|
||||
WaitForPressure();
|
||||
bool IsFinished() override;
|
||||
public:
|
||||
WaitForPressure();
|
||||
bool IsFinished() override;
|
||||
};
|
||||
|
||||
@@ -18,25 +18,23 @@
|
||||
#include "Subsystems/Pivot.h"
|
||||
|
||||
OI::OI() {
|
||||
m_r1.WhenPressed(new LowGoal());
|
||||
m_r2.WhenPressed(new Collect());
|
||||
m_r1.WhenPressed(new LowGoal());
|
||||
m_r2.WhenPressed(new Collect());
|
||||
|
||||
m_l1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
|
||||
m_l2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
|
||||
m_l1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
|
||||
m_l2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
|
||||
|
||||
m_sticks.WhenActive(new Shoot());
|
||||
m_sticks.WhenActive(new Shoot());
|
||||
|
||||
// SmartDashboard Buttons
|
||||
frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
|
||||
frc::SmartDashboard::PutData("Drive Backward", new DriveForward(-2.25));
|
||||
frc::SmartDashboard::PutData("Start Rollers",
|
||||
new SetCollectionSpeed(Collector::kForward));
|
||||
frc::SmartDashboard::PutData("Stop Rollers",
|
||||
new SetCollectionSpeed(Collector::kStop));
|
||||
frc::SmartDashboard::PutData("Reverse Rollers",
|
||||
new SetCollectionSpeed(Collector::kReverse));
|
||||
// SmartDashboard Buttons
|
||||
frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
|
||||
frc::SmartDashboard::PutData("Drive Backward", new DriveForward(-2.25));
|
||||
frc::SmartDashboard::PutData("Start Rollers",
|
||||
new SetCollectionSpeed(Collector::kForward));
|
||||
frc::SmartDashboard::PutData("Stop Rollers",
|
||||
new SetCollectionSpeed(Collector::kStop));
|
||||
frc::SmartDashboard::PutData("Reverse Rollers",
|
||||
new SetCollectionSpeed(Collector::kReverse));
|
||||
}
|
||||
|
||||
frc::Joystick& OI::GetJoystick() {
|
||||
return m_joystick;
|
||||
}
|
||||
frc::Joystick& OI::GetJoystick() { return m_joystick; }
|
||||
|
||||
@@ -13,17 +13,17 @@
|
||||
#include "Triggers/DoubleButton.h"
|
||||
|
||||
class OI {
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick& GetJoystick();
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick& GetJoystick();
|
||||
|
||||
private:
|
||||
frc::Joystick m_joystick{0};
|
||||
private:
|
||||
frc::Joystick m_joystick{0};
|
||||
|
||||
frc::JoystickButton m_l1{&m_joystick, 11};
|
||||
frc::JoystickButton m_l2{&m_joystick, 9};
|
||||
frc::JoystickButton m_r1{&m_joystick, 12};
|
||||
frc::JoystickButton m_r2{&m_joystick, 10};
|
||||
frc::JoystickButton m_l1{&m_joystick, 11};
|
||||
frc::JoystickButton m_l2{&m_joystick, 9};
|
||||
frc::JoystickButton m_r1{&m_joystick, 12};
|
||||
frc::JoystickButton m_r2{&m_joystick, 10};
|
||||
|
||||
DoubleButton m_sticks{&m_joystick, 2, 3};
|
||||
DoubleButton m_sticks{&m_joystick, 2, 3};
|
||||
};
|
||||
|
||||
@@ -21,67 +21,63 @@ Pneumatics Robot::pneumatics;
|
||||
OI Robot::oi;
|
||||
|
||||
void Robot::RobotInit() {
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(&drivetrain);
|
||||
frc::SmartDashboard::PutData(&pivot);
|
||||
frc::SmartDashboard::PutData(&collector);
|
||||
frc::SmartDashboard::PutData(&shooter);
|
||||
frc::SmartDashboard::PutData(&pneumatics);
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(&drivetrain);
|
||||
frc::SmartDashboard::PutData(&pivot);
|
||||
frc::SmartDashboard::PutData(&collector);
|
||||
frc::SmartDashboard::PutData(&shooter);
|
||||
frc::SmartDashboard::PutData(&pneumatics);
|
||||
|
||||
// instantiate the command used for the autonomous period
|
||||
m_autoChooser.AddDefault("Drive and Shoot", &m_driveAndShootAuto);
|
||||
m_autoChooser.AddObject("Drive Forward", &m_driveForwardAuto);
|
||||
frc::SmartDashboard::PutData("Auto Mode", &m_autoChooser);
|
||||
// instantiate the command used for the autonomous period
|
||||
m_autoChooser.AddDefault("Drive and Shoot", &m_driveAndShootAuto);
|
||||
m_autoChooser.AddObject("Drive Forward", &m_driveForwardAuto);
|
||||
frc::SmartDashboard::PutData("Auto Mode", &m_autoChooser);
|
||||
|
||||
pneumatics.Start(); // Pressurize the pneumatics.
|
||||
pneumatics.Start(); // Pressurize the pneumatics.
|
||||
}
|
||||
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand = m_autoChooser.GetSelected();
|
||||
m_autonomousCommand->Start();
|
||||
m_autonomousCommand = m_autoChooser.GetSelected();
|
||||
m_autonomousCommand->Start();
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
Log();
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
Log();
|
||||
}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != nullptr) {
|
||||
m_autonomousCommand->Cancel();
|
||||
}
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != nullptr) {
|
||||
m_autonomousCommand->Cancel();
|
||||
}
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
}
|
||||
|
||||
void Robot::TeleopPeriodic() {
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
Log();
|
||||
frc::Scheduler::GetInstance()->Run();
|
||||
Log();
|
||||
}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
void Robot::DisabledInit() {
|
||||
shooter.Unlatch();
|
||||
}
|
||||
void Robot::DisabledInit() { shooter.Unlatch(); }
|
||||
|
||||
void Robot::DisabledPeriodic() {
|
||||
Log();
|
||||
}
|
||||
void Robot::DisabledPeriodic() { Log(); }
|
||||
|
||||
/**
|
||||
* Log interesting values to the SmartDashboard.
|
||||
*/
|
||||
void Robot::Log() {
|
||||
Robot::pneumatics.WritePressure();
|
||||
frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot.GetAngle());
|
||||
frc::SmartDashboard::PutNumber("Left Distance",
|
||||
drivetrain.GetLeftEncoder().GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Right Distance",
|
||||
drivetrain.GetRightEncoder().GetDistance());
|
||||
Robot::pneumatics.WritePressure();
|
||||
frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot.GetAngle());
|
||||
frc::SmartDashboard::PutNumber("Left Distance",
|
||||
drivetrain.GetLeftEncoder().GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Right Distance",
|
||||
drivetrain.GetRightEncoder().GetDistance());
|
||||
}
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -21,28 +21,28 @@
|
||||
#include "Subsystems/Shooter.h"
|
||||
|
||||
class Robot : public IterativeRobot {
|
||||
public:
|
||||
static DriveTrain drivetrain;
|
||||
static Pivot pivot;
|
||||
static Collector collector;
|
||||
static Shooter shooter;
|
||||
static Pneumatics pneumatics;
|
||||
static OI oi;
|
||||
public:
|
||||
static DriveTrain drivetrain;
|
||||
static Pivot pivot;
|
||||
static Collector collector;
|
||||
static Shooter shooter;
|
||||
static Pneumatics pneumatics;
|
||||
static OI oi;
|
||||
|
||||
private:
|
||||
frc::Command* m_autonomousCommand = nullptr;
|
||||
DriveAndShootAutonomous m_driveAndShootAuto;
|
||||
DriveForward m_driveForwardAuto;
|
||||
SendableChooser<frc::Command*> m_autoChooser;
|
||||
private:
|
||||
frc::Command* m_autonomousCommand = nullptr;
|
||||
DriveAndShootAutonomous m_driveAndShootAuto;
|
||||
DriveForward m_driveForwardAuto;
|
||||
SendableChooser<frc::Command*> m_autoChooser;
|
||||
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
|
||||
void Log();
|
||||
void Log();
|
||||
};
|
||||
|
||||
@@ -9,37 +9,28 @@
|
||||
|
||||
#include <LiveWindow/LiveWindow.h>
|
||||
|
||||
Collector::Collector()
|
||||
: frc::Subsystem("Collector") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Roller Motor", m_rollerMotor);
|
||||
AddChild("Ball Detector", m_ballDetector);
|
||||
AddChild("Claw Open Detector", m_openDetector);
|
||||
AddChild("Piston", m_piston);
|
||||
Collector::Collector() : frc::Subsystem("Collector") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Roller Motor", m_rollerMotor);
|
||||
AddChild("Ball Detector", m_ballDetector);
|
||||
AddChild("Claw Open Detector", m_openDetector);
|
||||
AddChild("Piston", m_piston);
|
||||
}
|
||||
|
||||
bool Collector::HasBall() {
|
||||
return m_ballDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
return m_ballDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
}
|
||||
|
||||
void Collector::SetSpeed(double speed) {
|
||||
m_rollerMotor.Set(-speed);
|
||||
}
|
||||
void Collector::SetSpeed(double speed) { m_rollerMotor.Set(-speed); }
|
||||
|
||||
void Collector::Stop() {
|
||||
m_rollerMotor.Set(0);
|
||||
}
|
||||
void Collector::Stop() { m_rollerMotor.Set(0); }
|
||||
|
||||
bool Collector::IsOpen() {
|
||||
return m_openDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
return m_openDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
}
|
||||
|
||||
void Collector::Open() {
|
||||
m_piston.Set(true);
|
||||
}
|
||||
void Collector::Open() { m_piston.Set(true); }
|
||||
|
||||
void Collector::Close() {
|
||||
m_piston.Set(false);
|
||||
}
|
||||
void Collector::Close() { m_piston.Set(false); }
|
||||
|
||||
void Collector::InitDefaultCommand() {}
|
||||
|
||||
@@ -19,58 +19,58 @@
|
||||
* check if the piston is open.
|
||||
*/
|
||||
class Collector : public frc::Subsystem {
|
||||
public:
|
||||
// Constants for some useful speeds
|
||||
static constexpr double kForward = 1;
|
||||
static constexpr double kStop = 0;
|
||||
static constexpr double kReverse = -1;
|
||||
public:
|
||||
// Constants for some useful speeds
|
||||
static constexpr double kForward = 1;
|
||||
static constexpr double kStop = 0;
|
||||
static constexpr double kReverse = -1;
|
||||
|
||||
Collector();
|
||||
Collector();
|
||||
|
||||
/**
|
||||
* NOTE: The current simulation model uses the the lower part of the
|
||||
* claw
|
||||
* since the limit switch wasn't exported. At some point, this will be
|
||||
* updated.
|
||||
*
|
||||
* @return Whether or not the robot has the ball.
|
||||
*/
|
||||
bool HasBall();
|
||||
/**
|
||||
* NOTE: The current simulation model uses the the lower part of the
|
||||
* claw
|
||||
* since the limit switch wasn't exported. At some point, this will be
|
||||
* updated.
|
||||
*
|
||||
* @return Whether or not the robot has the ball.
|
||||
*/
|
||||
bool HasBall();
|
||||
|
||||
/**
|
||||
* @param speed The speed to spin the rollers.
|
||||
*/
|
||||
void SetSpeed(double speed);
|
||||
/**
|
||||
* @param speed The speed to spin the rollers.
|
||||
*/
|
||||
void SetSpeed(double speed);
|
||||
|
||||
/**
|
||||
* Stop the rollers from spinning
|
||||
*/
|
||||
void Stop();
|
||||
/**
|
||||
* Stop the rollers from spinning
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* @return Whether or not the claw is open.
|
||||
*/
|
||||
bool IsOpen();
|
||||
/**
|
||||
* @return Whether or not the claw is open.
|
||||
*/
|
||||
bool IsOpen();
|
||||
|
||||
/**
|
||||
* Open the claw up. (For shooting)
|
||||
*/
|
||||
void Open();
|
||||
/**
|
||||
* Open the claw up. (For shooting)
|
||||
*/
|
||||
void Open();
|
||||
|
||||
/**
|
||||
* Close the claw. (For collecting and driving)
|
||||
*/
|
||||
void Close();
|
||||
/**
|
||||
* Close the claw. (For collecting and driving)
|
||||
*/
|
||||
void Close();
|
||||
|
||||
/**
|
||||
* No default command.
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
/**
|
||||
* No default command.
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Spark m_rollerMotor{6};
|
||||
frc::DigitalInput m_ballDetector{10};
|
||||
frc::Solenoid m_piston{1};
|
||||
frc::DigitalInput m_openDetector{6};
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Spark m_rollerMotor{6};
|
||||
frc::DigitalInput m_ballDetector{10};
|
||||
frc::Solenoid m_piston{1};
|
||||
frc::DigitalInput m_openDetector{6};
|
||||
};
|
||||
|
||||
@@ -13,67 +13,58 @@
|
||||
|
||||
#include "../Commands/DriveWithJoystick.h"
|
||||
|
||||
DriveTrain::DriveTrain()
|
||||
: frc::Subsystem("DriveTrain") {
|
||||
// AddChild("Front Left CIM", m_frontLeftCIM);
|
||||
// AddChild("Front Right CIM", m_frontRightCIM);
|
||||
// AddChild("Back Left CIM", m_backLeftCIM);
|
||||
// AddChild("Back Right CIM", m_backRightCIM);
|
||||
DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
|
||||
// AddChild("Front Left CIM", m_frontLeftCIM);
|
||||
// AddChild("Front Right CIM", m_frontRightCIM);
|
||||
// AddChild("Back Left CIM", m_backLeftCIM);
|
||||
// AddChild("Back Right CIM", m_backRightCIM);
|
||||
|
||||
// Configure the DifferentialDrive to reflect the fact that all our
|
||||
// motors are wired backwards and our drivers sensitivity preferences.
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_robotDrive.SetMaxOutput(1.0);
|
||||
m_leftCIMs.SetInverted(true);
|
||||
m_rightCIMs.SetInverted(true);
|
||||
// Configure the DifferentialDrive to reflect the fact that all our
|
||||
// motors are wired backwards and our drivers sensitivity preferences.
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_robotDrive.SetMaxOutput(1.0);
|
||||
m_leftCIMs.SetInverted(true);
|
||||
m_rightCIMs.SetInverted(true);
|
||||
|
||||
// Configure encoders
|
||||
m_rightEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
m_leftEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
// Configure encoders
|
||||
m_rightEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
m_leftEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
|
||||
#ifndef SIMULATION
|
||||
// Converts to feet
|
||||
m_rightEncoder.SetDistancePerPulse(0.0785398);
|
||||
m_leftEncoder.SetDistancePerPulse(0.0785398);
|
||||
// Converts to feet
|
||||
m_rightEncoder.SetDistancePerPulse(0.0785398);
|
||||
m_leftEncoder.SetDistancePerPulse(0.0785398);
|
||||
#else
|
||||
// Convert to feet 4in diameter wheels with 360 tick simulated encoders
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
|
||||
// Convert to feet 4in diameter wheels with 360 tick simulated encoders
|
||||
m_rightEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
|
||||
(360.0 * 12.0 /*in/ft*/));
|
||||
m_leftEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
|
||||
(360.0 * 12.0 /*in/ft*/));
|
||||
#endif
|
||||
|
||||
AddChild("Right Encoder", m_rightEncoder);
|
||||
AddChild("Left Encoder", m_leftEncoder);
|
||||
AddChild("Right Encoder", m_rightEncoder);
|
||||
AddChild("Left Encoder", m_leftEncoder);
|
||||
|
||||
// Configure gyro
|
||||
#ifndef SIMULATION
|
||||
m_gyro.SetSensitivity(0.007); // TODO: Handle more gracefully?
|
||||
m_gyro.SetSensitivity(0.007); // TODO: Handle more gracefully?
|
||||
#endif
|
||||
AddChild("Gyro", m_gyro);
|
||||
AddChild("Gyro", m_gyro);
|
||||
}
|
||||
|
||||
void DriveTrain::InitDefaultCommand() {
|
||||
SetDefaultCommand(new DriveWithJoystick());
|
||||
SetDefaultCommand(new DriveWithJoystick());
|
||||
}
|
||||
|
||||
void DriveTrain::TankDrive(double leftAxis, double rightAxis) {
|
||||
m_robotDrive.TankDrive(leftAxis, rightAxis);
|
||||
m_robotDrive.TankDrive(leftAxis, rightAxis);
|
||||
}
|
||||
|
||||
void DriveTrain::Stop() {
|
||||
m_robotDrive.TankDrive(0.0, 0.0);
|
||||
}
|
||||
void DriveTrain::Stop() { m_robotDrive.TankDrive(0.0, 0.0); }
|
||||
|
||||
Encoder& DriveTrain::GetLeftEncoder() {
|
||||
return m_leftEncoder;
|
||||
}
|
||||
Encoder& DriveTrain::GetLeftEncoder() { return m_leftEncoder; }
|
||||
|
||||
Encoder& DriveTrain::GetRightEncoder() {
|
||||
return m_rightEncoder;
|
||||
}
|
||||
Encoder& DriveTrain::GetRightEncoder() { return m_rightEncoder; }
|
||||
|
||||
double DriveTrain::GetAngle() {
|
||||
return m_gyro.GetAngle();
|
||||
}
|
||||
double DriveTrain::GetAngle() { return m_gyro.GetAngle(); }
|
||||
|
||||
@@ -23,57 +23,57 @@ class Joystick;
|
||||
* information about it's speed and position.
|
||||
*/
|
||||
class DriveTrain : public frc::Subsystem {
|
||||
public:
|
||||
DriveTrain();
|
||||
public:
|
||||
DriveTrain();
|
||||
|
||||
/**
|
||||
* When other commands aren't using the drivetrain, allow tank drive
|
||||
* with
|
||||
* the joystick.
|
||||
*/
|
||||
void InitDefaultCommand();
|
||||
/**
|
||||
* When other commands aren't using the drivetrain, allow tank drive
|
||||
* with
|
||||
* the joystick.
|
||||
*/
|
||||
void InitDefaultCommand();
|
||||
|
||||
/**
|
||||
* @param leftAxis Left sides value
|
||||
* @param rightAxis Right sides value
|
||||
*/
|
||||
void TankDrive(double leftAxis, double rightAxis);
|
||||
/**
|
||||
* @param leftAxis Left sides value
|
||||
* @param rightAxis Right sides value
|
||||
*/
|
||||
void TankDrive(double leftAxis, double rightAxis);
|
||||
|
||||
/**
|
||||
* Stop the drivetrain from moving.
|
||||
*/
|
||||
void Stop();
|
||||
/**
|
||||
* Stop the drivetrain from moving.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* @return The encoder getting the distance and speed of left side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
Encoder& GetLeftEncoder();
|
||||
/**
|
||||
* @return The encoder getting the distance and speed of left side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
Encoder& GetLeftEncoder();
|
||||
|
||||
/**
|
||||
* @return The encoder getting the distance and speed of right side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
Encoder& GetRightEncoder();
|
||||
/**
|
||||
* @return The encoder getting the distance and speed of right side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
Encoder& GetRightEncoder();
|
||||
|
||||
/**
|
||||
* @return The current angle of the drivetrain.
|
||||
*/
|
||||
double GetAngle();
|
||||
/**
|
||||
* @return The current angle of the drivetrain.
|
||||
*/
|
||||
double GetAngle();
|
||||
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Spark m_frontLeftCIM{1};
|
||||
frc::Spark m_rearLeftCIM{2};
|
||||
frc::SpeedControllerGroup m_leftCIMs{m_frontLeftCIM, m_rearLeftCIM};
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Spark m_frontLeftCIM{1};
|
||||
frc::Spark m_rearLeftCIM{2};
|
||||
frc::SpeedControllerGroup m_leftCIMs{m_frontLeftCIM, m_rearLeftCIM};
|
||||
|
||||
frc::Spark m_frontRightCIM{3};
|
||||
frc::Spark m_rearRightCIM{4};
|
||||
frc::SpeedControllerGroup m_rightCIMs{m_frontRightCIM, m_rearRightCIM};
|
||||
frc::Spark m_frontRightCIM{3};
|
||||
frc::Spark m_rearRightCIM{4};
|
||||
frc::SpeedControllerGroup m_rightCIMs{m_frontRightCIM, m_rearRightCIM};
|
||||
|
||||
frc::DifferentialDrive m_robotDrive{m_leftCIMs, m_rightCIMs};
|
||||
frc::DifferentialDrive m_robotDrive{m_leftCIMs, m_rightCIMs};
|
||||
|
||||
frc::Encoder m_rightEncoder{1, 2, true, Encoder::k4X};
|
||||
frc::Encoder m_leftEncoder{3, 4, false, Encoder::k4X};
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
frc::Encoder m_rightEncoder{1, 2, true, Encoder::k4X};
|
||||
frc::Encoder m_leftEncoder{3, 4, false, Encoder::k4X};
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
};
|
||||
|
||||
@@ -7,43 +7,36 @@
|
||||
|
||||
#include "Pivot.h"
|
||||
|
||||
Pivot::Pivot()
|
||||
: frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) {
|
||||
SetAbsoluteTolerance(0.005);
|
||||
GetPIDController()->SetContinuous(false);
|
||||
Pivot::Pivot() : frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) {
|
||||
SetAbsoluteTolerance(0.005);
|
||||
GetPIDController()->SetContinuous(false);
|
||||
#ifdef SIMULATION
|
||||
// PID is different in simulation.
|
||||
GetPIDController()->SetPID(0.5, 0.001, 2);
|
||||
SetAbsoluteTolerance(5);
|
||||
// PID is different in simulation.
|
||||
GetPIDController()->SetPID(0.5, 0.001, 2);
|
||||
SetAbsoluteTolerance(5);
|
||||
#endif
|
||||
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Upper Limit Switch", m_upperLimitSwitch);
|
||||
AddChild("Lower Limit Switch", m_lowerLimitSwitch);
|
||||
AddChild("Pot", m_pot);
|
||||
AddChild("Motor", m_motor);
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Upper Limit Switch", m_upperLimitSwitch);
|
||||
AddChild("Lower Limit Switch", m_lowerLimitSwitch);
|
||||
AddChild("Pot", m_pot);
|
||||
AddChild("Motor", m_motor);
|
||||
}
|
||||
|
||||
void InitDefaultCommand() {}
|
||||
|
||||
double Pivot::ReturnPIDInput() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
double Pivot::ReturnPIDInput() { return m_pot.Get(); }
|
||||
|
||||
void Pivot::UsePIDOutput(double output) {
|
||||
m_motor.PIDWrite(output);
|
||||
}
|
||||
void Pivot::UsePIDOutput(double output) { m_motor.PIDWrite(output); }
|
||||
|
||||
bool Pivot::IsAtUpperLimit() {
|
||||
return m_upperLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
return m_upperLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
}
|
||||
|
||||
bool Pivot::IsAtLowerLimit() {
|
||||
return m_lowerLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
return m_lowerLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
}
|
||||
|
||||
double Pivot::GetAngle() {
|
||||
return m_pot.Get();
|
||||
}
|
||||
double Pivot::GetAngle() { return m_pot.Get(); }
|
||||
|
||||
@@ -17,58 +17,58 @@
|
||||
* of angle of the pivot and claw.
|
||||
*/
|
||||
class Pivot : public frc::PIDSubsystem {
|
||||
public:
|
||||
// Constants for some useful angles
|
||||
static constexpr double kCollect = 105;
|
||||
static constexpr double kLowGoal = 90;
|
||||
static constexpr double kShoot = 45;
|
||||
static constexpr double kShootNear = 30;
|
||||
public:
|
||||
// Constants for some useful angles
|
||||
static constexpr double kCollect = 105;
|
||||
static constexpr double kLowGoal = 90;
|
||||
static constexpr double kShoot = 45;
|
||||
static constexpr double kShootNear = 30;
|
||||
|
||||
Pivot();
|
||||
Pivot();
|
||||
|
||||
/**
|
||||
* No default command, if PID is enabled, the current setpoint will be
|
||||
* maintained.
|
||||
*/
|
||||
void InitDefaultCommand() override {}
|
||||
/**
|
||||
* No default command, if PID is enabled, the current setpoint will be
|
||||
* maintained.
|
||||
*/
|
||||
void InitDefaultCommand() override {}
|
||||
|
||||
/**
|
||||
* @return The angle read in by the potentiometer
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
/**
|
||||
* @return The angle read in by the potentiometer
|
||||
*/
|
||||
double ReturnPIDInput() override;
|
||||
|
||||
/**
|
||||
* Set the motor speed based off of the PID output
|
||||
*/
|
||||
void UsePIDOutput(double output) override;
|
||||
/**
|
||||
* Set the motor speed based off of the PID output
|
||||
*/
|
||||
void UsePIDOutput(double output) override;
|
||||
|
||||
/**
|
||||
* @return If the pivot is at its upper limit.
|
||||
*/
|
||||
bool IsAtUpperLimit();
|
||||
/**
|
||||
* @return If the pivot is at its upper limit.
|
||||
*/
|
||||
bool IsAtUpperLimit();
|
||||
|
||||
/**
|
||||
* @return If the pivot is at its lower limit.
|
||||
*/
|
||||
bool IsAtLowerLimit();
|
||||
/**
|
||||
* @return If the pivot is at its lower limit.
|
||||
*/
|
||||
bool IsAtLowerLimit();
|
||||
|
||||
/**
|
||||
* @return The current angle of the pivot.
|
||||
*/
|
||||
double GetAngle();
|
||||
/**
|
||||
* @return The current angle of the pivot.
|
||||
*/
|
||||
double GetAngle();
|
||||
|
||||
private:
|
||||
// Subsystem devices
|
||||
private:
|
||||
// Subsystem devices
|
||||
|
||||
// Sensors for measuring the position of the pivot
|
||||
frc::DigitalInput m_upperLimitSwitch{13};
|
||||
frc::DigitalInput m_lowerLimitSwitch{12};
|
||||
// Sensors for measuring the position of the pivot
|
||||
frc::DigitalInput m_upperLimitSwitch{13};
|
||||
frc::DigitalInput m_lowerLimitSwitch{12};
|
||||
|
||||
/* 0 degrees is vertical facing up.
|
||||
* Angle increases the more forward the pivot goes.
|
||||
*/
|
||||
frc::AnalogPotentiometer m_pot{1};
|
||||
/* 0 degrees is vertical facing up.
|
||||
* Angle increases the more forward the pivot goes.
|
||||
*/
|
||||
frc::AnalogPotentiometer m_pot{1};
|
||||
|
||||
// Motor to move the pivot
|
||||
frc::Spark m_motor{5};
|
||||
// Motor to move the pivot
|
||||
frc::Spark m_motor{5};
|
||||
};
|
||||
|
||||
@@ -9,9 +9,8 @@
|
||||
|
||||
#include <SmartDashboard/SmartDashboard.h>
|
||||
|
||||
Pneumatics::Pneumatics()
|
||||
: frc::Subsystem("Pneumatics") {
|
||||
AddChild("Pressure Sensor", m_pressureSensor);
|
||||
Pneumatics::Pneumatics() : frc::Subsystem("Pneumatics") {
|
||||
AddChild("Pressure Sensor", m_pressureSensor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -25,7 +24,7 @@ void Pneumatics::InitDefaultCommand() {}
|
||||
*/
|
||||
void Pneumatics::Start() {
|
||||
#ifndef SIMULATION
|
||||
m_compressor.Start();
|
||||
m_compressor.Start();
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -34,9 +33,9 @@ void Pneumatics::Start() {
|
||||
*/
|
||||
bool Pneumatics::IsPressurized() {
|
||||
#ifndef SIMULATION
|
||||
return kMaxPressure <= m_pressureSensor.GetVoltage();
|
||||
return kMaxPressure <= m_pressureSensor.GetVoltage();
|
||||
#else
|
||||
return true; // NOTE: Simulation always has full pressure
|
||||
return true; // NOTE: Simulation always has full pressure
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -44,6 +43,5 @@ bool Pneumatics::IsPressurized() {
|
||||
* Puts the pressure on the SmartDashboard.
|
||||
*/
|
||||
void Pneumatics::WritePressure() {
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Pressure", m_pressureSensor.GetVoltage());
|
||||
frc::SmartDashboard::PutNumber("Pressure", m_pressureSensor.GetVoltage());
|
||||
}
|
||||
|
||||
@@ -18,36 +18,36 @@
|
||||
* sensors.
|
||||
*/
|
||||
class Pneumatics : public frc::Subsystem {
|
||||
public:
|
||||
Pneumatics();
|
||||
public:
|
||||
Pneumatics();
|
||||
|
||||
/**
|
||||
* No default command
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
/**
|
||||
* No default command
|
||||
*/
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* Start the compressor going. The compressor automatically starts and
|
||||
* stops as it goes above and below maximum pressure.
|
||||
*/
|
||||
void Start();
|
||||
/**
|
||||
* Start the compressor going. The compressor automatically starts and
|
||||
* stops as it goes above and below maximum pressure.
|
||||
*/
|
||||
void Start();
|
||||
|
||||
/**
|
||||
* @return Whether or not the system is fully pressurized.
|
||||
*/
|
||||
bool IsPressurized();
|
||||
/**
|
||||
* @return Whether or not the system is fully pressurized.
|
||||
*/
|
||||
bool IsPressurized();
|
||||
|
||||
/**
|
||||
* Puts the pressure on the SmartDashboard.
|
||||
*/
|
||||
void WritePressure();
|
||||
/**
|
||||
* Puts the pressure on the SmartDashboard.
|
||||
*/
|
||||
void WritePressure();
|
||||
|
||||
private:
|
||||
frc::AnalogInput m_pressureSensor{3};
|
||||
private:
|
||||
frc::AnalogInput m_pressureSensor{3};
|
||||
|
||||
#ifndef SIMULATION
|
||||
frc::Compressor m_compressor{1}; // TODO: (1, 14, 1, 8);
|
||||
frc::Compressor m_compressor{1}; // TODO: (1, 14, 1, 8);
|
||||
#endif
|
||||
|
||||
static constexpr double kMaxPressure = 2.55;
|
||||
static constexpr double kMaxPressure = 2.55;
|
||||
};
|
||||
|
||||
@@ -7,79 +7,54 @@
|
||||
|
||||
#include "Shooter.h"
|
||||
|
||||
Shooter::Shooter()
|
||||
: Subsystem("Shooter") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Hot Goal Sensor", m_hotGoalSensor);
|
||||
AddChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
|
||||
AddChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
|
||||
AddChild("Latch Piston", m_latchPiston);
|
||||
Shooter::Shooter() : Subsystem("Shooter") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
AddChild("Hot Goal Sensor", m_hotGoalSensor);
|
||||
AddChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
|
||||
AddChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
|
||||
AddChild("Latch Piston", m_latchPiston);
|
||||
}
|
||||
|
||||
void Shooter::InitDefaultCommand() {
|
||||
// Set the default command for a subsystem here.
|
||||
// SetDefaultCommand(new MySpecialCommand());
|
||||
// Set the default command for a subsystem here.
|
||||
// SetDefaultCommand(new MySpecialCommand());
|
||||
}
|
||||
|
||||
void Shooter::ExtendBoth() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
|
||||
void Shooter::RetractBoth() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
|
||||
void Shooter::Extend1() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
void Shooter::Extend1() { m_piston1.Set(frc::DoubleSolenoid::kForward); }
|
||||
|
||||
void Shooter::Retract1() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
void Shooter::Retract1() { m_piston1.Set(frc::DoubleSolenoid::kReverse); }
|
||||
|
||||
void Shooter::Extend2() {
|
||||
m_piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
void Shooter::Extend2() { m_piston2.Set(frc::DoubleSolenoid::kReverse); }
|
||||
|
||||
void Shooter::Retract2() {
|
||||
m_piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
void Shooter::Retract2() { m_piston2.Set(frc::DoubleSolenoid::kForward); }
|
||||
|
||||
void Shooter::Off1() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
void Shooter::Off1() { m_piston1.Set(frc::DoubleSolenoid::kOff); }
|
||||
|
||||
void Shooter::Off2() {
|
||||
m_piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
void Shooter::Off2() { m_piston2.Set(frc::DoubleSolenoid::kOff); }
|
||||
|
||||
void Shooter::Unlatch() {
|
||||
m_latchPiston.Set(true);
|
||||
}
|
||||
void Shooter::Unlatch() { m_latchPiston.Set(true); }
|
||||
|
||||
void Shooter::Latch() {
|
||||
m_latchPiston.Set(false);
|
||||
}
|
||||
void Shooter::Latch() { m_latchPiston.Set(false); }
|
||||
|
||||
void Shooter::ToggleLatchPosition() {
|
||||
m_latchPiston.Set(!m_latchPiston.Get());
|
||||
}
|
||||
void Shooter::ToggleLatchPosition() { m_latchPiston.Set(!m_latchPiston.Get()); }
|
||||
|
||||
bool Shooter::Piston1IsExtended() {
|
||||
return !m_piston1ReedSwitchFront.Get();
|
||||
}
|
||||
bool Shooter::Piston1IsExtended() { return !m_piston1ReedSwitchFront.Get(); }
|
||||
|
||||
bool Shooter::Piston1IsRetracted() {
|
||||
return !m_piston1ReedSwitchBack.Get();
|
||||
}
|
||||
bool Shooter::Piston1IsRetracted() { return !m_piston1ReedSwitchBack.Get(); }
|
||||
|
||||
void Shooter::OffBoth() {
|
||||
m_piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
|
||||
bool Shooter::GoalIsHot() {
|
||||
return m_hotGoalSensor.Get();
|
||||
}
|
||||
bool Shooter::GoalIsHot() { return m_hotGoalSensor.Get(); }
|
||||
|
||||
@@ -23,105 +23,105 @@
|
||||
* and ignores the latch.
|
||||
*/
|
||||
class Shooter : public frc::Subsystem {
|
||||
public:
|
||||
Shooter();
|
||||
void InitDefaultCommand() override;
|
||||
public:
|
||||
Shooter();
|
||||
void InitDefaultCommand() override;
|
||||
|
||||
/**
|
||||
* Extend both solenoids to shoot.
|
||||
*/
|
||||
void ExtendBoth();
|
||||
/**
|
||||
* Extend both solenoids to shoot.
|
||||
*/
|
||||
void ExtendBoth();
|
||||
|
||||
/**
|
||||
* Retract both solenoids to prepare to shoot.
|
||||
*/
|
||||
void RetractBoth();
|
||||
/**
|
||||
* Retract both solenoids to prepare to shoot.
|
||||
*/
|
||||
void RetractBoth();
|
||||
|
||||
/**
|
||||
* Extend solenoid 1 to shoot.
|
||||
*/
|
||||
void Extend1();
|
||||
/**
|
||||
* Extend solenoid 1 to shoot.
|
||||
*/
|
||||
void Extend1();
|
||||
|
||||
/**
|
||||
* Retract solenoid 1 to prepare to shoot.
|
||||
*/
|
||||
void Retract1();
|
||||
/**
|
||||
* Retract solenoid 1 to prepare to shoot.
|
||||
*/
|
||||
void Retract1();
|
||||
|
||||
/**
|
||||
* Extend solenoid 2 to shoot.
|
||||
*/
|
||||
void Extend2();
|
||||
/**
|
||||
* Extend solenoid 2 to shoot.
|
||||
*/
|
||||
void Extend2();
|
||||
|
||||
/**
|
||||
* Retract solenoid 2 to prepare to shoot.
|
||||
*/
|
||||
void Retract2();
|
||||
/**
|
||||
* Retract solenoid 2 to prepare to shoot.
|
||||
*/
|
||||
void Retract2();
|
||||
|
||||
/**
|
||||
* Turns off the piston1 double solenoid. This won't actuate anything
|
||||
* because double solenoids preserve their state when turned off. This
|
||||
* should be called in order to reduce the amount of time that the coils
|
||||
* are
|
||||
* powered.
|
||||
*/
|
||||
void Off1();
|
||||
/**
|
||||
* Turns off the piston1 double solenoid. This won't actuate anything
|
||||
* because double solenoids preserve their state when turned off. This
|
||||
* should be called in order to reduce the amount of time that the coils
|
||||
* are
|
||||
* powered.
|
||||
*/
|
||||
void Off1();
|
||||
|
||||
/**
|
||||
* Turns off the piston1 double solenoid. This won't actuate anything
|
||||
* because double solenoids preserve their state when turned off. This
|
||||
* should be called in order to reduce the amount of time that the coils
|
||||
* are
|
||||
* powered.
|
||||
*/
|
||||
void Off2();
|
||||
/**
|
||||
* Turns off the piston1 double solenoid. This won't actuate anything
|
||||
* because double solenoids preserve their state when turned off. This
|
||||
* should be called in order to reduce the amount of time that the coils
|
||||
* are
|
||||
* powered.
|
||||
*/
|
||||
void Off2();
|
||||
|
||||
/**
|
||||
* Release the latch so that we can shoot
|
||||
*/
|
||||
void Unlatch();
|
||||
/**
|
||||
* Release the latch so that we can shoot
|
||||
*/
|
||||
void Unlatch();
|
||||
|
||||
/**
|
||||
* Latch so that pressure can build up and we aren't limited by air
|
||||
* flow.
|
||||
*/
|
||||
void Latch();
|
||||
/**
|
||||
* Latch so that pressure can build up and we aren't limited by air
|
||||
* flow.
|
||||
*/
|
||||
void Latch();
|
||||
|
||||
/**
|
||||
* Toggles the latch postions
|
||||
*/
|
||||
void ToggleLatchPosition();
|
||||
/**
|
||||
* Toggles the latch postions
|
||||
*/
|
||||
void ToggleLatchPosition();
|
||||
|
||||
/**
|
||||
* @return Whether or not piston 1 is fully extended.
|
||||
*/
|
||||
bool Piston1IsExtended();
|
||||
/**
|
||||
* @return Whether or not piston 1 is fully extended.
|
||||
*/
|
||||
bool Piston1IsExtended();
|
||||
|
||||
/**
|
||||
* @return Whether or not piston 1 is fully retracted.
|
||||
*/
|
||||
bool Piston1IsRetracted();
|
||||
/**
|
||||
* @return Whether or not piston 1 is fully retracted.
|
||||
*/
|
||||
bool Piston1IsRetracted();
|
||||
|
||||
/**
|
||||
* Turns off all double solenoids. Double solenoids hold their position
|
||||
* when
|
||||
* they are turned off. We should turn them off whenever possible to
|
||||
* extend
|
||||
* the life of the coils
|
||||
*/
|
||||
void OffBoth();
|
||||
/**
|
||||
* Turns off all double solenoids. Double solenoids hold their position
|
||||
* when
|
||||
* they are turned off. We should turn them off whenever possible to
|
||||
* extend
|
||||
* the life of the coils
|
||||
*/
|
||||
void OffBoth();
|
||||
|
||||
/**
|
||||
* @return Whether or not the goal is hot as read by the banner sensor
|
||||
*/
|
||||
bool GoalIsHot();
|
||||
/**
|
||||
* @return Whether or not the goal is hot as read by the banner sensor
|
||||
*/
|
||||
bool GoalIsHot();
|
||||
|
||||
private:
|
||||
// Devices
|
||||
frc::DoubleSolenoid m_piston1{3, 4};
|
||||
frc::DoubleSolenoid m_piston2{5, 6};
|
||||
frc::Solenoid m_latchPiston{1, 2};
|
||||
frc::DigitalInput m_piston1ReedSwitchFront{9};
|
||||
frc::DigitalInput m_piston1ReedSwitchBack{11};
|
||||
frc::DigitalInput m_hotGoalSensor{
|
||||
7}; // NOTE: Currently ignored in simulation
|
||||
private:
|
||||
// Devices
|
||||
frc::DoubleSolenoid m_piston1{3, 4};
|
||||
frc::DoubleSolenoid m_piston2{5, 6};
|
||||
frc::Solenoid m_latchPiston{1, 2};
|
||||
frc::DigitalInput m_piston1ReedSwitchFront{9};
|
||||
frc::DigitalInput m_piston1ReedSwitchBack{11};
|
||||
frc::DigitalInput m_hotGoalSensor{
|
||||
7}; // NOTE: Currently ignored in simulation
|
||||
};
|
||||
|
||||
@@ -11,10 +11,10 @@
|
||||
|
||||
DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2)
|
||||
: m_joy(*joy) {
|
||||
m_button1 = button1;
|
||||
m_button2 = button2;
|
||||
m_button1 = button1;
|
||||
m_button2 = button2;
|
||||
}
|
||||
|
||||
bool DoubleButton::Get() {
|
||||
return m_joy.GetRawButton(m_button1) && m_joy.GetRawButton(m_button2);
|
||||
return m_joy.GetRawButton(m_button1) && m_joy.GetRawButton(m_button2);
|
||||
}
|
||||
|
||||
@@ -14,13 +14,13 @@ class Joystick;
|
||||
} // namespace frc
|
||||
|
||||
class DoubleButton : public frc::Trigger {
|
||||
public:
|
||||
DoubleButton(frc::Joystick* joy, int button1, int button2);
|
||||
public:
|
||||
DoubleButton(frc::Joystick* joy, int button1, int button2);
|
||||
|
||||
bool Get();
|
||||
bool Get();
|
||||
|
||||
private:
|
||||
frc::Joystick& m_joy;
|
||||
int m_button1;
|
||||
int m_button2;
|
||||
private:
|
||||
frc::Joystick& m_joy;
|
||||
int m_button1;
|
||||
int m_button2;
|
||||
};
|
||||
|
||||
@@ -19,62 +19,56 @@
|
||||
* mechanism.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override { m_pidController.SetInputRange(0, 5); }
|
||||
public:
|
||||
void RobotInit() override { m_pidController.SetInputRange(0, 5); }
|
||||
|
||||
void TeleopInit() override { m_pidController.Enable(); }
|
||||
void TeleopInit() override { m_pidController.Enable(); }
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// when the button is pressed once, the selected elevator
|
||||
// setpoint
|
||||
// is incremented
|
||||
bool currentButtonValue = m_joystick.GetTrigger();
|
||||
if (currentButtonValue && !m_previousButtonValue) {
|
||||
// index of the elevator setpoint wraps around.
|
||||
m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
|
||||
}
|
||||
m_previousButtonValue = currentButtonValue;
|
||||
void TeleopPeriodic() override {
|
||||
// When the button is pressed once, the selected elevator setpoint is
|
||||
// incremented.
|
||||
bool currentButtonValue = m_joystick.GetTrigger();
|
||||
if (currentButtonValue && !m_previousButtonValue) {
|
||||
// Index of the elevator setpoint wraps around
|
||||
m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
|
||||
}
|
||||
m_previousButtonValue = currentButtonValue;
|
||||
|
||||
m_pidController.SetSetpoint(kSetPoints[m_index]);
|
||||
}
|
||||
m_pidController.SetSetpoint(kSetPoints[m_index]);
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr int kPotChannel = 1;
|
||||
static constexpr int kMotorChannel = 7;
|
||||
static constexpr int kJoystickChannel = 0;
|
||||
private:
|
||||
static constexpr int kPotChannel = 1;
|
||||
static constexpr int kMotorChannel = 7;
|
||||
static constexpr int kJoystickChannel = 0;
|
||||
|
||||
// Bottom, middle, and top elevator setpoints
|
||||
static constexpr std::array<double, 3> kSetPoints = {{1.0, 2.6, 4.3}};
|
||||
// Bottom, middle, and top elevator setpoints
|
||||
static constexpr std::array<double, 3> kSetPoints = {{1.0, 2.6, 4.3}};
|
||||
|
||||
/* proportional, integral, and derivative speed constants; motor
|
||||
* inverted
|
||||
* DANGER: when tuning PID constants, high/inappropriate values for
|
||||
* pGain,
|
||||
* iGain, and dGain may cause dangerous, uncontrollable, or
|
||||
* undesired behavior!
|
||||
*
|
||||
* These may need to be positive for a non-inverted motor
|
||||
*/
|
||||
static constexpr double kP = -5.0;
|
||||
static constexpr double kI = -0.02;
|
||||
static constexpr double kD = -2.0;
|
||||
/* Proportional, integral, and derivative speed constants; motor inverted.
|
||||
*
|
||||
* DANGER: When tuning PID constants, high/inappropriate values for pGain,
|
||||
* iGain, and dGain may cause dangerous, uncontrollable, or undesired
|
||||
* behavior!
|
||||
*
|
||||
* These may need to be positive for a non-inverted motor.
|
||||
*/
|
||||
static constexpr double kP = -5.0;
|
||||
static constexpr double kI = -0.02;
|
||||
static constexpr double kD = -2.0;
|
||||
|
||||
int m_index = 0;
|
||||
bool m_previousButtonValue = false;
|
||||
int m_index = 0;
|
||||
bool m_previousButtonValue = false;
|
||||
|
||||
frc::AnalogInput m_potentiometer{kPotChannel};
|
||||
frc::Joystick m_joystick{kJoystickChannel};
|
||||
frc::Spark m_elevatorMotor{kMotorChannel};
|
||||
frc::AnalogInput m_potentiometer{kPotChannel};
|
||||
frc::Joystick m_joystick{kJoystickChannel};
|
||||
frc::Spark m_elevatorMotor{kMotorChannel};
|
||||
|
||||
/* potentiometer (AnalogInput) and elevatorMotor (Victor) can be used as
|
||||
* a
|
||||
* PIDSource and PIDOutput respectively. The PIDController takes
|
||||
* pointers
|
||||
* to the PIDSource and PIDOutput, so you must use &potentiometer and
|
||||
* &elevatorMotor to get their pointers.
|
||||
*/
|
||||
frc::PIDController m_pidController{
|
||||
kP, kI, kD, &m_potentiometer, &m_elevatorMotor};
|
||||
/* Potentiometer (AnalogInput) and elevatorMotor (Victor) can be used as a
|
||||
* PIDSource and PIDOutput respectively.
|
||||
*/
|
||||
frc::PIDController m_pidController{kP, kI, kD, m_potentiometer,
|
||||
m_elevatorMotor};
|
||||
};
|
||||
|
||||
constexpr std::array<double, 3> Robot::kSetPoints;
|
||||
|
||||
@@ -11,21 +11,20 @@
|
||||
|
||||
/**
|
||||
* Uses the CameraServer class to automatically capture video from a USB webcam
|
||||
* and send it to the FRC dashboard without doing any vision processing. This
|
||||
* is the easiest way to get camera images to the dashboard. Just add this to
|
||||
* the
|
||||
* and send it to the FRC dashboard without doing any vision processing. This is
|
||||
* the easiest way to get camera images to the dashboard. Just add this to the
|
||||
* RobotInit() method in your program.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
#if defined(__linux__)
|
||||
CameraServer::GetInstance()->StartAutomaticCapture();
|
||||
CameraServer::GetInstance()->StartAutomaticCapture();
|
||||
#else
|
||||
wpi::errs() << "Vision only available on Linux.\n";
|
||||
wpi::errs().flush();
|
||||
wpi::errs() << "Vision only available on Linux.\n";
|
||||
wpi::errs().flush();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -11,45 +11,48 @@
|
||||
|
||||
/**
|
||||
* This is a sample program which uses joystick buttons to control a relay.
|
||||
*
|
||||
* A Relay (generally a spike) has two outputs, each of which can be at either
|
||||
* 0V or 12V and so can be used for actions such as turning a motor off,
|
||||
* full forwards, or full reverse, and is generally used on the compressor.
|
||||
* 0V or 12V and so can be used for actions such as turning a motor off, full
|
||||
* forwards, or full reverse, and is generally used on the compressor.
|
||||
*
|
||||
* This program uses two buttons on a joystick and each button corresponds to
|
||||
* one output; pressing the button sets the output to 12V and releasing sets
|
||||
* it to 0V.
|
||||
* one output; pressing the button sets the output to 12V and releasing sets it
|
||||
* to 0V.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* Retrieve the button values. GetRawButton will return
|
||||
* true if the button is pressed and false if not.
|
||||
*/
|
||||
bool forward = m_stick.GetRawButton(kRelayForwardButton);
|
||||
bool reverse = m_stick.GetRawButton(kRelayReverseButton);
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* Retrieve the button values. GetRawButton() will return true if the button
|
||||
* is pressed and false if not.
|
||||
*/
|
||||
bool forward = m_stick.GetRawButton(kRelayForwardButton);
|
||||
bool reverse = m_stick.GetRawButton(kRelayReverseButton);
|
||||
|
||||
/* Depending on the button values, we want to use one of
|
||||
* kOn, kOff, kForward, or kReverse.
|
||||
* kOn sets both outputs to 12V, kOff sets both to 0V,
|
||||
* kForward sets forward to 12V and reverse to 0V, and
|
||||
* kReverse sets reverse to 12V and forward to 0V.
|
||||
*/
|
||||
if (forward && reverse) {
|
||||
m_relay.Set(Relay::kOn);
|
||||
} else if (forward) {
|
||||
m_relay.Set(Relay::kForward);
|
||||
} else if (reverse) {
|
||||
m_relay.Set(Relay::kReverse);
|
||||
} else {
|
||||
m_relay.Set(Relay::kOff);
|
||||
}
|
||||
}
|
||||
/* Depending on the button values, we want to use one of kOn, kOff,
|
||||
* kForward, or kReverse.
|
||||
*
|
||||
* kOn sets both outputs to 12V, kOff sets both to 0V.
|
||||
* kForward sets forward to 12V and reverse to 0V.
|
||||
* kReverse sets reverse to 12V and forward to 0V.
|
||||
*/
|
||||
if (forward && reverse) {
|
||||
m_relay.Set(Relay::kOn);
|
||||
} else if (forward) {
|
||||
m_relay.Set(Relay::kForward);
|
||||
} else if (reverse) {
|
||||
m_relay.Set(Relay::kReverse);
|
||||
} else {
|
||||
m_relay.Set(Relay::kOff);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
frc::Relay m_relay{0};
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
frc::Relay m_relay{0};
|
||||
|
||||
static constexpr int kRelayForwardButton = 1;
|
||||
static constexpr int kRelayReverseButton = 2;
|
||||
static constexpr int kRelayForwardButton = 1;
|
||||
static constexpr int kRelayReverseButton = 2;
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -12,56 +12,59 @@
|
||||
|
||||
/**
|
||||
* This is a sample program showing the use of the solenoid classes during
|
||||
* operator control.
|
||||
* Three buttons from a joystick will be used to control two solenoids:
|
||||
* One button to control the position of a single solenoid and the other
|
||||
* two buttons to control a double solenoid.
|
||||
* operator control.
|
||||
*
|
||||
* Three buttons from a joystick will be used to control two solenoids: One
|
||||
* button to control the position of a single solenoid and the other two buttons
|
||||
* to control a double solenoid.
|
||||
*
|
||||
* Single solenoids can either be on or off, such that the air diverted through
|
||||
* them goes through either one channel or the other.
|
||||
* them goes through either one channel or the other.
|
||||
*
|
||||
* Double solenoids have three states: Off, Forward, and Reverse. Forward and
|
||||
* Reverse divert the air through the two channels and correspond to the
|
||||
* on and off of a single solenoid, but a double solenoid can also be "off",
|
||||
* where both channels are diverted to exhaust such that there is no pressure
|
||||
* in either channel.
|
||||
* Reverse divert the air through the two channels and correspond to the on and
|
||||
* off of a single solenoid, but a double solenoid can also be "off", where both
|
||||
* channels are diverted to exhaust such that there is no pressure in either
|
||||
* channel.
|
||||
*
|
||||
* Additionally, double solenoids take up two channels on your PCM whereas
|
||||
* single solenoids only take a single channel.
|
||||
* single solenoids only take a single channel.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* The output of GetRawButton is true/false depending on whether
|
||||
* the button is pressed; Set takes a boolean for for whether to
|
||||
* use the default (false) channel or the other (true).
|
||||
*/
|
||||
m_solenoid.Set(m_stick.GetRawButton(kSolenoidButton));
|
||||
public:
|
||||
void TeleopPeriodic() override {
|
||||
/* The output of GetRawButton is true/false depending on whether the button
|
||||
* is pressed; Set takes a boolean for for whether to use the default
|
||||
* (false) channel or the other (true).
|
||||
*/
|
||||
m_solenoid.Set(m_stick.GetRawButton(kSolenoidButton));
|
||||
|
||||
/* In order to set the double solenoid, we will say that if
|
||||
* neither
|
||||
* button is pressed, it is off, if just one button is pressed,
|
||||
* set the solenoid to correspond to that button, and if both
|
||||
* are pressed, set the solenoid to Forwards.
|
||||
*/
|
||||
if (m_stick.GetRawButton(kDoubleSolenoidForward)) {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kForward);
|
||||
} else if (m_stick.GetRawButton(kDoubleSolenoidReverse)) {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kReverse);
|
||||
} else {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
}
|
||||
/* In order to set the double solenoid, we will say that if neither button
|
||||
* is pressed, it is off, if just one button is pressed, set the solenoid to
|
||||
* correspond to that button, and if both are pressed, set the solenoid to
|
||||
* Forwards.
|
||||
*/
|
||||
if (m_stick.GetRawButton(kDoubleSolenoidForward)) {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kForward);
|
||||
} else if (m_stick.GetRawButton(kDoubleSolenoidReverse)) {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kReverse);
|
||||
} else {
|
||||
m_doubleSolenoid.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
private:
|
||||
frc::Joystick m_stick{0};
|
||||
|
||||
// Solenoid corresponds to a single solenoid.
|
||||
frc::Solenoid m_solenoid{0};
|
||||
// Solenoid corresponds to a single solenoid.
|
||||
frc::Solenoid m_solenoid{0};
|
||||
|
||||
// DoubleSolenoid corresponds to a double solenoid.
|
||||
frc::DoubleSolenoid m_doubleSolenoid{1, 2};
|
||||
// DoubleSolenoid corresponds to a double solenoid.
|
||||
frc::DoubleSolenoid m_doubleSolenoid{1, 2};
|
||||
|
||||
static constexpr int kSolenoidButton = 1;
|
||||
static constexpr int kDoubleSolenoidForward = 2;
|
||||
static constexpr int kDoubleSolenoidReverse = 3;
|
||||
static constexpr int kSolenoidButton = 1;
|
||||
static constexpr int kDoubleSolenoidForward = 2;
|
||||
static constexpr int kDoubleSolenoidReverse = 3;
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -15,40 +15,39 @@
|
||||
* proportional control to maintain a set distance from an object.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
/**
|
||||
* Tells the robot to drive to a set distance (in inches) from an object
|
||||
* using proportional control.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
// sensor returns a value from 0-4095 that is scaled to inches
|
||||
double currentDistance =
|
||||
m_ultrasonic.GetValue() * kValueToInches;
|
||||
// convert distance error to a motor speed
|
||||
double currentSpeed = (kHoldDistance - currentDistance) * kP;
|
||||
// drive robot
|
||||
m_robotDrive.ArcadeDrive(currentSpeed, 0);
|
||||
}
|
||||
public:
|
||||
/**
|
||||
* Tells the robot to drive to a set distance (in inches) from an object using
|
||||
* proportional control.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
// Sensor returns a value from 0-4095 that is scaled to inches
|
||||
double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
|
||||
// Convert distance error to a motor speed
|
||||
double currentSpeed = (kHoldDistance - currentDistance) * kP;
|
||||
// Drive robot
|
||||
m_robotDrive.ArcadeDrive(currentSpeed, 0);
|
||||
}
|
||||
|
||||
private:
|
||||
// Distance in inches the robot wants to stay from an object
|
||||
static constexpr int kHoldDistance = 12;
|
||||
private:
|
||||
// Distance in inches the robot wants to stay from an object
|
||||
static constexpr int kHoldDistance = 12;
|
||||
|
||||
// Factor to convert sensor values to a distance in inches
|
||||
static constexpr double kValueToInches = 0.125;
|
||||
// Factor to convert sensor values to a distance in inches
|
||||
static constexpr double kValueToInches = 0.125;
|
||||
|
||||
// Proportional speed constant
|
||||
static constexpr double kP = 0.05;
|
||||
// Proportional speed constant
|
||||
static constexpr double kP = 0.05;
|
||||
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -17,70 +17,67 @@
|
||||
* proportional control to maintain a set distance from an object.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
/**
|
||||
* Drives the robot a set distance from an object using PID control and
|
||||
* the
|
||||
* ultrasonic sensor.
|
||||
*/
|
||||
void TeleopInit() override {
|
||||
// Set expected range to 0-24 inches; e.g. at 24 inches from
|
||||
// object go
|
||||
// full forward, at 0 inches from object go full backward.
|
||||
m_pidController.SetInputRange(0, 24 * kValueToInches);
|
||||
public:
|
||||
/**
|
||||
* Drives the robot a set distance from an object using PID control and the
|
||||
* ultrasonic sensor.
|
||||
*/
|
||||
void TeleopInit() override {
|
||||
// Set expected range to 0-24 inches; e.g. at 24 inches from object go full
|
||||
// forward, at 0 inches from object go full backward.
|
||||
m_pidController.SetInputRange(0, 24 * kValueToInches);
|
||||
|
||||
// Set setpoint of the pidController
|
||||
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
|
||||
// Set setpoint of the PID Controller
|
||||
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
|
||||
|
||||
// Begin PID control
|
||||
m_pidController.Enable();
|
||||
}
|
||||
// Begin PID control
|
||||
m_pidController.Enable();
|
||||
}
|
||||
|
||||
private:
|
||||
// Internal class to write to robot drive using a PIDOutput
|
||||
class MyPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
explicit MyPIDOutput(frc::DifferentialDrive& r)
|
||||
: m_rd(r) {
|
||||
m_rd.SetSafetyEnabled(false);
|
||||
}
|
||||
private:
|
||||
// Internal class to write to robot drive using a PIDOutput
|
||||
class MyPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
explicit MyPIDOutput(frc::DifferentialDrive& r) : m_rd(r) {
|
||||
m_rd.SetSafetyEnabled(false);
|
||||
}
|
||||
|
||||
void PIDWrite(double output) override {
|
||||
// Write to robot drive by reference
|
||||
m_rd.ArcadeDrive(output, 0);
|
||||
}
|
||||
void PIDWrite(double output) override {
|
||||
// Write to robot drive by reference
|
||||
m_rd.ArcadeDrive(output, 0);
|
||||
}
|
||||
|
||||
private:
|
||||
frc::DifferentialDrive& m_rd;
|
||||
};
|
||||
private:
|
||||
frc::DifferentialDrive& m_rd;
|
||||
};
|
||||
|
||||
// Distance in inches the robot wants to stay from an object
|
||||
static constexpr int kHoldDistance = 12;
|
||||
// Distance in inches the robot wants to stay from an object
|
||||
static constexpr int kHoldDistance = 12;
|
||||
|
||||
// Factor to convert sensor values to a distance in inches
|
||||
static constexpr double kValueToInches = 0.125;
|
||||
// Factor to convert sensor values to a distance in inches
|
||||
static constexpr double kValueToInches = 0.125;
|
||||
|
||||
// proportional speed constant
|
||||
static constexpr double kP = 7.0;
|
||||
// proportional speed constant
|
||||
static constexpr double kP = 7.0;
|
||||
|
||||
// integral speed constant
|
||||
static constexpr double kI = 0.018;
|
||||
// integral speed constant
|
||||
static constexpr double kI = 0.018;
|
||||
|
||||
// derivative speed constant
|
||||
static constexpr double kD = 1.5;
|
||||
// derivative speed constant
|
||||
static constexpr double kD = 1.5;
|
||||
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
static constexpr int kLeftMotorPort = 0;
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
MyPIDOutput m_pidOutput{m_robotDrive};
|
||||
|
||||
frc::PIDController m_pidController{kP, kI, kD, &m_ultrasonic,
|
||||
new MyPIDOutput(m_robotDrive)};
|
||||
frc::PIDController m_pidController{kP, kI, kD, m_ultrasonic, m_pidOutput};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
Reference in New Issue
Block a user