Skip to content
This repository has been archived by the owner on Jul 29, 2021. It is now read-only.

Commit

Permalink
Merge pull request #52 from frc3197/pid-but-actually
Browse files Browse the repository at this point in the history 
Actual PID using SmartMotion and SmartVelocity
  • Loading branch information
@spencrr
spencrr committed Mar 12, 2019
2 parents 99760cc + 43d9234 commit 9c0e8b0
Show file tree
Hide file tree
Showing 6 changed files with 140 additions and 32 deletions.
7 changes: 5 additions & 2 deletions src/main/java/org/team3197/frc2019/robot/OI.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.team3197.frc2019.robot;

import org.team3197.frc2019.robot.RobotMap.MaxSpeeds;
import org.team3197.frc2019.robot.commands.AlignTurn;
import org.team3197.frc2019.robot.commands.presets.Cargo;
import org.team3197.frc2019.robot.commands.presets.LevelOne;
import org.team3197.frc2019.robot.commands.presets.LevelThree;
Expand Down Expand Up @@ -53,6 +54,8 @@ public class OI {

driverA.whenPressed(Robot.driveTrain.changeDriveMode);

driverB.whileHeld(new AlignTurn(Robot.driveTrain));
// secondaryX.whenPressed(Robot.elevator.reset);
// driverB.whileHeld(new AlignTurn(Robot.driveTrain));
secondaryX.whenPressed(Robot.arm.toggleGyro);

Expand Down Expand Up @@ -94,11 +97,11 @@ public static double elevatorSpeed() {
}

public static double elbowSpeed() {
return -secondary.getY(Hand.kRight) * RobotMap.elbowSpeedMultiplier;
return secondary.getY(Hand.kRight) * RobotMap.elbowSpeedMultiplier;
}

public static double wristSpeed() {
return secondary.getY(Hand.kLeft) * RobotMap.wristSpeedMultiplier;
return -secondary.getY(Hand.kLeft) * RobotMap.wristSpeedMultiplier;

}

Expand Down
8 changes: 5 additions & 3 deletions src/main/java/org/team3197/frc2019/robot/RobotMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ private ElevatorPreset(double pos) {
}

public static enum DeadbandType {
kElevator(0.03), kElbow(0.06), kWrist(0.06), kDrive(0.08);
kElevator(0.03), kElbow(0.06), kWrist(0.06), kDrive(0.08), kClimberVertical(0.05), kErector(0.05);
public double speed;

private DeadbandType(double speed) {
Expand Down Expand Up @@ -79,7 +79,7 @@ private Channel(int channel) {
};

public static enum GyroSensitivity {
kDrive(0.01), kArm(0.0175);
kDrive(0.01), kArm(-0.01);
public double val;

private GyroSensitivity(double val) {
Expand All @@ -88,7 +88,7 @@ private GyroSensitivity(double val) {
};

public static enum MaxSpeeds {
kElevator(.7, -.5), kArm(.6), kWrist(.45), kHatch(1), kCargo(1), kClimberVertical(.25), kClimberHorizontal(.25);
kElevator(.7, -.5), kArm(.6), kWrist(.45), kHatch(1), kCargo(1), kClimberVertical(.5), kClimberHorizontal(.75);

public double forwardSpeed;
public double reverseSpeed;
Expand Down Expand Up @@ -142,4 +142,6 @@ public static enum RobotType {
public static final double visionTargetX = .5;

public static final double visionTargetArea = 32000;

public static final int cameraPixelWidth = 640;
}
52 changes: 32 additions & 20 deletions src/main/java/org/team3197/frc2019/robot/commands/AlignTurn.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.team3197.frc2019.robot.RobotMap;
import org.team3197.frc2019.robot.RobotMap.DeadbandType;
import org.team3197.frc2019.robot.subsystems.DriveTrain;
Expand All @@ -18,6 +20,7 @@ public class AlignTurn extends Command {
private NetworkTable vision;
private NetworkTableEntry contourXsEntry;
private NetworkTableEntry contourAreasEntry;
private int turnDirection = -1;

public AlignTurn(DriveTrain driveTrain) {
super();
Expand All @@ -31,8 +34,9 @@ public AlignTurn(DriveTrain driveTrain) {

@Override
protected void execute() {
System.out.println("Inside execute");
getContourParameters();
driveTrain.arcadeDrive(verticalSpeed, turnSpeed);
System.out.println("After Get Contour");
}

@Override
Expand All @@ -41,30 +45,38 @@ protected boolean isFinished() {
}

private void getContourParameters() {
System.out.println("Inside the function!");
Number[] defaultValues = new Number[] {};
Number[] contourXs = contourXsEntry.getNumberArray(defaultValues);
Number[] contourAreas = contourAreasEntry.getNumberArray(defaultValues);
System.out.println(contourXs[0]);

if (contourXs.length == 2) {
double x0 = contourXs[0].doubleValue();
double x1 = contourXs[1].doubleValue();
double midpoint = (x0 + x1) / (2 * RobotMap.xMax) - 0.5;
turnSpeed = -3 * Math.copySign(Math.pow(Math.abs(midpoint), 1), midpoint);
// If the midX is greater than the target, turn left (-)
System.out.println(contourXs[0] + " " + contourXs[1] + " " + midpoint + " " + turnSpeed);
} else {
turnSpeed = 0;
}
if (contourAreas.length == 2 && Math.abs(turnSpeed) < DeadbandType.kDrive.speed) {
double area0 = contourAreas[0].doubleValue();
double area1 = contourAreas[1].doubleValue();
double areaError = ((area0 + area1) / RobotMap.visionTargetArea) - 1;
verticalSpeed = .6 * areaError;
System.out.println((area0 + area1) + " " + areaError + " " + verticalSpeed);
// If totalArea is greater than the target, go backward (-)
// boolean direction = Math
// .abs(contourXs[0].doubleValue() + contourXs[1].doubleValue()) >
// RobotMap.cameraPixelWidth;

if (contourXs.length == contourAreas.length) {
SmartDashboard.putNumber("length", contourXs.length);
switch (contourXs.length) {
case (0):
System.out.println("This feature has not been simulated.");
// TODO Add rumble
break;
case (1):
System.out.println("This feature is simulated, but probably broken.");
driveTrain.tankDrive(.5 * turnDirection, -.5 * turnDirection);
break;
case (2):
System.out.println("You have done exactly what our instructions neglected to tell you.");
double totalArea = contourAreas[0].doubleValue() + contourAreas[1].doubleValue();
driveTrain.tankDrive(contourAreas[1].doubleValue() / totalArea,
contourAreas[0].doubleValue() / totalArea);
turnDirection = (int) (Math.abs(contourAreas[0].doubleValue() - contourAreas[1].doubleValue())
/ contourAreas[0].doubleValue() - contourAreas[1].doubleValue());
break;
}
} else {
verticalSpeed = 0;
driveTrain.tankDrive(.5 * turnDirection, -.5 * turnDirection);
}
}

}
2 changes: 1 addition & 1 deletion src/main/java/org/team3197/frc2019/robot/commands/defaults/Articulate.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public Articulate(Arm arm) {
@Override
protected void execute() {
double elbowSpeed = OI.elbowSpeed();
double wristSpeed = OI.wristSpeed();
double wristSpeed = Math.abs(OI.wristSpeed()) > .05 ? OI.wristSpeed() : 0;
arm.wrist(wristSpeed);
arm.elbow(elbowSpeed);
}
Expand Down
52 changes: 48 additions & 4 deletions src/main/java/org/team3197/frc2019/robot/subsystems/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.revrobotics.CANDigitalInput;
import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

Expand Down Expand Up @@ -48,6 +49,36 @@ public Arm() {
} else {
wrist.setInverted(false);
}

final double kP = 5e-5;
final double kI = 1e-6;
final double kD = 0;
final double kIz = 0;
final double kFF = 0.000156;
final double kMaxOutput = 1;
final double kMinOutput = -1;
final double maxRPM = 5700;

// Smart Motion Coefficients
final double maxVel = 2000; // rpm
final double maxAcc = 1500;

// set PID coefficients
elbow.getPIDController().setP(kP);
elbow.getPIDController().setI(kI);
elbow.getPIDController().setD(kD);
elbow.getPIDController().setIZone(kIz);
elbow.getPIDController().setFF(kFF);
elbow.getPIDController().setOutputRange(kMinOutput, kMaxOutput);

final int smartMotionSlot = 0;
elbow.getPIDController().setSmartMotionMaxVelocity(maxVel, smartMotionSlot);
// elbow.getPIDController().setSmartMotionMinOutputVelocity(minVel,
// smartMotionSlot);
elbow.getPIDController().setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
// elbow.getPIDController().setSmartMotionAllowedClosedLoopError(allowedErr,
// smartMotionSlot);

}

@Override
Expand All @@ -60,17 +91,30 @@ public void drive(double speed, boolean hold) {
wrist(0);
}

boolean pidLast = false;
double referenceEncVal = 0;

public void elbow(double speed) {
double output = speed;

// Stops the elbow from constaltly moving upwards when not being moved by the
// Stops the elbow from constantly moving upwards when not being moved by the
// joystick
if (!elbowLimit.get() && Math.abs(output) < DeadbandType.kElbow.speed) {
output = 0;
// output = 0;
if (!pidLast) {
pidLast = true;
referenceEncVal = elbow.getEncoder().getPosition();
}
// elbow.getPIDController().setReference(referenceEncVal,
// ControlType.kSmartMotion);
elbow.getPIDController().setReference(0, ControlType.kSmartVelocity);
} else {
// elbow.set(output);
pidLast = false;
elbow.getPIDController().setReference(output, ControlType.kDutyCycle);
}
SmartDashboard.putNumber("ElbowEncoder", getElbowEncoderPosition());

elbow.set(output);
}

public void wrist(double speed) {
Expand All @@ -82,7 +126,7 @@ public void wrist(double speed) {
SmartDashboard.putNumber("deltaAngle", deltaAngle);
SmartDashboard.putNumber("WristEncoder", getWristEncoderPosition());

if (Math.abs(output) < DeadbandType.kWrist.speed && useGyro) {
if (Math.abs(output) < DeadbandType.kWrist.speed) {// && useGyro) {
output = gyroSpeed;
} else {
resetGyroAngle();
Expand Down
51 changes: 49 additions & 2 deletions src/main/java/org/team3197/frc2019/robot/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,10 @@
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;

import org.team3197.frc2019.robot.RobotMap.CANSparkMaxID;
import org.team3197.frc2019.robot.RobotMap.DeadbandType;
import org.team3197.frc2019.robot.commands.defaults.Climb;

import edu.wpi.first.wpilibj.command.Subsystem;
Expand All @@ -16,16 +18,61 @@ public class Climber extends Subsystem {
public Climber() {
super();
vertical.setIdleMode(IdleMode.kBrake);
horizontal.setIdleMode(IdleMode.kBrake);
horizontal.setIdleMode(IdleMode.kCoast);

final double kP = 5e-5;
final double kI = 1e-6;
final double kD = 0;
final double kIz = 0;
final double kFF = 0.000156;
final double kMaxOutput = 1;
final double kMinOutput = -1;
final double maxRPM = 5700;

// Smart Motion Coefficients
final double maxVel = 2000; // rpm
final double maxAcc = 1500;

// set PID coefficients
vertical.getPIDController().setP(kP);
vertical.getPIDController().setI(kI);
vertical.getPIDController().setD(kD);
vertical.getPIDController().setIZone(kIz);
vertical.getPIDController().setFF(kFF);
vertical.getPIDController().setOutputRange(kMinOutput, kMaxOutput);

final int smartMotionSlot = 0;
vertical.getPIDController().setSmartMotionMaxVelocity(maxVel, smartMotionSlot);
// elbow.getPIDController().setSmartMotionMinOutputVelocity(minVel,
// smartMotionSlot);
vertical.getPIDController().setSmartMotionMaxAccel(maxAcc, smartMotionSlot);
// elbow.getPIDController().setSmartMotionAllowedClosedLoopError(allowedErr,
// smartMotionSlot);
}

@Override
public void initDefaultCommand() {
setDefaultCommand(new Climb(this));
}

boolean pidLast = false;
double referenceEncVal = 0;

public void driveVertical(double speed) {
vertical.set(speed);
if (Math.abs(speed) < DeadbandType.kClimberVertical.speed) {
// output = 0;
if (!pidLast) {
pidLast = true;
referenceEncVal = vertical.getEncoder().getPosition();
}
// vertical.getPIDController().setReference(referenceEncVal,
// ControlType.kSmartMotion);
vertical.getPIDController().setReference(0, ControlType.kSmartVelocity);
} else {
// vertical.set(output);
pidLast = false;
vertical.getPIDController().setReference(speed, ControlType.kDutyCycle);
}
}

public void driveHorizontal(double speed) {
Expand Down

0 comments on commit 9c0e8b0

Please sign in to comment.