Skip to content

Commit

Permalink
Swap to NavX
Browse files Browse the repository at this point in the history
  • Loading branch information
@EganJ
EganJ committed Jan 18, 2020
1 parent 156b46f commit a58e6f9
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 33 deletions.
32 changes: 17 additions & 15 deletions src/main/java/frc/robot/commands/vision/PositionVisionCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,8 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Shooter;

Expand All @@ -25,6 +22,11 @@ public class PositionVisionCommand extends CommandBase {
private double distance;
private double angle;

private PIDController angleFollower;
private final double kP=0.2;
private final double kI=0.00001;
private final double kD=0.0;

public PositionVisionCommand(Shooter shooter, DriveTrain driveTrain) {
this.driveTrain = driveTrain;
this.shooter = shooter;
Expand All @@ -36,6 +38,7 @@ public PositionVisionCommand(Shooter shooter, DriveTrain driveTrain) {
distanceEntry = table.getEntry("distance");
distanceListenerHandle=distanceEntry.addListener(this::listenDistance, EntryListenerFlags.kUpdate);

angleFollower=new PIDController(kP, kI, kD);
}

@Override
Expand All @@ -45,7 +48,8 @@ public void initialize() {

@Override
public void execute() {

double out = angleFollower.calculate(driveTrain.getRot());
driveTrain.tankDrive(-out, out);
}

@Override
Expand All @@ -60,23 +64,21 @@ public boolean isFinished() {
}

private void listenAngle(EntryNotification n){
angle=n.value.getDouble();
if(!turning && angle>0.01){
turnByAngle(angle);
double val=n.value.getDouble();
if(!turning && val>0.01){
turnByAngle(val);
}
}

private void listenDistance(EntryNotification n){
distance = n.value.getDouble();
}

private void turnByAngle(double radians) {
DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.K_TRACK_WIDTH_METERS);
// kinematics works for speeds, but by inputing a position a position can also
// be yielded
DifferentialDriveWheelSpeeds turnPositions = kinematics.toWheelSpeeds(new ChassisSpeeds(0, 0, radians));
driveTrain.positionDrive(driveTrain.metersToRotations(turnPositions.rightMetersPerSecond),
driveTrain.metersToRotations(turnPositions.leftMetersPerSecond));
private void turnByAngle(double degrees) {
angle=degrees;
turning=true;
angleFollower.setSetpoint(degrees);
driveTrain.resetRot();
}

}
13 changes: 10 additions & 3 deletions src/main/java/frc/robot/subsystems/DriveTrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,13 @@

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;
import com.revrobotics.EncoderType;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.utils.Encoder;
Expand All @@ -29,7 +27,7 @@ public class DriveTrain extends SubsystemBase {
private final CANEncoder leftEnc;

private final Encoder enc = Encoder.Grayhill256;
private final AHRS gyro;
public final AHRS gyro;

// PID velocity constants
private static final double kVP = 0.3;
Expand Down Expand Up @@ -107,6 +105,15 @@ public double getLeftPos() {
return leftEnc.getPosition();
}

public double getRot(){
return gyro.getAngle();
}

public void resetRot(){
gyro.reset();
}


public void setPID(double kP, double kI, double kD) {
// right PID configuration
rightLeader.getPIDController().setP(kP);
Expand Down
30 changes: 15 additions & 15 deletions vendordeps/Phoenix.json
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix.json",
"name": "CTRE-Phoenix",
"version": "5.17.3",
"version": "5.17.4",
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [
"http://devsite.ctr-electronics.com/maven/release/"
Expand All @@ -11,19 +11,19 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-java",
"version": "5.17.3"
"version": "5.17.4"
},
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-java",
"version": "5.17.3"
"version": "5.17.4"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.17.3",
"version": "5.17.4",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
Expand All @@ -35,7 +35,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.17.3",
"version": "5.17.4",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
Expand All @@ -47,7 +47,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.17.3",
"version": "5.17.4",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
Expand All @@ -58,7 +58,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.17.3",
"version": "5.17.4",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
Expand All @@ -69,7 +69,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.17.3",
"version": "5.17.4",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
Expand All @@ -83,7 +83,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-cpp",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_Phoenix_WPI",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -97,7 +97,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-cpp",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_Phoenix",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -111,7 +111,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_PhoenixCCI",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -125,7 +125,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "diagnostics",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_PhoenixDiagnostics",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -139,7 +139,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "canutils",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_PhoenixCanutils",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -152,7 +152,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "platform-stub",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_PhoenixPlatform",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand All @@ -165,7 +165,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "core",
"version": "5.17.3",
"version": "5.17.4",
"libName": "CTRE_PhoenixCore",
"headerClassifier": "headers",
"sharedLibrary": false,
Expand Down

0 comments on commit a58e6f9

Please sign in to comment.