mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
Initial commit
This commit is contained in:
C4llSqin
@@ -0,0 +1,156 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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 frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be
|
||||
* declared globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final String CANBUS_NAME = "rio";
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double MAX_ROT_SPEED = 3.5;
|
||||
public static final double AUTO_MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 1.0;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double[] GEARS = {0.25, 0.5, 1.0};
|
||||
|
||||
public static final double SLOW_SPEED = 0.25;
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
// public static List<CanDevice> CAN_DEVICES = new ArrayList<>();
|
||||
|
||||
public static final class DefaultSwerveRotOffsets {
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 2);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 3);
|
||||
public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 10);
|
||||
|
||||
public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 5);
|
||||
public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 11);
|
||||
|
||||
public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6);
|
||||
public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7);
|
||||
public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12);
|
||||
|
||||
public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8);
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4);
|
||||
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
|
||||
|
||||
public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
|
||||
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
|
||||
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
|
||||
|
||||
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
|
||||
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final class Conversions {
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
|
||||
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
|
||||
public static final double TICKS_PER_MOTOR_REV = 0.5;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 3.9;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
|
||||
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
|
||||
|
||||
public static final double TICK_TIME_TO_SECONDS = 10;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
}
|
||||
|
||||
public static final class DriveConstants {
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final int XBOX_PROGRAMMER_ID = 2;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 frc4388.robot;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,231 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Base64;
|
||||
import java.util.List;
|
||||
import java.util.logging.Level;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.CANBus.CANBusStatus;
|
||||
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
//import frc4388.robot.subsystems.LED;
|
||||
/**
|
||||
* 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 build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
private RobotContainer m_robotContainer;
|
||||
//private LED mled = new LED();
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
|
||||
|
||||
new Thread() {
|
||||
public void run() {
|
||||
try{
|
||||
while(!this.isInterrupted() && this.isAlive()){
|
||||
Thread.sleep(500);
|
||||
for(int i=0;i<Subsystem.subsystems.size(); i++){
|
||||
Subsystem.subsystems.get(i).queryStatus();
|
||||
}
|
||||
|
||||
System.out.println("Updated statuses!");
|
||||
|
||||
}
|
||||
}catch(Exception e){
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}.start();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.doubl
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
//System.out.println(m_robotContainer.limelight.isNearSpeaker());
|
||||
//mled.updateLED();
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
* the robot is disabled.
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotTime.endMatchTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledExit() {
|
||||
DeferredBlock.execute();
|
||||
}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
* = new MyAutoCommand(); break; case "Default Auto": default:
|
||||
* autonomousCommand = new ExampleCommand(); break; }
|
||||
*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
@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.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
|
||||
List<String> errors = new ArrayList<>();
|
||||
|
||||
// Subsystems header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
|
||||
for(int i=0;i< Subsystem.subsystems.size();i++){
|
||||
|
||||
Subsystem subsystem = Subsystem.subsystems.get(i);
|
||||
System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
|
||||
Status status = subsystem.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(subsystem.getName() + " - " + r.toString());
|
||||
subsystem.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ==")));
|
||||
|
||||
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
|
||||
|
||||
CANBusStatus canInfo = canBus.getStatus();
|
||||
|
||||
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
|
||||
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
|
||||
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
|
||||
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
|
||||
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
|
||||
// Broken turniary operator
|
||||
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
|
||||
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
|
||||
if(canReportLevel == ReportLevel.ERROR) {
|
||||
errors.add(canStatus);
|
||||
}
|
||||
System.out.println(canStatus);
|
||||
|
||||
for(int i=0;i<CanDevice.devices.size();i++){
|
||||
|
||||
CanDevice device = CanDevice.devices.get(i);
|
||||
System.out.println("** CAN diagnostic report for " + device.name + ":");
|
||||
Status status = device.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(device.getName() + " - " + r.toString());
|
||||
device.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
|
||||
|
||||
if(errors.size() > 0) {
|
||||
// Errors header
|
||||
System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
for(int i=0;i<errors.size(); i++){
|
||||
System.out.println(errors.get(i));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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 frc4388.robot;
|
||||
|
||||
// Drive Systems
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
|
||||
// Autos
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
|
||||
// Subsystems
|
||||
// import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
// Utilites
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.configurable.ConfigurableString;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
// private final LED m_robotLED = new LED();
|
||||
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
|
||||
m_robotMap.rightFront,
|
||||
m_robotMap.leftBack,
|
||||
m_robotMap.rightBack,
|
||||
|
||||
m_robotMap.gyro);
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
|
||||
|
||||
/* Virtual Controllers */
|
||||
private final VirtualController m_virtualDriver = new VirtualController(0);
|
||||
private final VirtualController m_virtualOperator = new VirtualController(1);
|
||||
|
||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||
|
||||
// ! Teleop Commands
|
||||
|
||||
// ! /* Autos */
|
||||
private String lastAutoName = "defualt.auto";
|
||||
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
() -> autoplaybackName.get(), // lastAutoName
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false);
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
configureVirtualButtonBindings();
|
||||
new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// CameraServer.startAutomaticCapture();
|
||||
|
||||
/* Default Commands */
|
||||
// ! Swerve Drive Default Command (Regular Rotation)
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
m_robotSwerveDrive.setToSlow();
|
||||
|
||||
// this.subsystems.add(m_robotSwerveDrive);
|
||||
// this.subsystems.add(m_robotMap.leftFront);
|
||||
// this.subsystems.add(m_robotMap.rightFront);
|
||||
// this.subsystems.add(m_robotMap.rightBack);
|
||||
// this.subsystems.add(m_robotMap.leftBack);
|
||||
|
||||
// ! Swerve Drive One Module Test
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
|
||||
// }
|
||||
|
||||
// ! Swerve Drive Default Command (Orientation Rotation)
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(),
|
||||
// getDeadbandedDriverController().getRightX(),
|
||||
// getDeadbandedDriverController().getRightY(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive))
|
||||
// .withName("SwerveDrive OrientationCommand"));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotSwerveDrive.driveWithInput(
|
||||
// getDeadbandedDriverController().getLeft(),
|
||||
// getDeadbandedDriverController().getRight(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive));
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
|
||||
// ? /* Driver Buttons */
|
||||
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
// * /* Auto Recording */
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
() -> autoplaybackName.get()))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
() -> autoplaybackName.get(),
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false))
|
||||
.onFalse(new InstantCommand());
|
||||
}
|
||||
|
||||
/**
|
||||
* This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
|
||||
* Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
|
||||
*/
|
||||
private void configureVirtualButtonBindings() {
|
||||
|
||||
// ? /* Driver Buttons */
|
||||
|
||||
/* Notice: the following buttons have not been replicated
|
||||
* Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
|
||||
* Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode.
|
||||
* Auto Recording controls : We don't want an Null Ouroboros for an auto.
|
||||
*/
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
/* Notice: the following buttons have not been replicated
|
||||
* Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
|
||||
* We don't need it in an auto.
|
||||
* Climbing controls : We don't need to climb in auto.
|
||||
*/
|
||||
|
||||
// ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
return autoPlayback;
|
||||
}
|
||||
|
||||
/**
|
||||
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
|
||||
* @param joystickA A controller
|
||||
* @param joystickB A controller
|
||||
* @param buttonNumber The button to bind to
|
||||
*/
|
||||
public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
|
||||
return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedDriverController() {
|
||||
return this.m_driverXbox;
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedOperatorController() {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualDriverController() {
|
||||
return m_virtualDriver;
|
||||
}
|
||||
|
||||
public VirtualController getVirtualOperatorController() {
|
||||
return m_virtualOperator;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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 frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
|
||||
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
// import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightBack;
|
||||
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
/* Swreve Drive Subsystem */
|
||||
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id);
|
||||
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id);
|
||||
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER.id);
|
||||
|
||||
|
||||
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL.id);
|
||||
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER.id);
|
||||
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER.id);
|
||||
|
||||
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL.id);
|
||||
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER.id);
|
||||
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER.id);
|
||||
|
||||
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL.id);
|
||||
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER.id);
|
||||
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER.id);
|
||||
|
||||
void configureDriveMotorControllers() {
|
||||
// initialize SwerveModules
|
||||
this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
|
||||
this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
|
||||
this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
|
||||
this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,225 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,12
|
||||
0.0,0.0,0.0,0.0,26
|
||||
0.0,0.0,0.0,0.0,37
|
||||
0.0,0.0,0.0,0.0,50
|
||||
0.0,0.0,0.0,0.0,62
|
||||
0.0,0.0,0.0,0.0,73
|
||||
0.0,0.0,0.0,0.0,88
|
||||
0.0,0.0,0.0,0.0,103
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,160
|
||||
0.0,0.0,0.0,0.0,173
|
||||
0.0,0.0,0.0,0.0,185
|
||||
0.0,0.0,0.0,0.0,200
|
||||
0.0,0.0,0.0,0.0,211
|
||||
0.0,0.0,0.0,0.0,223
|
||||
0.0,0.0,0.0,0.0,235
|
||||
0.0,0.0,0.0,0.0,247
|
||||
0.0,0.0,0.0,0.0,263
|
||||
0.0,0.0,0.0,0.0,283
|
||||
0.0,0.0,0.0,0.0,303
|
||||
0.0,-0.109375,0.0,0.0,323
|
||||
0.0,-0.1484375,0.0,0.0,343
|
||||
0.0,-0.2109375,0.0,0.0,363
|
||||
0.0,-0.3671875,0.0,0.0,398
|
||||
0.0,-0.4140625,0.0,0.0,411
|
||||
0.0,-0.4765625,0.0,0.0,425
|
||||
0.0,-0.5078125,0.0,0.0,443
|
||||
0.0,-0.5078125,0.0,0.0,463
|
||||
0.0,-0.53125,0.0,0.0,483
|
||||
0.0,-0.5546875,0.0,0.0,504
|
||||
0.0,-0.5625,0.0,0.0,523
|
||||
0.0,-0.5625,0.0,0.0,544
|
||||
0.0,-0.5703125,0.0,0.0,563
|
||||
0.0,-0.5859375,0.0,0.0,584
|
||||
0.0,-0.5859375,0.0,0.0,603
|
||||
0.0,-0.5859375,0.0,0.0,640
|
||||
0.0,-0.59375,0.0,0.0,657
|
||||
0.0,-0.6015625,0.0,0.0,672
|
||||
0.0,-0.6015625,0.0,0.0,685
|
||||
0.0,-0.6015625,0.0,0.0,703
|
||||
0.0,-0.6015625,0.0,0.0,723
|
||||
0.0,-0.6015625,0.0,0.0,743
|
||||
0.0,-0.6015625,0.0,0.0,763
|
||||
0.0,-0.6015625,0.0,0.0,783
|
||||
0.0,-0.6015625,0.0,0.0,803
|
||||
0.0,-0.6015625,0.0,0.0,823
|
||||
0.0,-0.6015625,0.0,0.0,844
|
||||
0.0,-0.6015625,0.0,0.0,878
|
||||
0.0,-0.6015625,0.0,0.0,893
|
||||
0.0,-0.6015625,0.0,0.0,907
|
||||
0.0,-0.6015625,0.0,0.0,924
|
||||
0.0,-0.609375,0.0,0.0,943
|
||||
0.0,-0.609375,0.0,0.0,963
|
||||
0.0,-0.609375,0.0,0.0,983
|
||||
0.0,-0.609375,0.0,0.0,1004
|
||||
0.0,-0.609375,0.0,0.0,1023
|
||||
0.0,-0.609375,0.0,0.0,1043
|
||||
0.0,-0.609375,0.0,0.0,1064
|
||||
0.0,-0.609375,0.0,0.0,1083
|
||||
0.0,-0.609375,0.0,0.0,1156
|
||||
0.0,-0.609375,0.0,0.0,1172
|
||||
0.0,-0.609375,0.0,0.0,1185
|
||||
0.0,-0.609375,0.0,0.0,1200
|
||||
0.0,-0.609375,0.0,0.0,1215
|
||||
0.0,-0.609375,0.0,0.0,1225
|
||||
0.0,-0.609375,0.0,0.0,1236
|
||||
0.0,-0.609375,0.0,0.0,1249
|
||||
0.0,-0.609375,0.0,0.0,1263
|
||||
0.0,-0.609375,0.0,0.0,1283
|
||||
0.0,-0.609375,0.0,0.0,1303
|
||||
0.0,-0.609375,0.0,0.0,1323
|
||||
0.0,-0.609375,0.0,0.0,1363
|
||||
0.0,-0.6015625,0.0,0.0,1376
|
||||
0.0,-0.6015625,0.0,0.0,1394
|
||||
0.0,-0.6015625,0.0,0.0,1405
|
||||
0.0,-0.6015625,0.0,0.0,1423
|
||||
0.0,-0.6015625,0.0,0.0,1443
|
||||
0.0,-0.6015625,0.0,0.0,1463
|
||||
0.0,-0.6015625,0.0,0.0,1483
|
||||
0.0,-0.6015625,0.0,0.0,1503
|
||||
0.0,-0.6015625,0.0,0.0,1523
|
||||
0.0,-0.6015625,0.0,0.0,1543
|
||||
0.0,-0.6015625,0.0,0.0,1563
|
||||
0.0,-0.6015625,0.0,0.0,1597
|
||||
0.0,-0.6015625,0.0,0.0,1608
|
||||
0.0,-0.6015625,0.0,0.0,1624
|
||||
0.0,-0.6015625,0.0,0.0,1643
|
||||
0.0,-0.6015625,0.0,0.0,1664
|
||||
0.0,-0.5859375,0.0,0.0,1683
|
||||
0.0,-0.5859375,0.0,0.0,1703
|
||||
0.0,-0.5625,0.0,0.0,1723
|
||||
0.0,-0.5625,0.0,0.0,1743
|
||||
0.0,-0.5625,0.0,0.0,1763
|
||||
0.0,-0.5625,0.0,0.0,1783
|
||||
0.0,-0.5625,0.0,0.0,1803
|
||||
0.0,-0.5625,0.0,0.0,1843
|
||||
0.0,-0.5625,0.0,0.0,1855
|
||||
0.0,-0.5625,0.0,0.0,1868
|
||||
0.0,-0.5625,0.0,0.0,1883
|
||||
0.0,-0.5625,0.0,0.0,1903
|
||||
0.0,-0.5625,0.0,0.0,1923
|
||||
0.0,-0.5625,0.0,0.0,1943
|
||||
0.0,-0.5625,0.0,0.0,1963
|
||||
0.0,-0.5625,0.0,0.0,1983
|
||||
0.0,-0.5625,0.0,0.0,2003
|
||||
0.0,-0.5625,0.0,0.0,2024
|
||||
0.0,-0.5625,0.0,0.0,2043
|
||||
0.0,-0.5625,0.0,0.0,2081
|
||||
0.0,-0.5625,0.0,0.0,2093
|
||||
0.0,-0.5625,0.0,0.0,2105
|
||||
0.0,-0.5625,0.0,0.0,2123
|
||||
0.0,-0.5625,0.0,0.0,2143
|
||||
0.0,-0.5625,0.0,0.0,2163
|
||||
0.0,-0.5625,0.0,0.0,2183
|
||||
0.0,-0.5625,0.0,0.0,2203
|
||||
0.0,-0.5625,0.0,0.0,2223
|
||||
0.0,-0.5625,0.0,0.0,2243
|
||||
0.0,-0.5625,0.0,0.0,2263
|
||||
0.0,-0.5625,0.0,0.0,2283
|
||||
0.0,-0.5625,0.0,0.0,2366
|
||||
0.0,-0.5625,0.0,0.0,2377
|
||||
0.0,-0.5625,0.0,0.0,2394
|
||||
0.0,-0.5703125,0.0,0.0,2405
|
||||
0.0,-0.5703125,0.0,0.0,2418
|
||||
0.0,-0.5703125,0.0,0.0,2431
|
||||
0.0,-0.5703125,0.0,0.0,2444
|
||||
0.0,-0.5703125,0.0,0.0,2458
|
||||
0.0,-0.5703125,0.0,0.0,2470
|
||||
0.0,-0.5703125,0.0,0.0,2485
|
||||
0.0,-0.5703125,0.0,0.0,2503
|
||||
0.0,-0.5703125,0.0,0.0,2523
|
||||
0.0,-0.5703125,0.0,0.0,2563
|
||||
0.0,-0.5703125,0.0,0.0,2577
|
||||
0.0,-0.5703125,0.0,0.0,2591
|
||||
0.0,-0.5703125,0.0,0.0,2608
|
||||
0.0,-0.5703125,0.0,0.0,2624
|
||||
0.0,-0.5703125,0.0,0.0,2643
|
||||
0.0,-0.5703125,0.0,0.0,2677
|
||||
0.0,-0.5703125,0.0,0.0,2698
|
||||
0.0,-0.5703125,0.0,0.0,2711
|
||||
0.0,-0.5703125,0.0,0.0,2725
|
||||
0.0,-0.5703125,0.0,0.0,2743
|
||||
0.0,-0.5703125,0.0,0.0,2764
|
||||
0.0,-0.5703125,0.0,0.0,2810
|
||||
0.0,-0.5703125,0.0,0.0,2820
|
||||
0.0,-0.5703125,0.0,0.0,2833
|
||||
0.0,-0.5703125,0.0,0.0,2845
|
||||
0.0,-0.5703125,0.0,0.0,2863
|
||||
0.0,-0.5703125,0.0,0.0,2883
|
||||
0.0,-0.5703125,0.0,0.0,2904
|
||||
0.0,-0.5703125,0.0,0.0,2924
|
||||
0.0,-0.5703125,0.0,0.0,2943
|
||||
0.0,-0.5703125,0.0,0.0,2963
|
||||
0.0,-0.5703125,0.0,0.0,2983
|
||||
0.0,-0.5703125,0.0,0.0,3003
|
||||
0.0,-0.5703125,0.0,0.0,3033
|
||||
0.0,-0.5703125,0.0,0.0,3050
|
||||
0.0,-0.5703125,0.0,0.0,3065
|
||||
0.0,-0.5703125,0.0,0.0,3083
|
||||
0.0,-0.5703125,0.0,0.0,3103
|
||||
0.0,-0.5703125,0.0,0.0,3123
|
||||
0.0,-0.5703125,0.0,0.0,3144
|
||||
0.0,-0.5703125,0.0,0.0,3164
|
||||
0.0,-0.5703125,0.0,0.0,3184
|
||||
0.0,-0.5703125,0.0,0.0,3203
|
||||
0.0,-0.5703125,0.0,0.0,3223
|
||||
0.0,-0.5703125,0.0,0.0,3243
|
||||
0.0,-0.5703125,0.0,0.0,3272
|
||||
0.0,-0.5703125,0.0,0.0,3289
|
||||
0.0,-0.5703125,0.0,0.0,3303
|
||||
0.0,-0.5703125,0.0,0.0,3323
|
||||
0.0,-0.5703125,0.0,0.0,3343
|
||||
0.0,-0.5703125,0.0,0.0,3363
|
||||
0.0,-0.5703125,0.0,0.0,3383
|
||||
0.0,-0.5703125,0.0,0.0,3403
|
||||
0.0,-0.5703125,0.0,0.0,3423
|
||||
0.0,-0.5703125,0.0,0.0,3443
|
||||
0.0,-0.5703125,0.0,0.0,3463
|
||||
0.0,-0.5703125,0.0,0.0,3483
|
||||
0.0,-0.5703125,0.0,0.0,3566
|
||||
0.0,-0.5703125,0.0,0.0,3578
|
||||
0.0,-0.5703125,0.0,0.0,3596
|
||||
0.0,-0.5703125,0.0,0.0,3610
|
||||
0.0,-0.5703125,0.0,0.0,3623
|
||||
0.0,-0.5703125,0.0,0.0,3640
|
||||
0.0,-0.5703125,0.0,0.0,3651
|
||||
0.0,-0.5703125,0.0,0.0,3663
|
||||
0.0,-0.5703125,0.0,0.0,3678
|
||||
0.0,-0.5703125,0.0,0.0,3691
|
||||
0.0,-0.5703125,0.0,0.0,3706
|
||||
0.0,-0.5703125,0.0,0.0,3723
|
||||
0.0,-0.5703125,0.0,0.0,3766
|
||||
0.0,-0.5703125,0.0,0.0,3778
|
||||
0.0,-0.5703125,0.0,0.0,3792
|
||||
0.0,-0.5703125,0.0,0.0,3807
|
||||
0.0,-0.5703125,0.0,0.0,3823
|
||||
0.0,-0.5703125,0.0,0.0,3843
|
||||
0.0,-0.53125,0.0,0.0,3863
|
||||
0.0,-0.53125,0.0,0.0,3884
|
||||
0.0,-0.421875,0.0,0.0,3904
|
||||
0.0,0.0,0.0,0.0,3924
|
||||
0.0,0.0,0.0,0.0,3944
|
||||
0.0,0.0,0.0,0.0,3963
|
||||
0.0,0.0,0.0,0.0,3999
|
||||
0.0,0.0,0.0,0.0,4010
|
||||
0.0,0.0,0.0,0.0,4025
|
||||
0.0,0.0,0.0,0.0,4043
|
||||
0.0,0.0,0.0,0.0,4063
|
||||
0.0,0.0,0.0,0.0,4083
|
||||
0.0,0.0,0.0,0.0,4103
|
||||
0.0,0.0,0.0,0.0,4123
|
||||
0.0,0.0,0.0,0.0,4143
|
||||
0.0,0.0,0.0,0.0,4163
|
||||
0.0,0.0,0.0,0.0,4183
|
||||
0.0,0.0,0.0,0.0,4203
|
||||
0.0,0.0,0.0,0.0,4236
|
||||
0.0,0.0,0.0,0.0,4247
|
||||
0.0,0.0,0.0,0.0,4264
|
||||
0.0,0.0,0.0,0.0,4284
|
||||
0.0,0.0,0.0,0.0,4304
|
||||
0.0,0.0,0.0,0.0,4324
|
||||
0.0,0.0,0.0,0.0,4343
|
||||
0.0,0.0,0.0,0.0,4363
|
||||
@@ -0,0 +1,20 @@
|
||||
AUTO file format
|
||||
|
||||
HEADER static size 0x5
|
||||
0x00 BYTE NUM AXES: defualts to 6
|
||||
0x01 BYTE NUM POV: defualts to 1
|
||||
0x02 BYTE NUM CONTROLLERS: defualts to 2
|
||||
0x03 SHORT FRAMES: any value greator or equal than one.
|
||||
|
||||
FRAME PER CONTROLLER: defualt size 0x34
|
||||
0x00 DOUBLE AXES[NUM AXES]
|
||||
0x30 SHORT BUTTONS
|
||||
0x32 SHORT POVs[NUM POV]
|
||||
|
||||
FRAME: size varrys
|
||||
FRAME PER CONTROLLER[NUM CONTROLLERS]
|
||||
INT UNIXTIMESTAMP
|
||||
|
||||
FILE:
|
||||
HEADER
|
||||
FRAME[FRAMES]
|
||||
@@ -0,0 +1,107 @@
|
||||
// package frc4388.robot.commands.Autos;
|
||||
|
||||
// import java.io.File;
|
||||
// import java.util.ArrayList;
|
||||
// import java.util.HashMap;
|
||||
|
||||
// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
|
||||
// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
// import edu.wpi.first.wpilibj2.command.Command;
|
||||
// import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
// import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
// import frc4388.robot.subsystems.SwerveDrive;
|
||||
// import frc4388.utility.controller.VirtualController;
|
||||
|
||||
// public class neoPlaybackChooser {
|
||||
// private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();
|
||||
// private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
|
||||
// private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
|
||||
|
||||
// private final SwerveDrive m_swerve;
|
||||
// private final VirtualController[] m_controllers;
|
||||
// // private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
|
||||
// // private SendableChooser<Command> m_playback = null;
|
||||
// private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
|
||||
// // private final HashMap<String, Command> m_commandPool = new HashMap<>();
|
||||
|
||||
// // private final File m_dir = new File("/home/lvuser/autos/");
|
||||
// // private int m_cmdNum = 0;
|
||||
|
||||
// // commands
|
||||
// private Command m_noAuto = new InstantCommand();
|
||||
|
||||
// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
|
||||
// m_teamChosser.addOption("Red", "red");
|
||||
// m_teamChosser.setDefaultOption("Blue", "blue");
|
||||
// m_teamChosser.addOption("Nuetral", "nuetral");
|
||||
// m_possitionChosser.addOption("AMP", "amp");
|
||||
// m_possitionChosser.setDefaultOption("Center", "center");
|
||||
// m_possitionChosser.addOption("Source", "source");
|
||||
// m_swerve = swerve;
|
||||
// m_controllers = controllers;
|
||||
// }
|
||||
|
||||
// public neoPlaybackChooser addOption(String name, String option) {
|
||||
// m_autoNameChosser.addOption(name, option);
|
||||
// return this;
|
||||
// }
|
||||
|
||||
// // public PlaybackChooser buildDisplay() {
|
||||
// // for (int i = 0; i < 10; i++) {
|
||||
// // appendCommand();
|
||||
// // }
|
||||
// // m_playback = m_choosers.get(0);
|
||||
// // nextChooser();
|
||||
|
||||
// // // ! This does not work, why?
|
||||
// // Shuffleboard.getTab("Auto Chooser")
|
||||
// // .add("Add Sequence", new InstantCommand(() -> nextChooser()))
|
||||
// // .withPosition(4, 0);
|
||||
// // return this;
|
||||
// // }
|
||||
|
||||
// // This will be bound to a button for the time being
|
||||
// public void render() {
|
||||
// // var chooser = new SendableChooser<Command>();
|
||||
// // // chooser.setDefaultOption("No Auto", m_noAuto);
|
||||
|
||||
// // m_choosers.add(chooser);
|
||||
// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
|
||||
// .add("Command: " + m_choosers.size(), chooser)
|
||||
// .withSize(4, 1)
|
||||
// .withPosition(0, m_choosers.size() - 1)
|
||||
// .withWidget(BuiltInWidgets.kSplitButtonChooser)
|
||||
// .withWidget(BuiltInWidgets.kComboBoxChooser);
|
||||
|
||||
|
||||
// m_widgets.add(widget);
|
||||
// }
|
||||
|
||||
// // public void nextChooser() {
|
||||
// // SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
|
||||
|
||||
// // String[] dirs = m_dir.list();
|
||||
|
||||
// // if(dirs != null){ // Fix funny error
|
||||
// // for (String auto : dirs) {
|
||||
// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
// // }
|
||||
// // }
|
||||
|
||||
|
||||
// // for (var cmd_name : m_commandPool.keySet()) {
|
||||
// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
// // }
|
||||
// // }
|
||||
|
||||
// public String autoName() {
|
||||
// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
|
||||
// }
|
||||
|
||||
// public Command getCommand() {
|
||||
// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
|
||||
// }
|
||||
// }
|
||||
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public abstract class PID extends Command {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
private double tolerance = 0;
|
||||
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PID(double kp, double ki, double kd, double kf, double tolerance) {
|
||||
gains = new Gains(kp, ki, kd, kf, 0);
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
public PID(Gains gains, double tolerance) {
|
||||
this.gains = gains;
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
/** produces the error from the setpoint */
|
||||
public abstract double getError();
|
||||
|
||||
/** todo: javadoc */
|
||||
public abstract void runWithOutput(double output);
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public final void initialize() {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
private double prevError, cumError = 0;
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public final void execute() {
|
||||
double error = getError();
|
||||
cumError += error * .02; // 20 ms
|
||||
double delta = error - prevError;
|
||||
|
||||
output = error * gains.kP;
|
||||
output += cumError * gains.kI;
|
||||
output += delta * gains.kD;
|
||||
output += gains.kF;
|
||||
|
||||
runWithOutput(output);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
return Math.abs(getError()) < tolerance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PID;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PID {
|
||||
|
||||
SwerveDrive drive;
|
||||
double targetAngle;
|
||||
|
||||
/** Creates a new RotateToAngle. */
|
||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.targetAngle = targetAngle;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,197 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileInputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
|
||||
|
||||
/**
|
||||
* The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickPlayback extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final VirtualController[] controllers;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
private final Supplier<String> filenameGetter;
|
||||
private String filename;
|
||||
private int frame_index = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
private byte m_numAxes = 0;
|
||||
private byte m_numPOVs = 0;
|
||||
private byte m_numControllers = 0;
|
||||
private short m_numFrames = -1;
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree Unloads the auto on compleation or intruption.
|
||||
* @param instantload Load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this.swerve = swerve;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.controllers = controllers;
|
||||
this.m_shouldfree = shouldfree;
|
||||
|
||||
if (instantload) loadAuto();
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param shouldfree unloads the auto on compleation or intruption.
|
||||
* @param instantload load the auto on object instantiation
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
|
||||
this(swerve, () -> filename, controllers, shouldfree, instantload);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
|
||||
this(swerve, filenameGetter, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
*/
|
||||
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
|
||||
this(swerve, () -> filename, controllers, true, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Load the auto file from disk into memory
|
||||
* @return Returns true if loading was successful, else wise; return false
|
||||
* @implNote if the auto is already loaded, it will return true.
|
||||
*/
|
||||
public boolean loadAuto() {
|
||||
filename = filenameGetter.get();
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
||||
return true;
|
||||
}
|
||||
|
||||
m_numAxes = stream.readNBytes(1)[0];
|
||||
m_numPOVs = stream.readNBytes(1)[0];
|
||||
m_numControllers = stream.readNBytes(1)[0];
|
||||
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
|
||||
if (m_numControllers > controllers.length) {
|
||||
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
|
||||
+ " virtual controllers but only " + controllers.length + " were given");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < m_numFrames; i++) {
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
for (int j = 0; j < m_numControllers; j++) {
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = new double[m_numAxes];
|
||||
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
|
||||
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
|
||||
}
|
||||
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
short[] POV = new short[m_numPOVs];
|
||||
for (int k = 0; k < m_numPOVs; k++) {
|
||||
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
|
||||
}
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[j] = controllerFrame;
|
||||
}
|
||||
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
|
||||
frames.add(frame);
|
||||
}
|
||||
|
||||
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
|
||||
return true;
|
||||
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Unloads the auto.
|
||||
*/
|
||||
public void unloadAuto() {
|
||||
System.out.println("AUTOPLAYBACK: Auto unloaded");
|
||||
frames.clear();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
frame_index = 0;
|
||||
|
||||
m_finished = !loadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
if (frame_index >= m_numFrames) m_finished = true;
|
||||
if (m_finished) return;
|
||||
|
||||
// if (frame_index == 0) {
|
||||
// startTime = System.currentTimeMillis();
|
||||
// playbackTime = 0;
|
||||
// } else {
|
||||
// playbackTime = System.currentTimeMillis() - startTime;
|
||||
// }
|
||||
|
||||
AutoRecordingFrame frame = frames.get(frame_index);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
|
||||
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
|
||||
if (i == 0) {
|
||||
this.swerve.driveWithInput(
|
||||
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
|
||||
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
|
||||
true);
|
||||
}
|
||||
}
|
||||
frame_index++;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
for (VirtualController controller : controllers) controller.zeroControls();
|
||||
swerve.stopModules();
|
||||
if (m_shouldfree) unloadAuto();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
package frc4388.robot.commands.Swerve;
|
||||
|
||||
import java.io.FileOutputStream;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.DataUtils;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
* The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class neoJoystickRecorder extends Command {
|
||||
private final SwerveDrive swerve;
|
||||
private final XboxController[] controllers;
|
||||
private String filename;
|
||||
private final Supplier<String> filenameGetter;
|
||||
private long startTime = -1;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
|
||||
this.swerve = swerve;
|
||||
this.controllers = controllers;
|
||||
this.filenameGetter = filenameGetter;
|
||||
this.filename = "";
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an new NEO Joystick Playback with specifyed pramiters.
|
||||
* @param swerve m_robotSwerveDrive
|
||||
* @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
|
||||
* @param filename a String containing the name of the auto file you wish to playback.
|
||||
*/
|
||||
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
|
||||
this(swerve, controllers, () -> filename);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
frames.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
|
||||
frames.add(frame);
|
||||
this.filename = this.filenameGetter.get();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
System.out.println("AUTORECORD: RECORDING");
|
||||
AutoRecordingFrame frame = new AutoRecordingFrame();
|
||||
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
|
||||
for (int i = 0; i < controllers.length; i++) {
|
||||
XboxController controller = controllers[i];
|
||||
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
|
||||
double[] axes = {controller.getLeftX(), controller.getLeftY(),
|
||||
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
|
||||
controller.getRightX(), controller.getRightY()};
|
||||
short button = 0;
|
||||
for (int j = 0; j < 10; j++)
|
||||
if (controller.getRawButton(j+1))
|
||||
button |= 1 << j;
|
||||
short[] POV = {(short)(controller.getPOV())};
|
||||
controllerFrame.axes = axes;
|
||||
controllerFrame.button = button;
|
||||
controllerFrame.POV = POV;
|
||||
frame.controllerFrames[i] = controllerFrame;
|
||||
}
|
||||
|
||||
frames.add(frame);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
|
||||
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
|
||||
true); // Really jank way of doing this.
|
||||
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
||||
// header: size of 0x5
|
||||
// byte Number of axes per controller
|
||||
// byte Number of POVs per controller
|
||||
// byte Number of controllers
|
||||
// short Number of frames
|
||||
stream.write(new byte[]{6, 1, (byte) controllers.length});
|
||||
stream.write(DataUtils.shortToByteArray((short) frames.size()));
|
||||
|
||||
// frame
|
||||
// controller frame * number of controllers
|
||||
// int unix time stamp.
|
||||
for (AutoRecordingFrame frame : frames) {
|
||||
// controller frame
|
||||
// double axis * Number of axes per controller
|
||||
// short button states
|
||||
// short POV * Number of POVs per controller
|
||||
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
|
||||
for (double axis: controllerFrame.axes) {
|
||||
stream.write(DataUtils.doubleToByteArray(axis));
|
||||
}
|
||||
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
|
||||
for (short POV: controllerFrame.POV) {
|
||||
stream.write(DataUtils.shortToByteArray(POV));
|
||||
}
|
||||
}
|
||||
stream.write(DataUtils.intToByteArray(frame.timeStamp));
|
||||
}
|
||||
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class DiffDrive extends Subsystem {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private TalonFX m_leftFrontMotor;
|
||||
private TalonFX m_rightFrontMotor;
|
||||
private TalonFX m_leftBackMotor;
|
||||
private TalonFX m_rightBackMotor;
|
||||
private DifferentialDrive m_driveTrain;
|
||||
private RobotGyro m_gyro;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
|
||||
|
||||
super();
|
||||
|
||||
m_leftFrontMotor = leftFrontMotor;
|
||||
m_rightFrontMotor = rightFrontMotor;
|
||||
m_leftBackMotor = leftBackMotor;
|
||||
m_rightBackMotor = rightBackMotor;
|
||||
m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
|
||||
m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
|
||||
m_driveTrain = driveTrain;
|
||||
m_gyro = gyro;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_gyro.updatePigeonDeltas();
|
||||
|
||||
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
|
||||
updateSmartDashboard();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void tankDriveWithInput(double leftMove, double rightMove) {
|
||||
m_leftFrontMotor.set(leftMove);
|
||||
m_rightFrontMotor.set(rightMove);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
private void updateSmartDashboard() {
|
||||
|
||||
// Examples of the functionality of RobotGyro
|
||||
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
|
||||
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
|
||||
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
|
||||
//SmartDashboard.putData(m_gyro);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Diff Drive";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
// TODO: Add Stuff
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Log("Diagnostic info for this has not been inplemented!"); //TODO
|
||||
return new Status();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.AddressableLED;
|
||||
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends SubsystemBase {
|
||||
|
||||
static AddressableLED m_led;
|
||||
static AddressableLEDBuffer m_ledBuffer;
|
||||
static LED m_self;
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
|
||||
public LED(){
|
||||
if(m_self != null)
|
||||
return;
|
||||
m_led = new AddressableLED(9);
|
||||
m_ledBuffer = new AddressableLEDBuffer(10);
|
||||
m_led.setLength(m_ledBuffer.getLength());
|
||||
m_led.setData(m_ledBuffer);
|
||||
m_led.start();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
public static LED getInstance() {
|
||||
if(m_self == null)
|
||||
m_self = new LED();
|
||||
return m_self;
|
||||
}
|
||||
@Override
|
||||
public void periodic(){
|
||||
//gamermode();
|
||||
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
return;
|
||||
}
|
||||
static int firstcolor = 0;
|
||||
static void gamermode() {
|
||||
for(int i = 0; i < m_ledBuffer.getLength(); i++) {
|
||||
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
|
||||
setLEDHSV(i, hue, 255, 128);
|
||||
}
|
||||
firstcolor +=3;
|
||||
firstcolor %= 180;
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public static void updateLED (){
|
||||
gamermode();
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public static void setLEDRGB(int lednum, int r, int g, int b){
|
||||
m_ledBuffer.setRGB(lednum, r, g, b);
|
||||
//m_currentPattern = pattern;
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
public static void setLEDHSV(int lednum, int hue, int sat, int val){
|
||||
m_ledBuffer.setRGB(lednum, hue, sat, val);
|
||||
//m_currentPattern = pattern;
|
||||
// m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
* @return
|
||||
*/
|
||||
public AddressableLEDBuffer getBuffer() {
|
||||
return m_ledBuffer;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,364 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotUnits;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class SwerveDrive extends Subsystem {
|
||||
|
||||
private SwerveModule leftFront;
|
||||
private SwerveModule rightFront;
|
||||
private SwerveModule leftBack;
|
||||
private SwerveModule rightBack;
|
||||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
private int gear_index;
|
||||
private boolean stopped = false;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
||||
super();
|
||||
this.leftFront = leftFront;
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
reset_index();
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
|
||||
// double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// rightStick.getAngle()
|
||||
double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
|
||||
// System.out.println(ang);
|
||||
// module.go(ang);
|
||||
// Rotation2d rot = Rotation2d.fromRadians(ang);
|
||||
Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
|
||||
SwerveModuleState state = new SwerveModuleState(speed, rot);
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
|
||||
SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
|
||||
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot_correction = 0;
|
||||
// rot = rightStick.getX();
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
// rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
// chassisSpeeds = chassisSpeeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
// double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
|
||||
}
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
// Translation2d rightStick = new Translation2d(-rightX, rightY);
|
||||
double rightX = rightStick.getX();
|
||||
double rightY = rightStick.getY();
|
||||
|
||||
double rot_correction = 0;
|
||||
|
||||
// double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
|
||||
|
||||
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
|
||||
double min = tmp.getDegrees();
|
||||
min = Math.max(Math.abs(min), 2);
|
||||
if(tmp.getDegrees() < 0)
|
||||
min*=-1;
|
||||
tmp = new Rotation2d(min * Math.PI / 180);
|
||||
rot = tmp.getRadians(); // x x - y/x
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
// setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
double currentAngle = getGyroAngle();
|
||||
double error = angle - currentAngle;
|
||||
|
||||
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return -gyro.getAngle();
|
||||
}
|
||||
|
||||
public void add180() {
|
||||
gyro.reset(gyro.getAngle()+180);
|
||||
rotTarget = gyro.getAngle();
|
||||
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroFlip() {
|
||||
gyro.resetFlip();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightBlue() {
|
||||
gyro.resetRightSideBlue();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyroRightAmp() {
|
||||
gyro.resetAmpSide();
|
||||
rotTarget = gyro.getAngle();
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
for (SwerveModule module : this.modules) {
|
||||
module.stop();
|
||||
}
|
||||
}
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
public boolean getSpeedState() {
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index - 1;
|
||||
if (i == -1) i = 0;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
|
||||
int i = gear_index + 1;
|
||||
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(SwerveDriveConstants.GEARS[i]);
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("SLOW");
|
||||
}
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("FAST");
|
||||
}
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
|
||||
for(int i=0;i<5;i++){
|
||||
Log("TURBO");
|
||||
}
|
||||
}
|
||||
|
||||
public void toggleGear(double angle) {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
}
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Gyro angle", this.gyro.getAngle());
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Shift State", this.speedAdjust);
|
||||
|
||||
// this.leftFront.queryStatus();
|
||||
// this.leftBack.queryStatus();
|
||||
// this.rightFront.queryStatus();
|
||||
// this.rightBack.queryStatus();
|
||||
|
||||
//TODO: Add more status things
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
for (SwerveModule module : modules) {
|
||||
for (Report moduleDignostic : module.diagnosticStatus().reports) {
|
||||
status.addReport(moduleDignostic.reportLevel, "[" + module.getSubsystemName() + "] " + moduleDignostic.description);
|
||||
}
|
||||
}
|
||||
|
||||
status.diagnoseHardwareCTRE("Swerve Gyro", gyro.getPigeon());
|
||||
|
||||
return status;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.logging.Level;
|
||||
|
||||
import com.ctre.phoenix6.StatusSignal;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.ControlRequest;
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.signals.MagnetHealthValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.SensorDirectionValue;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class SwerveModule extends Subsystem {
|
||||
private String name = "Null";
|
||||
private TalonFX driveMotor;
|
||||
private TalonFX angleMotor;
|
||||
private CANcoder encoder;
|
||||
// private final StatusSignal<Double> cc_pos;
|
||||
// private final StatusSignal<Double> cc_vel;
|
||||
// private int selfid;
|
||||
// private ConfigurableDouble offsetGetter;
|
||||
private static int swerveId = 0;
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
|
||||
super();
|
||||
this.name = name;
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
|
||||
var motorCfg = new TalonFXConfiguration()
|
||||
.withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
|
||||
).withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(100)
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
.withSupplyCurrentLimit(100)
|
||||
.withSupplyCurrentLimitEnable(true)
|
||||
);
|
||||
|
||||
driveMotor.getConfigurator().apply(motorCfg);
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration()
|
||||
.withOpenLoopRamps(
|
||||
new OpenLoopRampsConfigs()
|
||||
.withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
|
||||
).withClosedLoopRamps(
|
||||
new ClosedLoopRampsConfigs()
|
||||
.withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Brake)
|
||||
.withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
|
||||
);
|
||||
|
||||
angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
|
||||
|
||||
angleConfig.Slot0.kP = swerveGains.kP;
|
||||
angleConfig.Slot0.kI = swerveGains.kI;
|
||||
angleConfig.Slot0.kD = swerveGains.kD;
|
||||
|
||||
angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
|
||||
angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
|
||||
angleMotor.getConfigurator().apply(angleConfig);
|
||||
|
||||
CANcoderConfiguration canconfig = new CANcoderConfiguration();
|
||||
canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
|
||||
canconfig.MagnetSensor.MagnetOffset = offset;
|
||||
encoder.getConfigurator().apply(canconfig);
|
||||
|
||||
rotateToAngle(0);
|
||||
}
|
||||
|
||||
// public void go(double ang){
|
||||
// // double curang = this.encoder.getAbsolutePosition().getValue();
|
||||
// System.out.println(getAngle().getDegrees());
|
||||
// rotateToAngle(ang);
|
||||
// }
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
//encoder.configMagnetOffset(offsetGetter.get());
|
||||
//SmartDashboard.putString("Error Code: " + selfid, getstuff());
|
||||
// SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
|
||||
// SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
|
||||
// SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
|
||||
// SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
|
||||
}
|
||||
/**
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANcoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle of a SwerveModule as a Rotation2d
|
||||
* @return the angle of a SwerveModule as a Rotation2d
|
||||
*/
|
||||
public Rotation2d getAngle() {
|
||||
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
// return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude());
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
// return this.angleMotor.getSelectedSensorVelocity();
|
||||
return angleMotor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
// return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
return driveMotor.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
// return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
return driveMotor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
final PositionVoltage m_request = new PositionVoltage(angle);
|
||||
angleMotor.setControl(m_request);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get state of swerve module
|
||||
* @return speed in m/s and angle in degrees
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() *
|
||||
SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV *
|
||||
SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV),
|
||||
getAngle()
|
||||
);
|
||||
}
|
||||
|
||||
// private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
|
||||
// Rotation2d curRot = this.getAngle();
|
||||
|
||||
// }
|
||||
|
||||
/**
|
||||
* Returns the current position of the SwerveModule
|
||||
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
|
||||
// */
|
||||
// public SwerveModulePosition getPosition() {
|
||||
// return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
// }
|
||||
|
||||
/**
|
||||
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
|
||||
* @param desiredState a SwerveModuleState representing the desired new state of the module
|
||||
// */
|
||||
public void setDesiredState(SwerveModuleState state) {
|
||||
Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
state.optimize(currentRotation);//SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// calculate the difference between our current rotational position and our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
|
||||
|
||||
rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
|
||||
|
||||
driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
// encoder.setPosition(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return this.name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
|
||||
SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
|
||||
//TODO: Add more status things
|
||||
}
|
||||
|
||||
public boolean motorsAlive() {
|
||||
return this.driveMotor.isAlive() && this.angleMotor.isAlive();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
status.diagnoseHardwareCTRE("Drive", this.driveMotor);
|
||||
status.diagnoseHardwareCTRE("Angle", this.angleMotor);
|
||||
status.diagnoseHardwareCTRE("Steer", this.encoder);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
// public double getCurrent() {
|
||||
// return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
// }
|
||||
|
||||
// public double getVoltage() {
|
||||
// return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
// }
|
||||
|
||||
// public String getstuff() {
|
||||
// encoder.getPosition();
|
||||
// return "" + encoder.getLastError().value;
|
||||
// }
|
||||
}
|
||||
Reference in New Issue
Block a user