Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Replace deprecated API usage in C++ examples

Browse Source 
Since there is a new version of GearsBot using the new command-based
API, the old GearsBot is just removed.

PR #1842 is being included to verify this PR is correct.
This commit is contained in:
Tyler Veness
2019-08-27 21:21:05 -07:00
committed by Peter Johnson
parent d6b9c7e148
commit c9f9feff1f
61 changed files with 20 additions and 2220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibcExamples/build.gradle
View File

@@ -30,6 +30,10 @@ templatesTree.list(new FilenameFilter() {
templatesMap.put(it, [])
}
nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.roborio).configure {
cppCompiler.args.remove('-Wno-error=deprecated-declarations')
cppCompiler.args.add('-Werror=deprecated-declarations')
}
ext {
sharedCvConfigs = examplesMap + templatesMap + [commands: []]

36
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp
View File

@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "OI.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include "commands/Autonomous.h"
#include "commands/CloseClaw.h"
#include "commands/OpenClaw.h"
#include "commands/Pickup.h"
#include "commands/Place.h"
#include "commands/PrepareToPickup.h"
#include "commands/SetElevatorSetpoint.h"
OI::OI() {
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());
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; }

47
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
View File

@@ -1,47 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Robot.h"
#include <iostream>
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);
frc::SmartDashboard::PutData(&elevator);
frc::SmartDashboard::PutData(&wrist);
frc::SmartDashboard::PutData(&claw);
}
void Robot::AutonomousInit() {
m_autonomousCommand.Start();
std::cout << "Starting Auto" << std::endl;
}
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;
}
void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

30
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
View File

@@ -1,30 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/Autonomous.h"
#include "commands/CloseClaw.h"
#include "commands/DriveStraight.h"
#include "commands/Pickup.h"
#include "commands/Place.h"
#include "commands/PrepareToPickup.h"
#include "commands/SetDistanceToBox.h"
#include "commands/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());
}

28
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
View File

@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/CloseClaw.h"
#include "Robot.h"
CloseClaw::CloseClaw() : frc::Command("CloseClaw") { Requires(&Robot::claw); }
// Called just before this Command runs the first time
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(); }
// Called once after isFinished returns true
void CloseClaw::End() {
// NOTE: Doesn't stop in simulation due to lower friction causing the can to
// fall out
// + there is no need to worry about stalling the motor or crushing the can.
#ifndef SIMULATION
Robot::claw.Stop();
#endif
}

42
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
View File

@@ -1,42 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/DriveStraight.h"
#include "Robot.h"
DriveStraight::DriveStraight(double 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();
}
// Make this return true when this Command no longer needs to run execute()
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);
}
double DriveStraight::DriveStraightPIDSource::PIDGet() {
return Robot::drivetrain.GetDistance();
}
void DriveStraight::DriveStraightPIDOutput::PIDWrite(double d) {
Robot::drivetrain.Drive(d, d);
}

24
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
View File

@@ -1,24 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/OpenClaw.h"
#include "Robot.h"
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(); }
// Make this return true when this Command no longer needs to run execute()
bool OpenClaw::IsFinished() { return IsTimedOut(); }
// Called once after isFinished returns true
void OpenClaw::End() { Robot::claw.Stop(); }

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/Pickup.h"
#include "commands/CloseClaw.h"
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
Pickup::Pickup() : frc::CommandGroup("Pickup") {
AddSequential(new CloseClaw());
AddParallel(new SetWristSetpoint(-45));
AddSequential(new SetElevatorSetpoint(0.25));
}

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/Place.h"
#include "commands/OpenClaw.h"
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
Place::Place() : frc::CommandGroup("Place") {
AddSequential(new SetElevatorSetpoint(0.25));
AddSequential(new SetWristSetpoint(0));
AddSequential(new OpenClaw());
}

18
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/PrepareToPickup.h"
#include "commands/OpenClaw.h"
#include "commands/SetElevatorSetpoint.h"
#include "commands/SetWristSetpoint.h"
PrepareToPickup::PrepareToPickup() : frc::CommandGroup("PrepareToPickup") {
AddParallel(new OpenClaw());
AddParallel(new SetWristSetpoint(0));
AddSequential(new SetElevatorSetpoint(0));
}

44
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/SetDistanceToBox.h"
#include <frc/PIDController.h>
#include "Robot.h"
SetDistanceToBox::SetDistanceToBox(double 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();
}
// Make this return true when this Command no longer needs to run execute()
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);
}
double SetDistanceToBox::SetDistanceToBoxPIDSource::PIDGet() {
return Robot::drivetrain.GetDistanceToObstacle();
}
void SetDistanceToBox::SetDistanceToBoxPIDOutput::PIDWrite(double d) {
Robot::drivetrain.Drive(d, d);
}

27
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
View File

@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/SetElevatorSetpoint.h"
#include <cmath>
#include "Robot.h"
SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
: frc::Command("SetElevatorSetpoint") {
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();
}
// Make this return true when this Command no longer needs to run execute()
bool SetElevatorSetpoint::IsFinished() { return Robot::elevator.OnTarget(); }

25
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
View File

@@ -1,25 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/SetWristSetpoint.h"
#include "Robot.h"
SetWristSetpoint::SetWristSetpoint(double setpoint)
: frc::Command("SetWristSetpoint") {
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();
}
// Make this return true when this Command no longer needs to run execute()
bool SetWristSetpoint::IsFinished() { return Robot::wrist.OnTarget(); }

27
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp
View File

@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "commands/TankDriveWithJoystick.h"
#include "Robot.h"
TankDriveWithJoystick::TankDriveWithJoystick()
: frc::Command("TankDriveWithJoystick") {
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));
}
// Make this return true when this Command no longer needs to run execute()
bool TankDriveWithJoystick::IsFinished() { return false; }
// Called once after isFinished returns true
void TankDriveWithJoystick::End() { Robot::drivetrain.Drive(0, 0); }

25
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
View File

@@ -1,25 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "subsystems/Claw.h"
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::Close() { m_motor.Set(1); }
void Claw::Stop() { m_motor.Set(0); }
bool Claw::IsGripping() { return m_contact.Get(); }
void Claw::Log() {}

75
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
View File

@@ -1,75 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "subsystems/DriveTrain.h"
#include <frc/Joystick.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include "commands/TankDriveWithJoystick.h"
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);
#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);
#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);
}
void DriveTrain::InitDefaultCommand() {
SetDefaultCommand(new TankDriveWithJoystick());
}
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());
}
void DriveTrain::Drive(double left, double right) {
m_robotDrive.TankDrive(left, right);
}
double DriveTrain::GetHeading() { return m_gyro.GetAngle(); }
void DriveTrain::Reset() {
m_gyro.Reset();
m_leftEncoder.Reset();
m_rightEncoder.Reset();
}
double DriveTrain::GetDistance() {
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();
}

32
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "subsystems/Elevator.h"
#include <frc/livewindow/LiveWindow.h>
#include <frc/smartdashboard/SmartDashboard.h>
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);
#endif
SetAbsoluteTolerance(0.005);
// 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);
}
double Elevator::ReturnPIDInput() { return m_pot.Get(); }
void Elevator::UsePIDOutput(double d) { m_motor.Set(d); }

31
wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
View File

@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "subsystems/Wrist.h"
#include <frc/smartdashboard/SmartDashboard.h>
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);
#endif
SetAbsoluteTolerance(2.5);
// 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);
}
double Wrist::ReturnPIDInput() { return m_pot.Get(); }
void Wrist::UsePIDOutput(double d) { m_motor.Set(d); }

30
wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h
View File

@@ -1,30 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/Joystick.h>
#include <frc/buttons/JoystickButton.h>
class OI {
public:
OI();
frc::Joystick& GetJoystick();
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};
};

41
wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
View File

@@ -1,41 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/TimedRobot.h>
#include <frc/commands/Command.h>
#include <frc/commands/Scheduler.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include "OI.h"
#include "commands/Autonomous.h"
#include "subsystems/Claw.h"
#include "subsystems/DriveTrain.h"
#include "subsystems/Elevator.h"
#include "subsystems/Wrist.h"
class Robot : public frc::TimedRobot {
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();
void RobotInit() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;
};

18
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/CommandGroup.h>
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
class Autonomous : public frc::CommandGroup {
public:
Autonomous();
};

22
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
View File

@@ -1,22 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/Command.h>
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
class CloseClaw : public frc::Command {
public:
CloseClaw();
void Initialize() override;
bool IsFinished() override;
void End() override;
};

44
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/PIDController.h>
#include <frc/PIDOutput.h>
#include <frc/PIDSource.h>
#include <frc/commands/Command.h>
/**
* Drive the given distance straight (negative values go backwards).
* Uses a local PID controller to run a simple PID loop that is only
* enabled while this command is running. The input is the averaged
* 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;
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:
DriveStraightPIDSource m_source;
DriveStraightPIDOutput m_output;
frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
};

22
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
View File

@@ -1,22 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/Command.h>
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
class OpenClaw : public frc::Command {
public:
OpenClaw();
void Initialize() override;
bool IsFinished() override;
void End() override;
};

19
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
View File

@@ -1,19 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/CommandGroup.h>
/**
* Pickup a soda can (if one is between the open claws) and
* get it in a safe state to drive around.
*/
class Pickup : public frc::CommandGroup {
public:
Pickup();
};

18
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/CommandGroup.h>
/**
* Place a held soda can onto the platform.
*/
class Place : public frc::CommandGroup {
public:
Place();
};

18
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/CommandGroup.h>
/**
* Make sure the robot is in a state to pickup soda cans.
*/
class PrepareToPickup : public frc::CommandGroup {
public:
PrepareToPickup();
};

44
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
View File

@@ -1,44 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/PIDController.h>
#include <frc/PIDOutput.h>
#include <frc/PIDSource.h>
#include <frc/commands/Command.h>
/**
* 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
* command is running. The input is the averaged values of the left and right
* encoders.
*/
class SetDistanceToBox : public frc::Command {
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 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};
};

27
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
View File

@@ -1,27 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/Command.h>
/**
* Move the elevator to a given location. This command finishes when it is
* within
* the tolerance, but leaves the PID loop running to maintain the position.
* Other
* 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;
private:
double m_setpoint;
};

25
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
View File

@@ -1,25 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/Command.h>
/**
* Move the wrist to a given angle. This command finishes when it is within
* the tolerance, but leaves the PID loop running to maintain the position.
* 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;
private:
double m_setpoint;
};

21
wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h
View File

@@ -1,21 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/commands/Command.h>
/**
* 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;
};

51
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
View File

@@ -1,51 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/DigitalInput.h>
#include <frc/PWMVictorSPX.h>
#include <frc/commands/Subsystem.h>
/**
* The claw subsystem is a simple system with a motor for opening and closing.
* If using stronger motors, you should probably use a sensor so that the
* motors don't stall.
*/
class Claw : public frc::Subsystem {
public:
Claw();
void InitDefaultCommand() override;
/**
* Set the claw motor to move in the open direction.
*/
void Open();
/**
* Set the claw motor to move in the close direction.
*/
void Close();
/**
* 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();
void Log();
private:
frc::PWMVictorSPX m_motor{7};
frc::DigitalInput m_contact{5};
};

84
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
View File

@@ -1,84 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/AnalogGyro.h>
#include <frc/AnalogInput.h>
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/commands/Subsystem.h>
#include <frc/drive/DifferentialDrive.h>
namespace frc {
class Joystick;
} // namespace frc
/**
* The DriveTrain subsystem incorporates the sensors and actuators attached to
* the robots chassis. These include four drive motors, a left and right encoder
* and a gyro.
*/
class DriveTrain : public frc::Subsystem {
public:
DriveTrain();
/**
* 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();
/**
* 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();
/**
* 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 to the obstacle detected by the rangefinder.
*/
double GetDistanceToObstacle();
private:
frc::PWMVictorSPX m_frontLeft{1};
frc::PWMVictorSPX m_rearLeft{2};
frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
frc::PWMVictorSPX m_frontRight{3};
frc::PWMVictorSPX 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};
};

59
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
View File

@@ -1,59 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
#include <frc/commands/PIDSubsystem.h>
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
* it's current
* state PID values for simulation are different than in the real world do to
* minor differences.
*/
class Elevator : public frc::PIDSubsystem {
public:
Elevator();
void InitDefaultCommand() override;
/**
* 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 motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
void UsePIDOutput(double d) override;
private:
frc::PWMVictorSPX m_motor{5};
// Conversion value of potentiometer varies between the real world and
// simulation
#ifndef SIMULATION
frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
#else
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;
};

55
wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
View File

@@ -1,55 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/AnalogPotentiometer.h>
#include <frc/PWMVictorSPX.h>
#include <frc/commands/PIDSubsystem.h>
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead
* of a linear joint.
*/
class Wrist : public frc::PIDSubsystem {
public:
Wrist();
void InitDefaultCommand() override;
/**
* 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 motor as the PID output. This method is automatically called
* by
* the subsystem.
*/
void UsePIDOutput(double d) override;
private:
frc::PWMVictorSPX m_motor{6};
// Conversion value of potentiometer varies between the real world and
// simulation
#ifndef SIMULATION
frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
#else
frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
#endif
static constexpr double kP_real = 1;
static constexpr double kP_simulation = 0.05;
};

15
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,9 +9,9 @@
#include <frc/AnalogInput.h>
#include <frc/Joystick.h>
#include <frc/PIDController.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
/**
* This is a sample program to demonstrate how to use a soft potentiometer and a
@@ -22,8 +22,6 @@ class Robot : public frc::TimedRobot {
public:
void RobotInit() override { m_pidController.SetInputRange(0, 5); }
void TeleopInit() override { m_pidController.Enable(); }
void TeleopPeriodic() override {
// When the button is pressed once, the selected elevator setpoint is
// incremented.
@@ -35,6 +33,9 @@ class Robot : public frc::TimedRobot {
m_previousButtonValue = currentButtonValue;
m_pidController.SetSetpoint(kSetPoints[m_index]);
double output =
m_pidController.Calculate(m_potentiometer.GetAverageVoltage());
m_elevatorMotor.Set(output);
}
private:
@@ -64,11 +65,7 @@ class Robot : public frc::TimedRobot {
frc::Joystick m_joystick{kJoystickChannel};
frc::PWMVictorSPX m_elevatorMotor{kMotorChannel};
/* 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};
frc2::PIDController m_pidController{kP, kI, kD};
};
constexpr std::array<double, 3> Robot::kSetPoints;

30
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
View File

@@ -1,15 +1,14 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <frc/AnalogInput.h>
#include <frc/PIDController.h>
#include <frc/PIDOutput.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
/**
@@ -29,28 +28,14 @@ class Robot : public frc::TimedRobot {
// Set setpoint of the PID Controller
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
}
// Begin PID control
m_pidController.Enable();
void TeleopPeriodic() override {
double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
m_robotDrive.ArcadeDrive(output, 0);
}
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);
}
private:
frc::DifferentialDrive& m_rd;
};
// Distance in inches the robot wants to stay from an object
static constexpr int kHoldDistance = 12;
@@ -75,9 +60,8 @@ class Robot : public frc::TimedRobot {
frc::PWMVictorSPX m_left{kLeftMotorPort};
frc::PWMVictorSPX 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, m_pidOutput};
frc2::PIDController m_pidController{kP, kI, kD};
};
#ifndef RUNNING_FRC_TESTS

10
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -230,16 +230,6 @@
"foldername": "AxisCameraSample",
"gradlebase": "cpp"
},
{
"name": "GearsBot",
"description": "A fully functional example CommandBased program for WPIs GearsBot robot. This code can run on your computer if it supports simulation.",
"tags": [
"CommandBased Robot",
"Complete List"
],
"foldername": "GearsBot",
"gradlebase": "cpp"
},
{
"name": "GearsBotNew",
"description": "A fully functional example CommandBased program for WPIs GearsBot robot, using the new command-based framework. This code can run on your computer if it supports simulation.",

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -220,16 +220,6 @@
"gradlebase": "java"
,"mainclass": "Main"
},
{
"name": "GearsBot",
"description": "A fully functional example CommandBased program for WPIs GearsBot robot. This code can run on your computer if it supports simulation.",
"tags": [
"Complete Robot"
],
"foldername": "gearsbot",
"gradlebase": "java",
"mainclass": "Main"
},
{
"name": "GearsBot (New)",
"description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.",

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
View File

@@ -1,29 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all.
* Unless you know what you are doing, do not modify this file except to
* change the parameter class to the startRobot call.
*/
public final class Main {
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

72
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java
View File

@@ -1,72 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
/**
* This class is the glue that binds the controls on the physical operator
* interface to the commands and command groups that allow control of the robot.
*/
public class OI {
private final Joystick m_joystick = new Joystick(0);
/**
* Construct the OI and all of the buttons on it.
*/
public OI() {
// Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0));
SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2));
SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3));
SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0));
SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45));
SmartDashboard.putData("Open Claw", new OpenClaw());
SmartDashboard.putData("Close Claw", new CloseClaw());
SmartDashboard.putData("Deliver Soda", new Autonomous());
// Create some buttons
final JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
final JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
final JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
final JoystickButton l2 = new JoystickButton(m_joystick, 9);
final JoystickButton r2 = new JoystickButton(m_joystick, 10);
final JoystickButton l1 = new JoystickButton(m_joystick, 11);
final JoystickButton r1 = new JoystickButton(m_joystick, 12);
// Connect the buttons to commands
dpadUp.whenPressed(new SetElevatorSetpoint(0.2));
dpadDown.whenPressed(new SetElevatorSetpoint(-0.2));
dpadRight.whenPressed(new CloseClaw());
dpadLeft.whenPressed(new OpenClaw());
r1.whenPressed(new PrepareToPickup());
r2.whenPressed(new Pickup());
l1.whenPressed(new Place());
l2.whenPressed(new Autonomous());
}
public Joystick getJoystick() {
return m_joystick;
}
}

108
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
View File

@@ -1,108 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the TimedRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends TimedRobot {
Command m_autonomousCommand;
public static DriveTrain m_drivetrain;
public static Elevator m_elevator;
public static Wrist m_wrist;
public static Claw m_claw;
public static OI m_oi;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
// Initialize all subsystems
m_drivetrain = new DriveTrain();
m_elevator = new Elevator();
m_wrist = new Wrist();
m_claw = new Claw();
m_oi = new OI();
// instantiate the command used for the autonomous period
m_autonomousCommand = new Autonomous();
// Show what command your subsystem is running on the SmartDashboard
SmartDashboard.putData(m_drivetrain);
SmartDashboard.putData(m_elevator);
SmartDashboard.putData(m_wrist);
SmartDashboard.putData(m_claw);
}
@Override
public void autonomousInit() {
m_autonomousCommand.start(); // schedule the autonomous command (example)
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();
log();
}
@Override
public void 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();
}
/**
* This function is called periodically during teleoperated mode.
*/
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
log();
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
private void log() {
m_wrist.log();
m_elevator.log();
m_drivetrain.log();
m_claw.log();
}
}

32
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
* The main autonomous command to pickup and deliver the soda to the box.
*/
public class Autonomous extends CommandGroup {
/**
* Create a new autonomous command.
*/
public 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());
}
}

46
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
View File

@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Closes the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
public class CloseClaw extends Command {
public CloseClaw() {
requires(Robot.m_claw);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
Robot.m_claw.close();
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return Robot.m_claw.isGrabbing();
}
// Called once after isFinished returns true
@Override
protected void end() {
// NOTE: Doesn't stop in simulation due to lower friction causing the
// can to fall out
// + there is no need to worry about stalling the motor or crushing the
// can.
if (!Robot.isSimulation()) {
Robot.m_claw.stop();
}
}
}

62
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
View File

@@ -1,62 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Drive the given distance straight (negative values go backwards). Uses a
* local PID controller to run a simple PID loop that is only enabled while this
* command is running. The input is the averaged values of the left and right
* encoders.
*/
public class DriveStraight extends Command {
private final PIDController m_pid = new PIDController(4, 0, 0);
/**
* Create a new DriveStraight command.
* @param distance The distance to drive
*/
public DriveStraight(double distance) {
requires(Robot.m_drivetrain);
m_pid.setAbsoluteTolerance(0.01);
m_pid.setSetpoint(distance);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
// Get everything in a safe starting state.
Robot.m_drivetrain.reset();
m_pid.reset();
}
@Override
protected void execute() {
double pidOut = m_pid.calculate(Robot.m_drivetrain.getDistance());
Robot.m_drivetrain.drive(pidOut, pidOut);
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return m_pid.atSetpoint();
}
// Called once after isFinished returns true
@Override
protected void end() {
// Stop the wheels
Robot.m_drivetrain.drive(0, 0);
}
}

35
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
View File

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.TimedCommand;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Opens the claw for one second. Real robots should use sensors, stalling
* motors is BAD!
*/
public class OpenClaw extends TimedCommand {
public OpenClaw() {
super(1);
requires(Robot.m_claw);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
Robot.m_claw.open();
}
// Called once after isFinished returns true
@Override
protected void end() {
Robot.m_claw.stop();
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
View File

@@ -1,25 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
* Pickup a soda can (if one is between the open claws) and get it in a safe
* state to drive around.
*/
public class Pickup extends CommandGroup {
/**
* Create a new pickup command.
*/
public Pickup() {
addSequential(new CloseClaw());
addParallel(new SetWristSetpoint(-45));
addSequential(new SetElevatorSetpoint(0.25));
}
}

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
View File

@@ -1,24 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
* Place a held soda can onto the platform.
*/
public class Place extends CommandGroup {
/**
* Create a new place command.
*/
public Place() {
addSequential(new SetElevatorSetpoint(0.25));
addSequential(new SetWristSetpoint(0));
addSequential(new OpenClaw());
}
}

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
View File

@@ -1,24 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
* Make sure the robot is in a state to pickup soda cans.
*/
public class PrepareToPickup extends CommandGroup {
/**
* Create a new prepare to pickup command.
*/
public PrepareToPickup() {
addParallel(new OpenClaw());
addParallel(new SetWristSetpoint(0));
addSequential(new SetElevatorSetpoint(0));
}
}

62
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
View File

@@ -1,62 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* 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
* command is running. The input is the averaged values of the left and right
* encoders.
*/
public class SetDistanceToBox extends Command {
private final PIDController m_pid = new PIDController(-2, 0, 0);
/**
* Create a new set distance to box command.
* @param distance The distance away from the box to drive to
*/
public SetDistanceToBox(double distance) {
requires(Robot.m_drivetrain);
m_pid.setAbsoluteTolerance(0.01);
m_pid.setSetpoint(distance);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
// Get everything in a safe starting state.
Robot.m_drivetrain.reset();
m_pid.reset();
}
@Override
protected void execute() {
double pidOut = m_pid.calculate(Robot.m_drivetrain.getDistanceToObstacle());
Robot.m_drivetrain.drive(pidOut, pidOut);
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return m_pid.atSetpoint();
}
// Called once after isFinished returns true
@Override
protected void end() {
// Stop the wheels
Robot.m_drivetrain.drive(0, 0);
}
}

40
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
View File

@@ -1,40 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Move the elevator to a given location. This command finishes when it is
* within the tolerance, but leaves the PID loop running to maintain the
* position. Other commands using the elevator should make sure they disable
* PID!
*/
public class SetElevatorSetpoint extends Command {
private final double m_setpoint;
public SetElevatorSetpoint(double setpoint) {
m_setpoint = setpoint;
requires(Robot.m_elevator);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
Robot.m_elevator.enable();
Robot.m_elevator.setSetpoint(m_setpoint);
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return Robot.m_elevator.onTarget();
}
}

39
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Move the wrist to a given angle. This command finishes when it is within the
* tolerance, but leaves the PID loop running to maintain the position. Other
* commands using the wrist should make sure they disable PID!
*/
public class SetWristSetpoint extends Command {
private final double m_setpoint;
public SetWristSetpoint(double setpoint) {
m_setpoint = setpoint;
requires(Robot.m_wrist);
}
// Called just before this Command runs the first time
@Override
protected void initialize() {
Robot.m_wrist.enable();
Robot.m_wrist.setSetpoint(m_setpoint);
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return Robot.m_wrist.onTarget();
}
}

39
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.commands;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Have the robot drive tank style using the PS3 Joystick until interrupted.
*/
public class TankDriveWithJoystick extends Command {
public TankDriveWithJoystick() {
requires(Robot.m_drivetrain);
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
Robot.m_drivetrain.drive(Robot.m_oi.getJoystick());
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
return false; // Runs until interrupted
}
// Called once after isFinished returns true
@Override
protected void end() {
Robot.m_drivetrain.drive(0, 0);
}
}

70
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
View File

@@ -1,70 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
* The claw subsystem is a simple system with a motor for opening and closing.
* If using stronger motors, you should probably use a sensor so that the motors
* don't stall.
*/
public class Claw extends Subsystem {
private final Victor m_motor = new Victor(7);
private final DigitalInput m_contact = new DigitalInput(5);
/**
* Create a new claw subsystem.
*/
public Claw() {
super();
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Limit Switch", m_contact);
}
@Override
public void initDefaultCommand() {
}
public void log() {
}
/**
* Set the claw motor to move in the open direction.
*/
public void open() {
m_motor.set(-1);
}
/**
* Set the claw motor to move in the close direction.
*/
@Override
public void close() {
m_motor.set(1);
}
/**
* Stops the claw motor from moving.
*/
public void stop() {
m_motor.set(0);
}
/**
* Return true when the robot is grabbing an object hard enough to trigger
* the limit switch.
*/
public boolean isGrabbing() {
return m_contact.get();
}
}

146
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
View File

@@ -1,146 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDriveWithJoystick;
/**
* The DriveTrain subsystem incorporates the sensors and actuators attached to
* the robots chassis. These include four drive motors, a left and right encoder
* and a gyro.
*/
public class DriveTrain extends Subsystem {
private final SpeedController m_leftMotor
= new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
private final SpeedController m_rightMotor
= new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
private final DifferentialDrive m_drive
= new DifferentialDrive(m_leftMotor, m_rightMotor);
private final Encoder m_leftEncoder = new Encoder(1, 2);
private final Encoder m_rightEncoder = new Encoder(3, 4);
private final AnalogInput m_rangefinder = new AnalogInput(6);
private final AnalogGyro m_gyro = new AnalogGyro(1);
/**
* Create a new drive train subsystem.
*/
public DriveTrain() {
super();
// 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.
if (Robot.isReal()) {
m_leftEncoder.setDistancePerPulse(0.042);
m_rightEncoder.setDistancePerPulse(0.042);
} else {
// Circumference in ft = 4in/12(in/ft)*PI
m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
}
// Let's name the sensors on the LiveWindow
addChild("Drive", m_drive);
addChild("Left Encoder", m_leftEncoder);
addChild("Right Encoder", m_rightEncoder);
addChild("Rangefinder", m_rangefinder);
addChild("Gyro", m_gyro);
}
/**
* When no other command is running let the operator drive around using the
* PS3 joystick.
*/
@Override
public void initDefaultCommand() {
setDefaultCommand(new TankDriveWithJoystick());
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
}
/**
* Tank style driving for the DriveTrain.
*
* @param left Speed in range [-1,1]
* @param right Speed in range [-1,1]
*/
public void drive(double left, double right) {
m_drive.tankDrive(left, right);
}
/**
* Tank style driving for the DriveTrain.
*
* @param joy The ps3 style joystick to use to drive tank style.
*/
public void drive(Joystick joy) {
drive(-joy.getY(), -joy.getThrottle());
}
/**
* Get the robot's heading.
*
* @return The robots heading in degrees.
*/
public double getHeading() {
return m_gyro.getAngle();
}
/**
* Reset the robots sensors to the zero states.
*/
public void reset() {
m_gyro.reset();
m_leftEncoder.reset();
m_rightEncoder.reset();
}
/**
* Get the average distance of the encoders since the last reset.
*
* @return The distance driven (average of left and right encoders).
*/
public double getDistance() {
return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
}
/**
* Get the distance to the obstacle.
*
* @return The distance to the obstacle detected by the rangefinder.
*/
public double getDistanceToObstacle() {
// Really meters in simulation since it's a rangefinder...
return m_rangefinder.getAverageVoltage();
}
}

84
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
View File

@@ -1,84 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in
* it's current state PID values for simulation are different than in the real
* world do to minor differences.
*/
public class Elevator extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
private static final double kP_real = 4;
private static final double kI_real = 0.07;
private static final double kP_simulation = 18;
private static final double kI_simulation = 0.2;
/**
* Create a new elevator subsystem.
*/
public Elevator() {
super(kP_real, kI_real, 0);
if (Robot.isSimulation()) { // Check for simulation and update PID values
getPIDController().setPID(kP_simulation, kI_simulation, 0, 0);
}
setAbsoluteTolerance(0.005);
m_motor = new Victor(5);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(2, -2.0 / 5);
} else {
m_pot = new AnalogPotentiometer(2); // Defaults to meters
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
@Override
public void initDefaultCommand() {
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putData("Elevator Pot", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
@Override
protected double returnPIDInput() {
return m_pot.get();
}
/**
* Use the motor as the PID output. This method is automatically called by
* the subsystem.
*/
@Override
protected void usePIDOutput(double power) {
m_motor.set(power);
}
}

81
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
View File

@@ -1,81 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead
* of a linear joint.
*/
public class Wrist extends PIDSubsystem {
private final Victor m_motor;
private final AnalogPotentiometer m_pot;
private static final double kP_real = 1;
private static final double kP_simulation = 0.05;
/**
* Create a new wrist subsystem.
*/
public Wrist() {
super(kP_real, 0, 0);
if (Robot.isSimulation()) { // Check for simulation and update PID values
getPIDController().setPID(kP_simulation, 0, 0, 0);
}
setAbsoluteTolerance(2.5);
m_motor = new Victor(6);
// Conversion value of potentiometer varies between the real world and
// simulation
if (Robot.isReal()) {
m_pot = new AnalogPotentiometer(3, -270.0 / 5);
} else {
m_pot = new AnalogPotentiometer(3); // Defaults to degrees
}
// Let's name everything on the LiveWindow
addChild("Motor", m_motor);
addChild("Pot", m_pot);
}
@Override
public void initDefaultCommand() {
}
/**
* The log method puts interesting information to the SmartDashboard.
*/
public void log() {
SmartDashboard.putData("Wrist Angle", m_pot);
}
/**
* Use the potentiometer as the PID sensor. This method is automatically
* called by the subsystem.
*/
@Override
protected double returnPIDInput() {
return m_pot.get();
}
/**
* Use the motor as the PID output. This method is automatically called by
* the subsystem.
*/
@Override
protected void usePIDOutput(double power) {
m_motor.set(power);
}
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java
View File

@@ -9,8 +9,6 @@ package edu.wpi.first.wpilibj.examples.gearsbotnew;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java
View File

@@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot;
public class DriveTrain extends SubsystemBase {
/**

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java
View File

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot;
/**
* The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java
View File

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;
import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot;
/**
* The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.