Skip to content

Commit

Permalink
2Good.
Browse files Browse the repository at this point in the history
  • Loading branch information
@cttdev
cttdev committed Oct 17, 2021
1 parent 7bb1f57 commit bfaed15
Show file tree
Hide file tree
Showing 12 changed files with 434 additions and 92 deletions.
292 changes: 292 additions & 0 deletions src/com/_604robotics/deploy/simple_test_slow.csv
View file

Large diffs are not rendered by default.

27 changes: 17 additions & 10 deletions src/com/_604robotics/robot2020/auto/QuixPlanSwerveTrajectoryTracker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,33 +11,40 @@
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;

public class QuixPlanSwerveTrajectoryTracker extends QuixPlanSwerveTrackingCoordinator {
private final SimpleMotorFeedforward feedforward;
private final Swerve.Auto auto;
private final Swerve swerve;

public QuixPlanSwerveTrajectoryTracker(QuikPlanSwerveReader reader, Swerve swerve, SwerveTrackerConstants constants) {
super(reader, swerve, constants);
this.feedforward = constants.feedforward;
this.swerve = swerve;

this.auto = swerve.new Auto();
}

@Override
public void useOutput(QuixSwerveModuleState[] moduleStates) {
auto.moduleStates.set(moduleStates);
// auto.moduleStates.set(moduleStates);

double[] feedforwards = {};
for (int i = 0; i < moduleStates.length; i++) {
feedforwards[i] = feedforward.calculate(moduleStates[i].speedMetersPerSecond);
}
auto.feedforwards.set(feedforwards);
swerve.driveClosedLoop(moduleStates);

// System.out.println("Velocity: " + moduleStates[0].speedMetersPerSecond);
// System.out.println("Angle: " + moduleStates[0].angle);

auto.activate();
}

@Override
public void stop() {
auto.moduleStates.set(new QuixSwerveModuleState[4]);
auto.feedforwards.set(new double[4]);
QuixSwerveModuleState[] moduleStates = {
new QuixSwerveModuleState(),
new QuixSwerveModuleState(),
new QuixSwerveModuleState(),
new QuixSwerveModuleState()
};

swerve.driveClosedLoop(moduleStates);

// auto.moduleStates.set(moduleStates);

auto.activate();
}
Expand Down
19 changes: 19 additions & 0 deletions src/com/_604robotics/robot2020/auto/paths/QuikPlanPreGen.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package com._604robotics.robot2020.auto.paths;

import com._604robotics.robot2020.Robot2020;
import com._604robotics.robot2020.auto.QuixPlanSwerveTrajectoryTracker;
import com._604robotics.robot2020.constants.Calibration;
import com._604robotics.robotnik.prefabs.auto.QuikPlanReader;
import com._604robotics.robotnik.prefabs.auto.QuikPlanSwerveReader;
import com._604robotics.robotnik.prefabs.coordinators.StatefulCoordinator;

public class QuikPlanPreGen extends StatefulCoordinator {
public QuikPlanPreGen(Robot2020 robot, QuikPlanSwerveReader reader) {
super(QuikPlanPreGen.class);

addState(
"Driver Slalom",
new QuixPlanSwerveTrajectoryTracker(
reader, robot.drive, Calibration.Auto.TRACKER_CONSTANTS));
}
}
19 changes: 11 additions & 8 deletions src/com/_604robotics/robot2020/constants/Calibration.java
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,13 @@
package com._604robotics.robot2020.constants;

import com._604robotics.quixsam.mathematics.DoubleInterpolatableTreeMap;
import com._604robotics.robotnik.prefabs.auto.SwerveTrackerConstants;
import com._604robotics.robotnik.prefabs.auto.TrackerConstants;
import com._604robotics.robotnik.prefabs.motorcontrol.gearing.GearRatio;
import com._604robotics.robotnik.utils.annotations.Unreal;

import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.util.Units;

Expand Down Expand Up @@ -84,9 +87,11 @@ public static final class Drive {
public static final double WHEEL_BASE = Units.inchesToMeters(25.5);
public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);

public static final GearRatio DRIVE_RATIO = new GearRatio(1, 6.86);
public static final GearRatio DRIVE_RATIO = new GearRatio(1, -6.86);
public static final GearRatio STEERING_RATIO = new GearRatio(1, 12.8);

public static final SimpleMotorFeedforward DRIVE_FEEDFORWARD = new SimpleMotorFeedforward(0.667, 2.44, 0.27);

public static final double MAX_DRIVE_VELOCITY = 4.5;
public static final double MAX_ANGULAR_VELOCITY = 11.5;

Expand Down Expand Up @@ -118,13 +123,11 @@ public static final class Auto {
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2; // 1.5
public static final double MAX_CENTRIPETAL_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2; // 1.5

public static final TrackerConstants TRACKER_CONSTANTS =
new TrackerConstants(
new SimpleMotorFeedforward(
KS_VOLTS, KV_VOLT_SECONDS_PER_METER, KA_VOLT_SECONDS_SQUARED_PER_METER),
KP_DRIVE_VELCOTIY,
RAMSETE_B,
RAMSETE_ZETA,
public static final SwerveTrackerConstants TRACKER_CONSTANTS =
new SwerveTrackerConstants(
new PIDController(1.0, 0.0, 0.0),
new PIDController(1.0, 0.0, 0.0),
new PIDController(1.0, 0.0, 0.0),
MAX_SPEED_METERS_PER_SECOND,
MAX_ACCELERATION_METERS_PER_SECOND_SQUARED);

Expand Down
78 changes: 31 additions & 47 deletions src/com/_604robotics/robot2020/modes/AutonomousMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,14 @@
import static com._604robotics.robot2020.constants.Calibration.Auto.MAX_SPEED_METERS_PER_SECOND;

import com._604robotics.robot2020.Robot2020;
import com._604robotics.robot2020.auto.paths.QuikPlanPreGen;
import com._604robotics.robot2020.constants.Calibration;
import com._604robotics.robotnik.Coordinator;
import com._604robotics.robotnik.DashboardManager;
import com._604robotics.robotnik.Logger;
import com._604robotics.robotnik.Output;
import com._604robotics.robotnik.prefabs.auto.QuikPlanReader;
import com._604robotics.robotnik.prefabs.auto.QuikPlanSwerveReader;
import com._604robotics.robotnik.prefabs.auto.TrajectoryCreator;
import com._604robotics.robotnik.prefabs.auto.angular.AngularMotionConstraint;
import com._604robotics.robotnik.prefabs.auto.angular.TurnInPlaceTrajectory;
Expand All @@ -35,7 +37,7 @@ public class AutonomousMode extends Coordinator {
private Coordinator selectedModeMacro;

private TrajectoryCreator trajectoryCreator;
private QuikPlanReader reader;
private QuikPlanSwerveReader reader;

public final Output<AutonMode> autonMode;

Expand All @@ -62,71 +64,53 @@ public AutonomousMode(Robot2020 robot) {
// new CentripetalAccelerationConstraint(
// MAX_CENTRIPETAL_ACCELERATION_RADIANS_PER_SECOND_SQUARED));

// reader = new QuikPlanReader(robot.drive);
reader = new QuikPlanSwerveReader(robot.drive);
}

@Override
public void begin() {
// System.out.println(autonMode.get());
System.out.println(autonMode.get());
// robot.drive.setIdleMode(IdleMode.kCoast);

// reader.loadChosenFile();

// switch (autonMode.get()) {
// case MANUAL:
// selectedModeMacro = robot.teleopMode;
// break;
// case FAILSAFE_FORWARD_12:
// selectedModeMacro = new FallForwardMacro();
// break;
// case TURN_IN_PLACE_LEFT:
// selectedModeMacro = new TurnInPlaceLeft();
// break;
// case FAILSAFE_BACKWARD_12:
// selectedModeMacro = new FallBackMacro();
// break;
// // case FULL_FIELD_10_BALL:
// // selectedModeMacro = new QuikPlanPreGen(robot, reader);
// // break;
// case QUIX_PLAN_PREGEN:
// selectedModeMacro = new QuikPlanPreGen(robot, reader);
// break;
// case GALATIC_SEARCH:
// robot.disabledMode.livePlan.loadTrajectory();
// selectedModeMacro = new QuikPlanSearch(robot, robot.disabledMode.livePlan);
// break;
// case OFF:
// default:
// selectedModeMacro = null;
// break;
// }

// System.out.println(selectedModeMacro);
reader.loadChosenFile();

switch (autonMode.get()) {
case MANUAL:
selectedModeMacro = robot.teleopMode;
case QUIX_PLAN_PREGEN:
selectedModeMacro = new QuikPlanPreGen(robot, reader);
break;
case OFF:
default:
selectedModeMacro = null;
break;
}

System.out.println(selectedModeMacro);
// robot.drive.disableMotorSafety();

// if (selectedModeMacro != null) {
// selectedModeMacro.start();
// }
if (selectedModeMacro != null) {
selectedModeMacro.start();
}
}

@Override
public boolean run() {
// robot.drive.updateOdometry();
robot.drive.updateOdometry();

// if (selectedModeMacro == null) {
// return false;
// }
// return selectedModeMacro.execute();
return false;
if (selectedModeMacro == null) {
return false;
}
return selectedModeMacro.execute();
}

@Override
public void end() {
// robot.drive.enableMotorSafety();
// robot.drive.setIdleMode(IdleMode.kCoast);
// if (selectedModeMacro != null) {
// selectedModeMacro.stop();
// }
if (selectedModeMacro != null) {
selectedModeMacro.stop();
}
}

// public QuikPlanReader getReader() {
Expand Down
4 changes: 3 additions & 1 deletion src/com/_604robotics/robot2020/modes/DisabledMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import com._604robotics.robot2020.constants.Calibration;
import com._604robotics.robotnik.Coordinator;
import com._604robotics.robotnik.Logger;
import com._604robotics.robotnik.prefabs.auto.FalconDashboard;
import com._604robotics.robotnik.prefabs.auto.QuikPlanLive;
import com._604robotics.robotnik.prefabs.vision.VisionCamera.GamePiece;
import com._604robotics.robotnik.prefabs.vision.VisionManager;
Expand Down Expand Up @@ -56,7 +57,8 @@ public void begin() {

@Override
public boolean run() {
// robot.drive.updateOdometry();
robot.drive.updateOdometry();
FalconDashboard.getInstance().publishRobotPose(robot.drive.getPose());

// // Search Gamepieces
// if (SmartDashboard.getBoolean("Enable Searching", false)) {
Expand Down
4 changes: 4 additions & 0 deletions src/com/_604robotics/robot2020/modes/TeleopMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import com._604robotics.robot2020.modules.Tower;
import com._604robotics.robotnik.Coordinator;
import com._604robotics.robotnik.Logger;
import com._604robotics.robotnik.prefabs.auto.FalconDashboard;
import com._604robotics.robotnik.prefabs.controller.ProfiledPIDController;
import com._604robotics.robotnik.prefabs.flow.SmartTimer;
import com._604robotics.robotnik.prefabs.flow.Toggle;
Expand Down Expand Up @@ -343,6 +344,9 @@ public void run() {
autoAngle.activate();
break;
}

robot.drive.updateOdometry();
FalconDashboard.getInstance().publishRobotPose(robot.drive.getPose());
}

private void snapTo(CardinalDirections direction) {
Expand Down
Loading

0 comments on commit bfaed15

Please sign in to comment.