mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
Cleaned up C++ examples (#672)
This commit is contained in:
committed by
Peter Johnson
parent
6401aa1fde
commit
45d48d6b5a
@@ -12,12 +12,15 @@
|
||||
* Runs the motors with arcade steering.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
frc::RobotDrive myRobot{0, 1};
|
||||
frc::Joystick stick{0};
|
||||
frc::Spark m_left{0};
|
||||
frc::Spark m_right{1};
|
||||
frc::RobotDrive m_robotDrive{m_left, m_right};
|
||||
frc::Joystick m_stick{0};
|
||||
|
||||
public:
|
||||
void TeleopPeriodic() {
|
||||
myRobot.ArcadeDrive(stick); // drive with arcade style
|
||||
// drive with arcade style
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -26,26 +26,6 @@
|
||||
* autonomous mode.
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
/**
|
||||
* 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};
|
||||
|
||||
public:
|
||||
Robot() {
|
||||
/* Defines the number of samples to average when determining the
|
||||
@@ -88,6 +68,27 @@ public:
|
||||
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};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -7,8 +7,6 @@
|
||||
|
||||
#include "Autonomous.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "CloseClaw.h"
|
||||
#include "DriveStraight.h"
|
||||
#include "Pickup.h"
|
||||
|
||||
@@ -11,17 +11,17 @@
|
||||
|
||||
CloseClaw::CloseClaw()
|
||||
: frc::Command("CloseClaw") {
|
||||
Requires(Robot::claw.get());
|
||||
Requires(&Robot::claw);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void CloseClaw::Initialize() {
|
||||
Robot::claw->Close();
|
||||
Robot::claw.Close();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool CloseClaw::IsFinished() {
|
||||
return Robot::claw->IsGripping();
|
||||
return Robot::claw.IsGripping();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
@@ -30,6 +30,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
|
||||
}
|
||||
|
||||
@@ -7,42 +7,38 @@
|
||||
|
||||
#include "DriveStraight.h"
|
||||
|
||||
#include <PIDController.h>
|
||||
|
||||
#include "../Robot.h"
|
||||
|
||||
DriveStraight::DriveStraight(double distance) {
|
||||
Requires(Robot::drivetrain.get());
|
||||
pid = new frc::PIDController(4, 0, 0, new DriveStraightPIDSource(),
|
||||
new DriveStraightPIDOutput());
|
||||
pid->SetAbsoluteTolerance(0.01);
|
||||
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();
|
||||
pid->Reset();
|
||||
pid->Enable();
|
||||
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 pid->OnTarget();
|
||||
return m_pid.OnTarget();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveStraight::End() {
|
||||
// Stop PID and the wheels
|
||||
pid->Disable();
|
||||
Robot::drivetrain->Drive(0, 0);
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
|
||||
double DriveStraightPIDSource::PIDGet() {
|
||||
return Robot::drivetrain->GetDistance();
|
||||
double DriveStraight::DriveStraightPIDSource::PIDGet() {
|
||||
return Robot::drivetrain.GetDistance();
|
||||
}
|
||||
|
||||
void DriveStraightPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain->Drive(d, d);
|
||||
void DriveStraight::DriveStraightPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
}
|
||||
|
||||
@@ -8,13 +8,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <Commands/Command.h>
|
||||
#include <PIDController.h>
|
||||
#include <PIDOutput.h>
|
||||
#include <PIDSource.h>
|
||||
|
||||
namespace frc {
|
||||
class PIDController;
|
||||
} // namespace frc
|
||||
|
||||
/**
|
||||
* Drive the given distance straight (negative values go backwards).
|
||||
* Uses a local PID controller to run a simple PID loop that is only
|
||||
@@ -28,18 +25,20 @@ public:
|
||||
bool IsFinished() override;
|
||||
void End() 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;
|
||||
};
|
||||
|
||||
private:
|
||||
frc::PIDController* pid;
|
||||
};
|
||||
|
||||
class DriveStraightPIDSource : public PIDSource {
|
||||
public:
|
||||
virtual ~DriveStraightPIDSource() = default;
|
||||
double PIDGet() override;
|
||||
};
|
||||
|
||||
class DriveStraightPIDOutput : public PIDOutput {
|
||||
public:
|
||||
virtual ~DriveStraightPIDOutput() = default;
|
||||
void PIDWrite(double d) override;
|
||||
DriveStraightPIDSource m_source;
|
||||
DriveStraightPIDOutput m_output;
|
||||
frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
|
||||
};
|
||||
|
||||
@@ -11,13 +11,13 @@
|
||||
|
||||
OpenClaw::OpenClaw()
|
||||
: frc::Command("OpenClaw") {
|
||||
Requires(Robot::claw.get());
|
||||
Requires(&Robot::claw);
|
||||
SetTimeout(1);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void OpenClaw::Initialize() {
|
||||
Robot::claw->Open();
|
||||
Robot::claw.Open();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
@@ -27,5 +27,5 @@ bool OpenClaw::IsFinished() {
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void OpenClaw::End() {
|
||||
Robot::claw->Stop();
|
||||
Robot::claw.Stop();
|
||||
}
|
||||
|
||||
@@ -12,37 +12,35 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetDistanceToBox::SetDistanceToBox(double distance) {
|
||||
Requires(Robot::drivetrain.get());
|
||||
pid = new frc::PIDController(-2, 0, 0, new SetDistanceToBoxPIDSource(),
|
||||
new SetDistanceToBoxPIDOutput());
|
||||
pid->SetAbsoluteTolerance(0.01);
|
||||
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();
|
||||
pid->Reset();
|
||||
pid->Enable();
|
||||
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 pid->OnTarget();
|
||||
return m_pid.OnTarget();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void SetDistanceToBox::End() {
|
||||
// Stop PID and the wheels
|
||||
pid->Disable();
|
||||
Robot::drivetrain->Drive(0, 0);
|
||||
m_pid.Disable();
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
|
||||
double SetDistanceToBoxPIDSource::PIDGet() {
|
||||
return Robot::drivetrain->GetDistanceToObstacle();
|
||||
double SetDistanceToBox::SetDistanceToBoxPIDSource::PIDGet() {
|
||||
return Robot::drivetrain.GetDistanceToObstacle();
|
||||
}
|
||||
|
||||
void SetDistanceToBoxPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain->Drive(d, d);
|
||||
void SetDistanceToBox::SetDistanceToBoxPIDOutput::PIDWrite(double d) {
|
||||
Robot::drivetrain.Drive(d, d);
|
||||
}
|
||||
|
||||
@@ -8,13 +8,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <Commands/Command.h>
|
||||
#include <PIDController.h>
|
||||
#include <PIDOutput.h>
|
||||
#include <PIDSource.h>
|
||||
|
||||
namespace frc {
|
||||
class PIDController;
|
||||
} // namespace frc
|
||||
|
||||
/**
|
||||
* Drive until the robot is the given distance away from the box. Uses a local
|
||||
* PID controller to run a simple PID loop that is only enabled while this
|
||||
@@ -28,18 +25,20 @@ public:
|
||||
bool IsFinished() override;
|
||||
void End() 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;
|
||||
};
|
||||
|
||||
private:
|
||||
frc::PIDController* pid;
|
||||
};
|
||||
|
||||
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;
|
||||
SetDistanceToBoxPIDSource m_source;
|
||||
SetDistanceToBoxPIDOutput m_output;
|
||||
frc::PIDController m_pid{-2, 0, 0, &m_source, &m_output};
|
||||
};
|
||||
|
||||
@@ -13,17 +13,17 @@
|
||||
|
||||
SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
|
||||
: frc::Command("SetElevatorSetpoint") {
|
||||
this->setpoint = setpoint;
|
||||
Requires(Robot::elevator.get());
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::elevator);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetElevatorSetpoint::Initialize() {
|
||||
Robot::elevator->SetSetpoint(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();
|
||||
return Robot::elevator.OnTarget();
|
||||
}
|
||||
|
||||
@@ -23,5 +23,5 @@ public:
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double setpoint;
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -11,17 +11,17 @@
|
||||
|
||||
SetWristSetpoint::SetWristSetpoint(double setpoint)
|
||||
: frc::Command("SetWristSetpoint") {
|
||||
this->setpoint = setpoint;
|
||||
Requires(Robot::wrist.get());
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::wrist);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetWristSetpoint::Initialize() {
|
||||
Robot::wrist->SetSetpoint(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();
|
||||
return Robot::wrist.OnTarget();
|
||||
}
|
||||
|
||||
@@ -21,5 +21,5 @@ public:
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double setpoint;
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -11,12 +11,13 @@
|
||||
|
||||
TankDriveWithJoystick::TankDriveWithJoystick()
|
||||
: frc::Command("TankDriveWithJoystick") {
|
||||
Requires(Robot::drivetrain.get());
|
||||
Requires(&Robot::drivetrain);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void TankDriveWithJoystick::Execute() {
|
||||
Robot::drivetrain->Drive(Robot::oi->GetJoystick());
|
||||
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()
|
||||
@@ -26,5 +27,5 @@ bool TankDriveWithJoystick::IsFinished() {
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void TankDriveWithJoystick::End() {
|
||||
Robot::drivetrain->Drive(0, 0);
|
||||
Robot::drivetrain.Drive(0, 0);
|
||||
}
|
||||
|
||||
@@ -22,17 +22,17 @@ OI::OI() {
|
||||
frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
|
||||
|
||||
// Connect the buttons to commands
|
||||
d_up.WhenPressed(new SetElevatorSetpoint(0.2));
|
||||
d_down.WhenPressed(new SetElevatorSetpoint(-0.2));
|
||||
d_right.WhenPressed(new CloseClaw());
|
||||
d_left.WhenPressed(new OpenClaw());
|
||||
m_dUp.WhenPressed(new SetElevatorSetpoint(0.2));
|
||||
m_dDown.WhenPressed(new SetElevatorSetpoint(-0.2));
|
||||
m_dRight.WhenPressed(new CloseClaw());
|
||||
m_dLeft.WhenPressed(new OpenClaw());
|
||||
|
||||
r1.WhenPressed(new PrepareToPickup());
|
||||
r2.WhenPressed(new Pickup());
|
||||
l1.WhenPressed(new Place());
|
||||
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 &joy;
|
||||
frc::Joystick& OI::GetJoystick() {
|
||||
return m_joy;
|
||||
}
|
||||
|
||||
@@ -13,18 +13,18 @@
|
||||
class OI {
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick* GetJoystick();
|
||||
frc::Joystick& GetJoystick();
|
||||
|
||||
private:
|
||||
frc::Joystick joy{0};
|
||||
frc::Joystick m_joy{0};
|
||||
|
||||
// Create some buttons
|
||||
frc::JoystickButton d_up{&joy, 5};
|
||||
frc::JoystickButton d_right{&joy, 6};
|
||||
frc::JoystickButton d_down{&joy, 7};
|
||||
frc::JoystickButton d_left{&joy, 8};
|
||||
frc::JoystickButton l2{&joy, 9};
|
||||
frc::JoystickButton r2{&joy, 10};
|
||||
frc::JoystickButton l1{&joy, 11};
|
||||
frc::JoystickButton r1{&joy, 12};
|
||||
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};
|
||||
};
|
||||
|
||||
@@ -9,22 +9,22 @@
|
||||
|
||||
#include <iostream>
|
||||
|
||||
std::shared_ptr<DriveTrain> Robot::drivetrain = std::make_shared<DriveTrain>();
|
||||
std::shared_ptr<Elevator> Robot::elevator = std::make_shared<Elevator>();
|
||||
std::shared_ptr<Wrist> Robot::wrist = std::make_shared<Wrist>();
|
||||
std::shared_ptr<Claw> Robot::claw = std::make_shared<Claw>();
|
||||
std::unique_ptr<OI> Robot::oi = std::make_unique<OI>();
|
||||
DriveTrain Robot::drivetrain;
|
||||
Elevator Robot::elevator;
|
||||
Wrist Robot::wrist;
|
||||
Claw Robot::claw;
|
||||
OI Robot::oi;
|
||||
|
||||
void Robot::RobotInit() {
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(drivetrain.get());
|
||||
frc::SmartDashboard::PutData(elevator.get());
|
||||
frc::SmartDashboard::PutData(wrist.get());
|
||||
frc::SmartDashboard::PutData(claw.get());
|
||||
frc::SmartDashboard::PutData(&drivetrain);
|
||||
frc::SmartDashboard::PutData(&elevator);
|
||||
frc::SmartDashboard::PutData(&wrist);
|
||||
frc::SmartDashboard::PutData(&claw);
|
||||
}
|
||||
|
||||
void Robot::AutonomousInit() {
|
||||
autonomousCommand.Start();
|
||||
m_autonomousCommand.Start();
|
||||
std::cout << "Starting Auto" << std::endl;
|
||||
}
|
||||
|
||||
@@ -37,7 +37,7 @@ void Robot::TeleopInit() {
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
autonomousCommand.Cancel();
|
||||
m_autonomousCommand.Cancel();
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
}
|
||||
|
||||
@@ -46,7 +46,7 @@ void Robot::TeleopPeriodic() {
|
||||
}
|
||||
|
||||
void Robot::TestPeriodic() {
|
||||
lw->Run();
|
||||
m_lw.Run();
|
||||
}
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -7,8 +7,6 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <Commands/Command.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <LiveWindow/LiveWindow.h>
|
||||
@@ -22,15 +20,15 @@
|
||||
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
static std::shared_ptr<DriveTrain> drivetrain;
|
||||
static std::shared_ptr<Elevator> elevator;
|
||||
static std::shared_ptr<Wrist> wrist;
|
||||
static std::shared_ptr<Claw> claw;
|
||||
static std::unique_ptr<OI> oi;
|
||||
static DriveTrain drivetrain;
|
||||
static Elevator elevator;
|
||||
static Wrist wrist;
|
||||
static Claw claw;
|
||||
static OI oi;
|
||||
|
||||
private:
|
||||
Autonomous autonomousCommand;
|
||||
frc::LiveWindow* lw = frc::LiveWindow::GetInstance();
|
||||
Autonomous m_autonomousCommand;
|
||||
frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
|
||||
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
|
||||
@@ -18,19 +18,19 @@ Claw::Claw()
|
||||
void Claw::InitDefaultCommand() {}
|
||||
|
||||
void Claw::Open() {
|
||||
motor.Set(-1);
|
||||
m_motor.Set(-1);
|
||||
}
|
||||
|
||||
void Claw::Close() {
|
||||
motor.Set(1);
|
||||
m_motor.Set(1);
|
||||
}
|
||||
|
||||
void Claw::Stop() {
|
||||
motor.Set(0);
|
||||
m_motor.Set(0);
|
||||
}
|
||||
|
||||
bool Claw::IsGripping() {
|
||||
return contact.Get();
|
||||
return m_contact.Get();
|
||||
}
|
||||
|
||||
void Claw::Log() {}
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
#include <Commands/Subsystem.h>
|
||||
#include <DigitalInput.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* The claw subsystem is a simple system with a motor for opening and closing.
|
||||
@@ -46,6 +46,6 @@ public:
|
||||
void Log();
|
||||
|
||||
private:
|
||||
frc::Victor motor{7};
|
||||
frc::DigitalInput contact{5};
|
||||
frc::Spark m_motor{7};
|
||||
frc::DigitalInput m_contact{5};
|
||||
};
|
||||
|
||||
@@ -20,36 +20,36 @@ DriveTrain::DriveTrain()
|
||||
// simulate 360 tick encoders. This if statement allows for the
|
||||
// real robot to handle this difference in devices.
|
||||
#ifndef SIMULATION
|
||||
leftEncoder.SetDistancePerPulse(0.042);
|
||||
rightEncoder.SetDistancePerPulse(0.042);
|
||||
m_leftEncoder.SetDistancePerPulse(0.042);
|
||||
m_rightEncoder.SetDistancePerPulse(0.042);
|
||||
#else
|
||||
// Circumference in ft = 4in/12(in/ft)*PI
|
||||
leftEncoder.SetDistancePerPulse(
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
|
||||
rightEncoder.SetDistancePerPulse(
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
static_cast<double>(4.0 / 12.0 * M_PI) / 360.0);
|
||||
#endif
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
|
||||
// "Front_Left Motor", &frontLeft);
|
||||
// "Front_Left Motor", &m_frontLeft);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
|
||||
// "Rear Left Motor", &rearLeft);
|
||||
// "Rear Left Motor", &m_rearLeft);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
|
||||
// "Front Right Motor", &frontRight);
|
||||
// "Front Right Motor", &m_frontRight);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Drive Train",
|
||||
// "Rear Right Motor", &rearRight);
|
||||
// "Rear Right Motor", &m_rearRight);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Left
|
||||
// Encoder",
|
||||
// &leftEncoder);
|
||||
// &m_leftEncoder);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Right
|
||||
// Encoder",
|
||||
// &rightEncoder);
|
||||
// &m_rightEncoder);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Drive Train",
|
||||
// "Rangefinder",
|
||||
// &rangefinder);
|
||||
// &m_rangefinder);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Gyro",
|
||||
// &gyro);
|
||||
// &m_gyro);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -65,37 +65,34 @@ void DriveTrain::InitDefaultCommand() {
|
||||
*/
|
||||
void DriveTrain::Log() {
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Left Distance", leftEncoder.GetDistance());
|
||||
"Left Distance", m_leftEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Right Distance", rightEncoder.GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Left Speed", leftEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Right Speed", rightEncoder.GetRate());
|
||||
frc::SmartDashboard::PutNumber("Gyro", gyro.GetAngle());
|
||||
"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) {
|
||||
drive.TankDrive(left, right);
|
||||
}
|
||||
|
||||
void DriveTrain::Drive(frc::Joystick* joy) {
|
||||
Drive(-joy->GetY(), -joy->GetRawAxis(4));
|
||||
m_robotDrive.TankDrive(left, right);
|
||||
}
|
||||
|
||||
double DriveTrain::GetHeading() {
|
||||
return gyro.GetAngle();
|
||||
return m_gyro.GetAngle();
|
||||
}
|
||||
|
||||
void DriveTrain::Reset() {
|
||||
gyro.Reset();
|
||||
leftEncoder.Reset();
|
||||
rightEncoder.Reset();
|
||||
m_gyro.Reset();
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double DriveTrain::GetDistance() {
|
||||
return (leftEncoder.GetDistance() + rightEncoder.GetDistance()) / 2;
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance())
|
||||
/ 2.0;
|
||||
}
|
||||
|
||||
double DriveTrain::GetDistanceToObstacle() {
|
||||
// Really meters in simulation since it's a rangefinder...
|
||||
return rangefinder.GetAverageVoltage();
|
||||
return m_rangefinder.GetAverageVoltage();
|
||||
}
|
||||
|
||||
@@ -10,9 +10,10 @@
|
||||
#include <AnalogGyro.h>
|
||||
#include <AnalogInput.h>
|
||||
#include <Commands/Subsystem.h>
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <Encoder.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Talon.h>
|
||||
#include <Spark.h>
|
||||
#include <SpeedControllerGroup.h>
|
||||
|
||||
namespace frc {
|
||||
class Joystick;
|
||||
@@ -45,11 +46,6 @@ public:
|
||||
*/
|
||||
void Drive(double left, double right);
|
||||
|
||||
/**
|
||||
* @param joy The ps3 style joystick to use to drive tank style.
|
||||
*/
|
||||
void Drive(frc::Joystick* joy);
|
||||
|
||||
/**
|
||||
* @return The robots heading in degrees.
|
||||
*/
|
||||
@@ -71,13 +67,18 @@ public:
|
||||
double GetDistanceToObstacle();
|
||||
|
||||
private:
|
||||
frc::Talon frontLeft{1};
|
||||
frc::Talon rearLeft{2};
|
||||
frc::Talon frontRight{3};
|
||||
frc::Talon rearRight{4};
|
||||
frc::RobotDrive drive{frontLeft, rearLeft, frontRight, rearRight};
|
||||
frc::Encoder leftEncoder{1, 2};
|
||||
frc::Encoder rightEncoder{3, 4};
|
||||
frc::AnalogInput rangefinder{6};
|
||||
frc::AnalogGyro gyro{1};
|
||||
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::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};
|
||||
};
|
||||
|
||||
@@ -19,8 +19,8 @@ Elevator::Elevator()
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Elevator", "Motor",
|
||||
// &motor);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Elevator", "Pot", &pot);
|
||||
// &m_motor);
|
||||
// frc::LiveWindow::GetInstance()->AddSensor("Elevator", "Pot", &m_pot);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("Elevator", "PID",
|
||||
// GetPIDController());
|
||||
}
|
||||
@@ -28,13 +28,13 @@ Elevator::Elevator()
|
||||
void Elevator::InitDefaultCommand() {}
|
||||
|
||||
void Elevator::Log() {
|
||||
// frc::SmartDashboard::PutData("Wrist Pot", &pot);
|
||||
// frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
|
||||
}
|
||||
|
||||
double Elevator::ReturnPIDInput() {
|
||||
return pot.Get();
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
void Elevator::UsePIDOutput(double d) {
|
||||
motor.Set(d);
|
||||
m_motor.Set(d);
|
||||
}
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
#include <AnalogPotentiometer.h>
|
||||
#include <Commands/PIDSubsystem.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
|
||||
@@ -42,14 +42,14 @@ public:
|
||||
void UsePIDOutput(double d);
|
||||
|
||||
private:
|
||||
frc::Victor motor{5};
|
||||
frc::Spark m_motor{5};
|
||||
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
#ifndef SIMULATION
|
||||
frc::AnalogPotentiometer pot{2, -2.0 / 5};
|
||||
frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
|
||||
#else
|
||||
frc::AnalogPotentiometer pot{2}; // Defaults to meters
|
||||
frc::AnalogPotentiometer m_pot{2}; // Defaults to meters
|
||||
#endif
|
||||
|
||||
static constexpr double kP_real = 4;
|
||||
|
||||
@@ -28,13 +28,13 @@ Wrist::Wrist()
|
||||
void Wrist::InitDefaultCommand() {}
|
||||
|
||||
void Wrist::Log() {
|
||||
// frc::SmartDashboard::PutData("Wrist Angle", &pot);
|
||||
// frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
|
||||
}
|
||||
|
||||
double Wrist::ReturnPIDInput() {
|
||||
return pot.Get();
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
void Wrist::UsePIDOutput(double d) {
|
||||
motor.Set(d);
|
||||
m_motor.Set(d);
|
||||
}
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
#include <AnalogPotentiometer.h>
|
||||
#include <Commands/PIDSubsystem.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* The wrist subsystem is like the elevator, but with a rotational joint instead
|
||||
@@ -40,14 +40,14 @@ public:
|
||||
void UsePIDOutput(double d) override;
|
||||
|
||||
private:
|
||||
frc::Victor motor{6};
|
||||
frc::Spark m_motor{6};
|
||||
|
||||
// Conversion value of potentiometer varies between the real world and
|
||||
// simulation
|
||||
#ifndef SIMULATION
|
||||
frc::AnalogPotentiometer pot{3, -270.0 / 5};
|
||||
frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
|
||||
#else
|
||||
frc::AnalogPotentiometer pot{3}; // Defaults to degrees
|
||||
frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
|
||||
#endif
|
||||
|
||||
static constexpr double kP_real = 1;
|
||||
|
||||
@@ -5,36 +5,33 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <Joystick.h>
|
||||
#include <LiveWindow/LiveWindow.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
#include <Timer.h>
|
||||
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
Robot() {
|
||||
myRobot.SetExpiration(0.1);
|
||||
timer.Start();
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
private:
|
||||
frc::RobotDrive myRobot{0, 1}; // Robot drive system
|
||||
frc::Joystick stick{0}; // Only joystick
|
||||
frc::LiveWindow* lw = frc::LiveWindow::GetInstance();
|
||||
frc::Timer timer;
|
||||
|
||||
void AutonomousInit() override {
|
||||
timer.Reset();
|
||||
timer.Start();
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
|
||||
void AutonomousPeriodic() override {
|
||||
// Drive for 2 seconds
|
||||
if (timer.Get() < 2.0) {
|
||||
myRobot.Drive(-0.5, 0.0); // Drive forwards half speed
|
||||
if (m_timer.Get() < 2.0) {
|
||||
// Drive forwards half speed
|
||||
m_robotDrive.ArcadeDrive(-0.5, 0.0);
|
||||
} else {
|
||||
myRobot.Drive(0.0, 0.0); // Stop robot
|
||||
// Stop robot
|
||||
m_robotDrive.ArcadeDrive(0.0, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -42,10 +39,20 @@ private:
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Drive with arcade style (use right stick)
|
||||
myRobot.ArcadeDrive(stick);
|
||||
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
|
||||
}
|
||||
|
||||
void TestPeriodic() override { lw->Run(); }
|
||||
void TestPeriodic() override { m_lw.Run(); }
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -8,9 +8,10 @@
|
||||
#include <cmath>
|
||||
|
||||
#include <AnalogGyro.h>
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <Joystick.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a gyro sensor to make a
|
||||
@@ -22,7 +23,7 @@
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -31,10 +32,10 @@ public:
|
||||
* angle.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
double turningValue = (kAngleSetpoint - gyro.GetAngle()) * kP;
|
||||
double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
|
||||
// Invert the direction of the turn if we are going backwards
|
||||
turningValue = std::copysign(turningValue, joystick.GetY());
|
||||
myRobot.Drive(joystick.GetY(), turningValue);
|
||||
turningValue = std::copysign(turningValue, m_joystick.GetY());
|
||||
m_robotDrive.ArcadeDrive(m_joystick.GetY(), turningValue);
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -50,9 +51,12 @@ private:
|
||||
static constexpr int kGyroPort = 0;
|
||||
static constexpr int kJoystickPort = 0;
|
||||
|
||||
frc::RobotDrive myRobot{kLeftMotorPort, kRightMotorPort};
|
||||
frc::AnalogGyro gyro{kGyroPort};
|
||||
frc::Joystick joystick{kJoystickPort};
|
||||
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};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -6,9 +6,10 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <AnalogGyro.h>
|
||||
#include <Drive/MecanumDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <Joystick.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* This is a sample program that uses mecanum drive with a gyro sensor to
|
||||
@@ -20,18 +21,19 @@ public:
|
||||
void RobotInit() override {
|
||||
// invert the left side motors
|
||||
// you may need to change or remove this to match your robot
|
||||
myRobot.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
|
||||
myRobot.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
|
||||
gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
|
||||
}
|
||||
|
||||
/**
|
||||
* Mecanum drive is used with the gyro angle as an input.
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
myRobot.MecanumDrive_Cartesian(joystick.GetX(), joystick.GetY(),
|
||||
joystick.GetZ(), gyro.GetAngle());
|
||||
m_robotDrive.DriveCartesian(m_joystick.GetX(),
|
||||
m_joystick.GetY(), m_joystick.GetZ(),
|
||||
m_gyro.GetAngle());
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -40,16 +42,21 @@ private:
|
||||
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
|
||||
|
||||
static constexpr int kFrontLeftMotorPort = 0;
|
||||
static constexpr int kFrontRightMotorPort = 1;
|
||||
static constexpr int kRearLeftMotorPort = 2;
|
||||
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::RobotDrive myRobot{kFrontLeftMotorPort, kFrontRightMotorPort,
|
||||
kRearLeftMotorPort, kRearRightMotorPort};
|
||||
frc::AnalogGyro gyro{kGyroPort};
|
||||
frc::Joystick joystick{kJoystickPort};
|
||||
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};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -5,9 +5,10 @@
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <Drive/MecanumDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <Joystick.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* This is a demo program showing how to use Mecanum control with the RobotDrive
|
||||
@@ -18,8 +19,8 @@ public:
|
||||
void RobotInit() {
|
||||
// Invert the left side motors
|
||||
// You may need to change or remove this to match your robot
|
||||
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
|
||||
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
|
||||
m_frontLeft.SetInverted(true);
|
||||
m_rearLeft.SetInverted(true);
|
||||
}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
@@ -27,22 +28,26 @@ public:
|
||||
* forward
|
||||
* movement, and Z axis for rotation.
|
||||
*/
|
||||
robotDrive.MecanumDrive_Cartesian(
|
||||
stick.GetX(), stick.GetY(), stick.GetZ());
|
||||
m_robotDrive.DriveCartesian(
|
||||
m_stick.GetX(), m_stick.GetY(), m_stick.GetZ());
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr int kFrontLeftChannel = 2;
|
||||
static constexpr int kRearLeftChannel = 3;
|
||||
static constexpr int kFrontRightChannel = 1;
|
||||
static constexpr int kRearRightChannel = 0;
|
||||
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;
|
||||
|
||||
frc::RobotDrive robotDrive{kFrontLeftChannel, kRearLeftChannel,
|
||||
kFrontRightChannel, kRearRightChannel};
|
||||
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 stick{kJoystickChannel};
|
||||
frc::Joystick m_stick{kJoystickChannel};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -15,5 +15,5 @@ CheckForHotGoal::CheckForHotGoal(double 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();
|
||||
}
|
||||
|
||||
@@ -10,10 +10,10 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
CloseClaw::CloseClaw() {
|
||||
Requires(Robot::collector.get());
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void CloseClaw::Initialize() {
|
||||
Robot::collector->Close();
|
||||
Robot::collector.Close();
|
||||
}
|
||||
|
||||
@@ -12,9 +12,9 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
void DriveForward::init(double dist, double maxSpeed) {
|
||||
Requires(Robot::drivetrain.get());
|
||||
distance = dist;
|
||||
driveForwardSpeed = maxSpeed;
|
||||
Requires(&Robot::drivetrain);
|
||||
m_distance = dist;
|
||||
m_driveForwardSpeed = maxSpeed;
|
||||
}
|
||||
|
||||
DriveForward::DriveForward() {
|
||||
@@ -31,29 +31,29 @@ DriveForward::DriveForward(double dist, double maxSpeed) {
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void DriveForward::Initialize() {
|
||||
Robot::drivetrain->GetRightEncoder()->Reset();
|
||||
Robot::drivetrain.GetRightEncoder().Reset();
|
||||
SetTimeout(2);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void DriveForward::Execute() {
|
||||
error = (distance
|
||||
- Robot::drivetrain->GetRightEncoder()->GetDistance());
|
||||
if (driveForwardSpeed * kP * error >= driveForwardSpeed) {
|
||||
Robot::drivetrain->TankDrive(
|
||||
driveForwardSpeed, driveForwardSpeed);
|
||||
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(driveForwardSpeed * kP * error,
|
||||
driveForwardSpeed * kP * error);
|
||||
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(error) <= kTolerance) || IsTimedOut();
|
||||
return (std::fabs(m_error) <= kTolerance) || IsTimedOut();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveForward::End() {
|
||||
Robot::drivetrain->Stop();
|
||||
Robot::drivetrain.Stop();
|
||||
}
|
||||
|
||||
@@ -24,9 +24,9 @@ public:
|
||||
void End() override;
|
||||
|
||||
private:
|
||||
double driveForwardSpeed;
|
||||
double distance;
|
||||
double error = 0;
|
||||
double m_driveForwardSpeed;
|
||||
double m_distance;
|
||||
double m_error = 0;
|
||||
static constexpr double kTolerance = 0.1;
|
||||
static constexpr double kP = -1.0 / 5.0;
|
||||
|
||||
|
||||
@@ -10,12 +10,13 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
DriveWithJoystick::DriveWithJoystick() {
|
||||
Requires(Robot::drivetrain.get());
|
||||
Requires(&Robot::drivetrain);
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
void DriveWithJoystick::Execute() {
|
||||
Robot::drivetrain->TankDrive(Robot::oi->GetJoystick());
|
||||
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()
|
||||
@@ -25,5 +26,5 @@ bool DriveWithJoystick::IsFinished() {
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void DriveWithJoystick::End() {
|
||||
Robot::drivetrain->Stop();
|
||||
Robot::drivetrain.Stop();
|
||||
}
|
||||
|
||||
@@ -11,15 +11,15 @@
|
||||
|
||||
ExtendShooter::ExtendShooter()
|
||||
: frc::TimedCommand(1.0) {
|
||||
Requires(Robot::shooter.get());
|
||||
Requires(&Robot::shooter);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void ExtendShooter::Initialize() {
|
||||
Robot::shooter->ExtendBoth();
|
||||
Robot::shooter.ExtendBoth();
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
void ExtendShooter::End() {
|
||||
Robot::shooter->RetractBoth();
|
||||
Robot::shooter.RetractBoth();
|
||||
}
|
||||
|
||||
@@ -10,15 +10,15 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
OpenClaw::OpenClaw() {
|
||||
Requires(Robot::collector.get());
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void OpenClaw::Initialize() {
|
||||
Robot::collector->Open();
|
||||
Robot::collector.Open();
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool OpenClaw::IsFinished() {
|
||||
return Robot::collector->IsOpen();
|
||||
return Robot::collector.IsOpen();
|
||||
}
|
||||
|
||||
@@ -10,11 +10,11 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetCollectionSpeed::SetCollectionSpeed(double speed) {
|
||||
Requires(Robot::collector.get());
|
||||
this->speed = speed;
|
||||
Requires(&Robot::collector);
|
||||
m_speed = speed;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetCollectionSpeed::Initialize() {
|
||||
Robot::collector->SetSpeed(speed);
|
||||
Robot::collector.SetSpeed(m_speed);
|
||||
}
|
||||
|
||||
@@ -20,5 +20,5 @@ public:
|
||||
void Initialize() override;
|
||||
|
||||
private:
|
||||
double speed;
|
||||
double m_speed;
|
||||
};
|
||||
|
||||
@@ -10,17 +10,17 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
SetPivotSetpoint::SetPivotSetpoint(double setpoint) {
|
||||
this->setpoint = setpoint;
|
||||
Requires(Robot::pivot.get());
|
||||
m_setpoint = setpoint;
|
||||
Requires(&Robot::pivot);
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
void SetPivotSetpoint::Initialize() {
|
||||
Robot::pivot->Enable();
|
||||
Robot::pivot->SetSetpoint(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();
|
||||
return Robot::pivot.OnTarget();
|
||||
}
|
||||
|
||||
@@ -21,5 +21,5 @@ public:
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
double setpoint;
|
||||
double m_setpoint;
|
||||
};
|
||||
|
||||
@@ -10,10 +10,10 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
WaitForBall::WaitForBall() {
|
||||
Requires(Robot::collector.get());
|
||||
Requires(&Robot::collector);
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool WaitForBall::IsFinished() {
|
||||
return Robot::collector->HasBall();
|
||||
return Robot::collector.HasBall();
|
||||
}
|
||||
|
||||
@@ -10,10 +10,10 @@
|
||||
#include "../Robot.h"
|
||||
|
||||
WaitForPressure::WaitForPressure() {
|
||||
Requires(Robot::pneumatics.get());
|
||||
Requires(&Robot::pneumatics);
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
bool WaitForPressure::IsFinished() {
|
||||
return Robot::pneumatics->IsPressurized();
|
||||
return Robot::pneumatics.IsPressurized();
|
||||
}
|
||||
|
||||
@@ -17,13 +17,13 @@
|
||||
#include "Subsystems/Pivot.h"
|
||||
|
||||
OI::OI() {
|
||||
R1.WhenPressed(new LowGoal());
|
||||
R2.WhenPressed(new Collect());
|
||||
m_r1.WhenPressed(new LowGoal());
|
||||
m_r2.WhenPressed(new Collect());
|
||||
|
||||
L1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
|
||||
L2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
|
||||
m_l1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
|
||||
m_l2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
|
||||
|
||||
sticks.WhenActive(new Shoot());
|
||||
m_sticks.WhenActive(new Shoot());
|
||||
|
||||
// SmartDashboard Buttons
|
||||
frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
|
||||
@@ -36,6 +36,6 @@ OI::OI() {
|
||||
new SetCollectionSpeed(Collector::kReverse));
|
||||
}
|
||||
|
||||
frc::Joystick* OI::GetJoystick() {
|
||||
return &joystick;
|
||||
frc::Joystick& OI::GetJoystick() {
|
||||
return m_joystick;
|
||||
}
|
||||
|
||||
@@ -15,15 +15,15 @@
|
||||
class OI {
|
||||
public:
|
||||
OI();
|
||||
frc::Joystick* GetJoystick();
|
||||
frc::Joystick& GetJoystick();
|
||||
|
||||
private:
|
||||
frc::Joystick joystick{0};
|
||||
frc::Joystick m_joystick{0};
|
||||
|
||||
frc::JoystickButton L1{&joystick, 11};
|
||||
frc::JoystickButton L2{&joystick, 9};
|
||||
frc::JoystickButton R1{&joystick, 12};
|
||||
frc::JoystickButton R2{&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 sticks{&joystick, 2, 3};
|
||||
DoubleButton m_sticks{&m_joystick, 2, 3};
|
||||
};
|
||||
|
||||
@@ -11,32 +11,32 @@
|
||||
|
||||
#include <SmartDashboard/SmartDashboard.h>
|
||||
|
||||
std::shared_ptr<DriveTrain> Robot::drivetrain = std::make_shared<DriveTrain>();
|
||||
std::shared_ptr<Pivot> Robot::pivot = std::make_shared<Pivot>();
|
||||
std::shared_ptr<Collector> Robot::collector = std::make_shared<Collector>();
|
||||
std::shared_ptr<Shooter> Robot::shooter = std::make_shared<Shooter>();
|
||||
std::shared_ptr<Pneumatics> Robot::pneumatics = std::make_shared<Pneumatics>();
|
||||
std::unique_ptr<OI> Robot::oi = std::make_unique<OI>();
|
||||
DriveTrain Robot::drivetrain;
|
||||
Pivot Robot::pivot;
|
||||
Collector Robot::collector;
|
||||
Shooter Robot::shooter;
|
||||
Pneumatics Robot::pneumatics;
|
||||
OI Robot::oi;
|
||||
|
||||
void Robot::RobotInit() {
|
||||
// Show what command your subsystem is running on the SmartDashboard
|
||||
frc::SmartDashboard::PutData(drivetrain.get());
|
||||
frc::SmartDashboard::PutData(pivot.get());
|
||||
frc::SmartDashboard::PutData(collector.get());
|
||||
frc::SmartDashboard::PutData(shooter.get());
|
||||
frc::SmartDashboard::PutData(pneumatics.get());
|
||||
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
|
||||
autoChooser.AddDefault("Drive and Shoot", driveAndShootAuto.get());
|
||||
autoChooser.AddObject("Drive Forward", driveForwardAuto.get());
|
||||
frc::SmartDashboard::PutData("Auto Mode", &autoChooser);
|
||||
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() {
|
||||
autonomousCommand = autoChooser.GetSelected();
|
||||
autonomousCommand->Start();
|
||||
m_autonomousCommand = m_autoChooser.GetSelected();
|
||||
m_autonomousCommand->Start();
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {
|
||||
@@ -49,8 +49,8 @@ void Robot::TeleopInit() {
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (autonomousCommand != nullptr) {
|
||||
autonomousCommand->Cancel();
|
||||
if (m_autonomousCommand != nullptr) {
|
||||
m_autonomousCommand->Cancel();
|
||||
}
|
||||
std::cout << "Starting Teleop" << std::endl;
|
||||
}
|
||||
@@ -65,7 +65,7 @@ void Robot::TestPeriodic() {
|
||||
}
|
||||
|
||||
void Robot::DisabledInit() {
|
||||
shooter->Unlatch();
|
||||
shooter.Unlatch();
|
||||
}
|
||||
|
||||
void Robot::DisabledPeriodic() {
|
||||
@@ -76,12 +76,12 @@ void Robot::DisabledPeriodic() {
|
||||
* Log interesting values to the SmartDashboard.
|
||||
*/
|
||||
void Robot::Log() {
|
||||
Robot::pneumatics->WritePressure();
|
||||
frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot->GetAngle());
|
||||
Robot::pneumatics.WritePressure();
|
||||
frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot.GetAngle());
|
||||
frc::SmartDashboard::PutNumber("Left Distance",
|
||||
drivetrain->GetLeftEncoder()->GetDistance());
|
||||
drivetrain.GetLeftEncoder().GetDistance());
|
||||
frc::SmartDashboard::PutNumber("Right Distance",
|
||||
drivetrain->GetRightEncoder()->GetDistance());
|
||||
drivetrain.GetRightEncoder().GetDistance());
|
||||
}
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -7,8 +7,6 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <Commands/Command.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <SmartDashboard/SendableChooser.h>
|
||||
@@ -24,19 +22,18 @@
|
||||
|
||||
class Robot : public IterativeRobot {
|
||||
public:
|
||||
static std::shared_ptr<DriveTrain> drivetrain;
|
||||
static std::shared_ptr<Pivot> pivot;
|
||||
static std::shared_ptr<Collector> collector;
|
||||
static std::shared_ptr<Shooter> shooter;
|
||||
static std::shared_ptr<Pneumatics> pneumatics;
|
||||
static std::unique_ptr<OI> oi;
|
||||
static DriveTrain drivetrain;
|
||||
static Pivot pivot;
|
||||
static Collector collector;
|
||||
static Shooter shooter;
|
||||
static Pneumatics pneumatics;
|
||||
static OI oi;
|
||||
|
||||
private:
|
||||
frc::Command* autonomousCommand = nullptr;
|
||||
std::unique_ptr<frc::Command> driveAndShootAuto{
|
||||
new DriveAndShootAutonomous()};
|
||||
std::unique_ptr<frc::Command> driveForwardAuto{new DriveForward()};
|
||||
SendableChooser<frc::Command*> autoChooser;
|
||||
frc::Command* m_autonomousCommand = nullptr;
|
||||
DriveAndShootAutonomous m_driveAndShootAuto;
|
||||
DriveForward m_driveForwardAuto;
|
||||
SendableChooser<frc::Command*> m_autoChooser;
|
||||
|
||||
void RobotInit() override;
|
||||
void AutonomousInit() override;
|
||||
|
||||
@@ -13,36 +13,37 @@ Collector::Collector()
|
||||
: frc::Subsystem("Collector") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
// XXX: LiveWindow::GetInstance()->AddActuator("Collector", "Roller
|
||||
// Motor", &rollerMotor);
|
||||
// Motor", &m_rollerMotor);
|
||||
LiveWindow::GetInstance()->AddSensor(
|
||||
"Collector", "Ball Detector", &ballDetector);
|
||||
"Collector", "Ball Detector", &m_ballDetector);
|
||||
LiveWindow::GetInstance()->AddSensor(
|
||||
"Collector", "Claw Open Detector", &openDetector);
|
||||
LiveWindow::GetInstance()->AddActuator("Collector", "Piston", &piston);
|
||||
"Collector", "Claw Open Detector", &m_openDetector);
|
||||
LiveWindow::GetInstance()->AddActuator(
|
||||
"Collector", "Piston", &m_piston);
|
||||
}
|
||||
|
||||
bool Collector::HasBall() {
|
||||
return ballDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
return m_ballDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
}
|
||||
|
||||
void Collector::SetSpeed(double speed) {
|
||||
rollerMotor.Set(-speed);
|
||||
m_rollerMotor.Set(-speed);
|
||||
}
|
||||
|
||||
void Collector::Stop() {
|
||||
rollerMotor.Set(0);
|
||||
m_rollerMotor.Set(0);
|
||||
}
|
||||
|
||||
bool Collector::IsOpen() {
|
||||
return openDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
return m_openDetector.Get(); // TODO: prepend ! to reflect real robot
|
||||
}
|
||||
|
||||
void Collector::Open() {
|
||||
piston.Set(true);
|
||||
m_piston.Set(true);
|
||||
}
|
||||
|
||||
void Collector::Close() {
|
||||
piston.Set(false);
|
||||
m_piston.Set(false);
|
||||
}
|
||||
|
||||
void Collector::InitDefaultCommand() {}
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#include <Commands/Subsystem.h>
|
||||
#include <DigitalInput.h>
|
||||
#include <Solenoid.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* The Collector subsystem has one motor for the rollers, a limit switch for
|
||||
@@ -69,8 +69,8 @@ public:
|
||||
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Victor rollerMotor{6};
|
||||
frc::DigitalInput ballDetector{10};
|
||||
frc::Solenoid piston{1};
|
||||
frc::DigitalInput openDetector{6};
|
||||
frc::Spark m_rollerMotor{6};
|
||||
frc::DigitalInput m_ballDetector{10};
|
||||
frc::Solenoid m_piston{1};
|
||||
frc::DigitalInput m_openDetector{6};
|
||||
};
|
||||
|
||||
@@ -17,77 +17,70 @@
|
||||
DriveTrain::DriveTrain()
|
||||
: frc::Subsystem("DriveTrain") {
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Left
|
||||
// CIM", &frontLeftCIM);
|
||||
// CIM", &m_frontLeftCIM);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front
|
||||
// Right CIM", &frontRightCIM);
|
||||
// Right CIM", &m_frontRightCIM);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Left
|
||||
// CIM", &backLeftCIM);
|
||||
// CIM", &m_backLeftCIM);
|
||||
// frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Right
|
||||
// CIM", &backRightCIM);
|
||||
// CIM", &m_backRightCIM);
|
||||
|
||||
// Configure the RobotDrive to reflect the fact that all our motors are
|
||||
// wired backwards and our drivers sensitivity preferences.
|
||||
drive.SetSafetyEnabled(false);
|
||||
drive.SetExpiration(0.1);
|
||||
drive.SetSensitivity(0.5);
|
||||
drive.SetMaxOutput(1.0);
|
||||
drive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
|
||||
drive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
|
||||
drive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
|
||||
drive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetExpiration(0.1);
|
||||
m_robotDrive.SetMaxOutput(1.0);
|
||||
m_leftCIMs.SetInverted(true);
|
||||
m_rightCIMs.SetInverted(true);
|
||||
|
||||
// Configure encoders
|
||||
rightEncoder->SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
leftEncoder->SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
m_rightEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
m_leftEncoder.SetPIDSourceType(PIDSourceType::kDisplacement);
|
||||
|
||||
#ifndef SIMULATION
|
||||
// Converts to feet
|
||||
rightEncoder->SetDistancePerPulse(0.0785398);
|
||||
leftEncoder->SetDistancePerPulse(0.0785398);
|
||||
m_rightEncoder.SetDistancePerPulse(0.0785398);
|
||||
m_leftEncoder.SetDistancePerPulse(0.0785398);
|
||||
#else
|
||||
// Convert to feet 4in diameter wheels with 360 tick simulated encoders
|
||||
rightEncoder->SetDistancePerPulse(
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
|
||||
leftEncoder->SetDistancePerPulse(
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
(4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/));
|
||||
#endif
|
||||
|
||||
LiveWindow::GetInstance()->AddSensor(
|
||||
"DriveTrain", "Right Encoder", rightEncoder);
|
||||
"DriveTrain", "Right Encoder", m_rightEncoder);
|
||||
LiveWindow::GetInstance()->AddSensor(
|
||||
"DriveTrain", "Left Encoder", leftEncoder);
|
||||
"DriveTrain", "Left Encoder", m_leftEncoder);
|
||||
|
||||
// Configure gyro
|
||||
#ifndef SIMULATION
|
||||
gyro.SetSensitivity(0.007); // TODO: Handle more gracefully?
|
||||
m_gyro.SetSensitivity(0.007); // TODO: Handle more gracefully?
|
||||
#endif
|
||||
LiveWindow::GetInstance()->AddSensor("DriveTrain", "Gyro", &gyro);
|
||||
LiveWindow::GetInstance()->AddSensor("DriveTrain", "Gyro", &m_gyro);
|
||||
}
|
||||
|
||||
void DriveTrain::InitDefaultCommand() {
|
||||
SetDefaultCommand(new DriveWithJoystick());
|
||||
}
|
||||
|
||||
void DriveTrain::TankDrive(Joystick* joy) {
|
||||
drive.TankDrive(joy->GetY(), joy->GetRawAxis(4));
|
||||
}
|
||||
|
||||
void DriveTrain::TankDrive(double leftAxis, double rightAxis) {
|
||||
drive.TankDrive(leftAxis, rightAxis);
|
||||
m_robotDrive.TankDrive(leftAxis, rightAxis);
|
||||
}
|
||||
|
||||
void DriveTrain::Stop() {
|
||||
drive.TankDrive(0.0, 0.0);
|
||||
m_robotDrive.TankDrive(0.0, 0.0);
|
||||
}
|
||||
|
||||
std::shared_ptr<Encoder> DriveTrain::GetLeftEncoder() {
|
||||
return leftEncoder;
|
||||
Encoder& DriveTrain::GetLeftEncoder() {
|
||||
return m_leftEncoder;
|
||||
}
|
||||
|
||||
std::shared_ptr<Encoder> DriveTrain::GetRightEncoder() {
|
||||
return rightEncoder;
|
||||
Encoder& DriveTrain::GetRightEncoder() {
|
||||
return m_rightEncoder;
|
||||
}
|
||||
|
||||
double DriveTrain::GetAngle() {
|
||||
return gyro.GetAngle();
|
||||
return m_gyro.GetAngle();
|
||||
}
|
||||
|
||||
@@ -7,13 +7,12 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <AnalogGyro.h>
|
||||
#include <Commands/Subsystem.h>
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <Encoder.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
#include <SpeedControllerGroup.h>
|
||||
|
||||
namespace frc {
|
||||
class Joystick;
|
||||
@@ -34,11 +33,6 @@ public:
|
||||
*/
|
||||
void InitDefaultCommand();
|
||||
|
||||
/**
|
||||
* @param joy PS3 style joystick to use as the input for tank drive.
|
||||
*/
|
||||
void TankDrive(frc::Joystick* joy);
|
||||
|
||||
/**
|
||||
* @param leftAxis Left sides value
|
||||
* @param rightAxis Right sides value
|
||||
@@ -54,13 +48,13 @@ public:
|
||||
* @return The encoder getting the distance and speed of left side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
std::shared_ptr<Encoder> GetLeftEncoder();
|
||||
Encoder& GetLeftEncoder();
|
||||
|
||||
/**
|
||||
* @return The encoder getting the distance and speed of right side of
|
||||
* the drivetrain.
|
||||
*/
|
||||
std::shared_ptr<Encoder> GetRightEncoder();
|
||||
Encoder& GetRightEncoder();
|
||||
|
||||
/**
|
||||
* @return The current angle of the drivetrain.
|
||||
@@ -69,17 +63,17 @@ public:
|
||||
|
||||
private:
|
||||
// Subsystem devices
|
||||
frc::Victor frontLeftCIM{1};
|
||||
frc::Victor rearLeftCIM{2};
|
||||
frc::Victor frontRightCIM{3};
|
||||
frc::Victor rearRightCIM{4};
|
||||
frc::RobotDrive drive{frontRightCIM, rearLeftCIM, frontRightCIM,
|
||||
rearRightCIM};
|
||||
std::shared_ptr<frc::Encoder> rightEncoder =
|
||||
std::make_shared<frc::Encoder>(
|
||||
1, 2, true, Encoder::k4X);
|
||||
std::shared_ptr<frc::Encoder> leftEncoder =
|
||||
std::make_shared<frc::Encoder>(
|
||||
3, 4, false, Encoder::k4X);
|
||||
frc::AnalogGyro gyro{0};
|
||||
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::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};
|
||||
};
|
||||
|
||||
@@ -21,12 +21,13 @@ Pivot::Pivot()
|
||||
|
||||
// Put everything to the LiveWindow for testing.
|
||||
frc::LiveWindow::GetInstance()->AddSensor(
|
||||
"Pivot", "Upper Limit Switch", &upperLimitSwitch);
|
||||
"Pivot", "Upper Limit Switch", &m_upperLimitSwitch);
|
||||
frc::LiveWindow::GetInstance()->AddSensor(
|
||||
"Pivot", "Lower Limit Switch", &lowerLimitSwitch);
|
||||
// XXX: frc::LiveWindow::GetInstance()->AddSensor("Pivot", "Pot", &pot);
|
||||
"Pivot", "Lower Limit Switch", &m_lowerLimitSwitch);
|
||||
// XXX: frc::LiveWindow::GetInstance()->AddSensor("Pivot", "Pot",
|
||||
// &m_pot);
|
||||
// XXX: frc::LiveWindow::GetInstance()->AddActuator("Pivot", "Motor",
|
||||
// &motor);
|
||||
// &m_motor);
|
||||
frc::LiveWindow::GetInstance()->AddActuator(
|
||||
"Pivot", "PIDSubsystem Controller", GetPIDController());
|
||||
}
|
||||
@@ -34,23 +35,23 @@ Pivot::Pivot()
|
||||
void InitDefaultCommand() {}
|
||||
|
||||
double Pivot::ReturnPIDInput() {
|
||||
return pot.Get();
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
void Pivot::UsePIDOutput(double output) {
|
||||
motor.PIDWrite(output);
|
||||
m_motor.PIDWrite(output);
|
||||
}
|
||||
|
||||
bool Pivot::IsAtUpperLimit() {
|
||||
return upperLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
return m_upperLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
}
|
||||
|
||||
bool Pivot::IsAtLowerLimit() {
|
||||
return lowerLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
return m_lowerLimitSwitch.Get(); // TODO: inverted from real robot
|
||||
// (prefix with !)
|
||||
}
|
||||
|
||||
double Pivot::GetAngle() {
|
||||
return pot.Get();
|
||||
return m_pot.Get();
|
||||
}
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#include <AnalogPotentiometer.h>
|
||||
#include <Commands/PIDSubsystem.h>
|
||||
#include <DigitalInput.h>
|
||||
#include <Victor.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* The Pivot subsystem contains the Van-door motor and the pot for PID control
|
||||
@@ -61,14 +61,14 @@ private:
|
||||
// Subsystem devices
|
||||
|
||||
// Sensors for measuring the position of the pivot
|
||||
frc::DigitalInput upperLimitSwitch{13};
|
||||
frc::DigitalInput lowerLimitSwitch{12};
|
||||
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 pot{1};
|
||||
frc::AnalogPotentiometer m_pot{1};
|
||||
|
||||
// Motor to move the pivot
|
||||
frc::Victor motor{5};
|
||||
frc::Spark m_motor{5};
|
||||
};
|
||||
|
||||
@@ -12,7 +12,7 @@
|
||||
Pneumatics::Pneumatics()
|
||||
: frc::Subsystem("Pneumatics") {
|
||||
frc::LiveWindow::GetInstance()->AddSensor(
|
||||
"Pneumatics", "Pressure Sensor", pressureSensor);
|
||||
"Pneumatics", "Pressure Sensor", m_pressureSensor);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -26,7 +26,7 @@ void Pneumatics::InitDefaultCommand() {}
|
||||
*/
|
||||
void Pneumatics::Start() {
|
||||
#ifndef SIMULATION
|
||||
compressor.Start();
|
||||
m_compressor.Start();
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -35,7 +35,7 @@ void Pneumatics::Start() {
|
||||
*/
|
||||
bool Pneumatics::IsPressurized() {
|
||||
#ifndef SIMULATION
|
||||
return kMaxPressure <= pressureSensor.GetVoltage();
|
||||
return kMaxPressure <= m_pressureSensor.GetVoltage();
|
||||
#else
|
||||
return true; // NOTE: Simulation always has full pressure
|
||||
#endif
|
||||
@@ -45,5 +45,6 @@ bool Pneumatics::IsPressurized() {
|
||||
* Puts the pressure on the SmartDashboard.
|
||||
*/
|
||||
void Pneumatics::WritePressure() {
|
||||
frc::SmartDashboard::PutNumber("Pressure", pressureSensor.GetVoltage());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
"Pressure", m_pressureSensor.GetVoltage());
|
||||
}
|
||||
|
||||
@@ -43,10 +43,10 @@ public:
|
||||
void WritePressure();
|
||||
|
||||
private:
|
||||
frc::AnalogInput pressureSensor{3};
|
||||
frc::AnalogInput m_pressureSensor{3};
|
||||
|
||||
#ifndef SIMULATION
|
||||
frc::Compressor compressor{1}; // TODO: (1, 14, 1, 8);
|
||||
frc::Compressor m_compressor{1}; // TODO: (1, 14, 1, 8);
|
||||
#endif
|
||||
|
||||
static constexpr double kMaxPressure = 2.55;
|
||||
|
||||
@@ -13,13 +13,14 @@ Shooter::Shooter()
|
||||
: Subsystem("Shooter") {
|
||||
// Put everything to the LiveWindow for testing.
|
||||
frc::LiveWindow::GetInstance()->AddSensor(
|
||||
"Shooter", "Hot Goal Sensor", &hotGoalSensor);
|
||||
"Shooter", "Hot Goal Sensor", &m_hotGoalSensor);
|
||||
frc::LiveWindow::GetInstance()->AddSensor("Shooter",
|
||||
"Piston1 Reed Switch Front ", &piston1ReedSwitchFront);
|
||||
"Piston1 Reed Switch Front ",
|
||||
&m_piston1ReedSwitchFront);
|
||||
frc::LiveWindow::GetInstance()->AddSensor("Shooter",
|
||||
"Piston1 Reed Switch Back ", &piston1ReedSwitchBack);
|
||||
"Piston1 Reed Switch Back ", &m_piston1ReedSwitchBack);
|
||||
frc::LiveWindow::GetInstance()->AddActuator(
|
||||
"Shooter", "Latch Piston", &latchPiston);
|
||||
"Shooter", "Latch Piston", &m_latchPiston);
|
||||
}
|
||||
|
||||
void Shooter::InitDefaultCommand() {
|
||||
@@ -28,64 +29,64 @@ void Shooter::InitDefaultCommand() {
|
||||
}
|
||||
|
||||
void Shooter::ExtendBoth() {
|
||||
piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
|
||||
void Shooter::RetractBoth() {
|
||||
piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
|
||||
void Shooter::Extend1() {
|
||||
piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
|
||||
void Shooter::Retract1() {
|
||||
piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
|
||||
void Shooter::Extend2() {
|
||||
piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kReverse);
|
||||
}
|
||||
|
||||
void Shooter::Retract2() {
|
||||
piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kForward);
|
||||
}
|
||||
|
||||
void Shooter::Off1() {
|
||||
piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
|
||||
void Shooter::Off2() {
|
||||
piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
|
||||
void Shooter::Unlatch() {
|
||||
latchPiston.Set(true);
|
||||
m_latchPiston.Set(true);
|
||||
}
|
||||
|
||||
void Shooter::Latch() {
|
||||
latchPiston.Set(false);
|
||||
m_latchPiston.Set(false);
|
||||
}
|
||||
|
||||
void Shooter::ToggleLatchPosition() {
|
||||
latchPiston.Set(!latchPiston.Get());
|
||||
m_latchPiston.Set(!m_latchPiston.Get());
|
||||
}
|
||||
|
||||
bool Shooter::Piston1IsExtended() {
|
||||
return !piston1ReedSwitchFront.Get();
|
||||
return !m_piston1ReedSwitchFront.Get();
|
||||
}
|
||||
|
||||
bool Shooter::Piston1IsRetracted() {
|
||||
return !piston1ReedSwitchBack.Get();
|
||||
return !m_piston1ReedSwitchBack.Get();
|
||||
}
|
||||
|
||||
void Shooter::OffBoth() {
|
||||
piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston1.Set(frc::DoubleSolenoid::kOff);
|
||||
m_piston2.Set(frc::DoubleSolenoid::kOff);
|
||||
}
|
||||
|
||||
bool Shooter::GoalIsHot() {
|
||||
return hotGoalSensor.Get();
|
||||
return m_hotGoalSensor.Get();
|
||||
}
|
||||
|
||||
@@ -117,11 +117,11 @@ public:
|
||||
|
||||
private:
|
||||
// Devices
|
||||
frc::DoubleSolenoid piston1{3, 4};
|
||||
frc::DoubleSolenoid piston2{5, 6};
|
||||
frc::Solenoid latchPiston{1, 2};
|
||||
frc::DigitalInput piston1ReedSwitchFront{9};
|
||||
frc::DigitalInput piston1ReedSwitchBack{11};
|
||||
frc::DigitalInput hotGoalSensor{
|
||||
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
|
||||
};
|
||||
|
||||
@@ -9,12 +9,12 @@
|
||||
|
||||
#include <Joystick.h>
|
||||
|
||||
DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2) {
|
||||
this->joy = joy;
|
||||
this->button1 = button1;
|
||||
this->button2 = button2;
|
||||
DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2)
|
||||
: m_joy(*joy) {
|
||||
m_button1 = button1;
|
||||
m_button2 = button2;
|
||||
}
|
||||
|
||||
bool DoubleButton::Get() {
|
||||
return joy->GetRawButton(button1) && joy->GetRawButton(button2);
|
||||
return m_joy.GetRawButton(m_button1) && m_joy.GetRawButton(m_button2);
|
||||
}
|
||||
|
||||
@@ -20,7 +20,7 @@ public:
|
||||
bool Get();
|
||||
|
||||
private:
|
||||
frc::Joystick* joy;
|
||||
int button1;
|
||||
int button2;
|
||||
frc::Joystick& m_joy;
|
||||
int m_button1;
|
||||
int m_button2;
|
||||
};
|
||||
|
||||
@@ -20,22 +20,22 @@
|
||||
*/
|
||||
class Robot : public frc::IterativeRobot {
|
||||
public:
|
||||
void RobotInit() override { pidController.SetInputRange(0, 5); }
|
||||
void RobotInit() override { m_pidController.SetInputRange(0, 5); }
|
||||
|
||||
void TeleopInit() override { 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 = joystick.GetTrigger();
|
||||
if (currentButtonValue && !previousButtonValue) {
|
||||
bool currentButtonValue = m_joystick.GetTrigger();
|
||||
if (currentButtonValue && !m_previousButtonValue) {
|
||||
// index of the elevator setpoint wraps around.
|
||||
index = (index + 1) % (sizeof(kSetPoints) / 8);
|
||||
m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
|
||||
}
|
||||
previousButtonValue = currentButtonValue;
|
||||
m_previousButtonValue = currentButtonValue;
|
||||
|
||||
pidController.SetSetpoint(kSetPoints[index]);
|
||||
m_pidController.SetSetpoint(kSetPoints[m_index]);
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -59,12 +59,12 @@ private:
|
||||
static constexpr double kI = -0.02;
|
||||
static constexpr double kD = -2.0;
|
||||
|
||||
int index = 0;
|
||||
bool previousButtonValue = false;
|
||||
int m_index = 0;
|
||||
bool m_previousButtonValue = false;
|
||||
|
||||
frc::AnalogInput potentiometer{kPotChannel};
|
||||
frc::Joystick joystick{kJoystickChannel};
|
||||
frc::Spark 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
|
||||
@@ -73,8 +73,8 @@ private:
|
||||
* to the PIDSource and PIDOutput, so you must use &potentiometer and
|
||||
* &elevatorMotor to get their pointers.
|
||||
*/
|
||||
frc::PIDController pidController{
|
||||
kP, kI, kD, &potentiometer, &elevatorMotor};
|
||||
frc::PIDController m_pidController{
|
||||
kP, kI, kD, &m_potentiometer, &m_elevatorMotor};
|
||||
};
|
||||
|
||||
constexpr std::array<double, 3> Robot::kSetPoints;
|
||||
|
||||
@@ -6,8 +6,9 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <AnalogInput.h>
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* This is a sample program demonstrating how to use an ultrasonic sensor and
|
||||
@@ -21,11 +22,12 @@ public:
|
||||
*/
|
||||
void TeleopPeriodic() override {
|
||||
// sensor returns a value from 0-4095 that is scaled to inches
|
||||
double currentDistance = ultrasonic.GetValue() * kValueToInches;
|
||||
double currentDistance =
|
||||
m_ultrasonic.GetValue() * kValueToInches;
|
||||
// convert distance error to a motor speed
|
||||
double currentSpeed = (kHoldDistance - currentDistance) * kP;
|
||||
// drive robot
|
||||
myRobot.Drive(currentSpeed, 0);
|
||||
m_robotDrive.ArcadeDrive(currentSpeed, 0);
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -42,8 +44,11 @@ private:
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
|
||||
frc::AnalogInput ultrasonic{kUltrasonicPort};
|
||||
frc::RobotDrive myRobot{kLeftMotorPort, kRightMotorPort};
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
@@ -6,10 +6,11 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <AnalogInput.h>
|
||||
#include <Drive/DifferentialDrive.h>
|
||||
#include <IterativeRobot.h>
|
||||
#include <PIDController.h>
|
||||
#include <PIDOutput.h>
|
||||
#include <RobotDrive.h>
|
||||
#include <Spark.h>
|
||||
|
||||
/**
|
||||
* This is a sample program demonstrating how to use an ultrasonic sensor and
|
||||
@@ -26,29 +27,31 @@ public:
|
||||
// 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.
|
||||
pidController.SetInputRange(0, 24 * kValueToInches);
|
||||
m_pidController.SetInputRange(0, 24 * kValueToInches);
|
||||
|
||||
// Set setpoint of the pidController
|
||||
pidController.SetSetpoint(kHoldDistance * kValueToInches);
|
||||
pidController.Enable(); // begin PID control
|
||||
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
|
||||
|
||||
// Begin PID control
|
||||
m_pidController.Enable();
|
||||
}
|
||||
|
||||
private:
|
||||
// internal class to write to myRobot (a RobotDrive object) using a
|
||||
// PIDOutput
|
||||
// Internal class to write to robot drive using a PIDOutput
|
||||
class MyPIDOutput : public frc::PIDOutput {
|
||||
public:
|
||||
explicit MyPIDOutput(frc::RobotDrive& r)
|
||||
: rd(r) {
|
||||
rd.SetSafetyEnabled(false);
|
||||
explicit MyPIDOutput(frc::DifferentialDrive& r)
|
||||
: m_rd(r) {
|
||||
m_rd.SetSafetyEnabled(false);
|
||||
}
|
||||
|
||||
void PIDWrite(double output) override {
|
||||
rd.Drive(output, 0); // write to myRobot (RobotDrive)
|
||||
// by reference
|
||||
// Write to robot drive by reference
|
||||
m_rd.ArcadeDrive(output, 0);
|
||||
}
|
||||
|
||||
private:
|
||||
frc::RobotDrive& rd;
|
||||
frc::DifferentialDrive& m_rd;
|
||||
};
|
||||
|
||||
// Distance in inches the robot wants to stay from an object
|
||||
@@ -70,10 +73,14 @@ private:
|
||||
static constexpr int kRightMotorPort = 1;
|
||||
static constexpr int kUltrasonicPort = 0;
|
||||
|
||||
frc::AnalogInput ultrasonic{kUltrasonicPort};
|
||||
frc::RobotDrive myRobot{kLeftMotorPort, kRightMotorPort};
|
||||
frc::PIDController pidController{
|
||||
kP, kI, kD, &ultrasonic, new MyPIDOutput(myRobot)};
|
||||
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
|
||||
|
||||
frc::Spark m_left{kLeftMotorPort};
|
||||
frc::Spark m_right{kRightMotorPort};
|
||||
frc::DifferentialDrive m_robotDrive{m_left, m_right};
|
||||
|
||||
frc::PIDController m_pidController{kP, kI, kD, &m_ultrasonic,
|
||||
new MyPIDOutput(m_robotDrive)};
|
||||
};
|
||||
|
||||
START_ROBOT_CLASS(Robot)
|
||||
|
||||
Reference in New Issue
Block a user