From f994259246a65d9a9133afb5045afcc61e37162e Mon Sep 17 00:00:00 2001 From: Tri Force Date: Wed, 16 Jan 2019 20:03:20 -0500 Subject: [PATCH 01/15] Basic vision support --- src/main/java/frc/robot/Robot.java | 6 +- .../java/frc/robot/commands/TeleopVision.java | 50 +++++++++ .../frc/robot/subsystems/VisionRocket.java | 100 +++++++++++++++++- 3 files changed, 148 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TeleopVision.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e8008d8..2a03350 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.commands.ExampleCommand; import frc.robot.commands.TeleopDefault; -import frc.robot.commands.TeleopTriggerControl; +import frc.robot.commands.TeleopVision; import frc.robot.subsystems.ExampleSubsystem; /** @@ -27,7 +27,7 @@ public class Robot extends TimedRobot { public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); public static CommandBase cmdBase; - public static TeleopDefault teleop; + public static TeleopVision teleop; Command m_autonomousCommand; SendableChooser m_chooser = new SendableChooser<>(); @@ -40,7 +40,7 @@ public class Robot extends TimedRobot { public void robotInit() { cmdBase = new CommandBase(); //CommandBase.drivetrain.InvertTalons(); - teleop = new TeleopDefault(); + teleop = new TeleopVision(); m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); // chooser.addOption("My Auto", new MyAutoCommand()); SmartDashboard.putData("Auto mode", m_chooser); diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java new file mode 100644 index 0000000..3071578 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.commands; + +import java.util.ArrayList; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.*; +import frc.robot.CommandBase; + +public class TeleopVision extends Command { + public TeleopVision() { + requires(CommandBase.visionRocket); + requires(CommandBase.drivetrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + ArrayList speeds = CommandBase.visionRocket.DriveToPair(0, 1.75); + CommandBase.drivetrain.Drive(speeds.get(0), speeds.get(1)); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index d1ee9c3..a21a77c 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -45,10 +45,18 @@ public ArrayList> GetContours() { Number[] ratios = ntratios.getNumberArray(def); Number[] areas = ntareas.getNumberArray(def); + int smallestLen = 1000; + if (widths.length < smallestLen) { smallestLen = widths.length; } + if (heights.length < smallestLen) { smallestLen = heights.length; } + if (xs.length < smallestLen) { smallestLen = xs.length; } + if (ys.length < smallestLen) { smallestLen = ys.length; } + if (ratios.length < smallestLen) { smallestLen = ratios.length; } + if (areas.length < smallestLen) { smallestLen = areas.length; } + ArrayList> contours = new ArrayList>(); - if (widths.length > 0) { - for (int i = 0; i < widths.length; i++) { + if (smallestLen > 0) { + for (int i = 0; i < smallestLen; i++) { ArrayList newContour = new ArrayList(); newContour.add((double)xs[i]); newContour.add((double)ys[i]); @@ -92,9 +100,6 @@ public ArrayList>> GetContourPairs() { newPair.add(contours.get(i)); newPair.add(contours.get(j)); - SmartDashboard.putNumber("ContourPair1", contours.get(i).get(0)); - SmartDashboard.putNumber("ContourPair2", contours.get(j).get(0)); - pairs.add(newPair); } @@ -102,4 +107,89 @@ public ArrayList>> GetContourPairs() { } return pairs; } + + public ArrayList DriveToPair(int mode, double speed) { + // mode 0 is cargo (center), mode 1 is the hatch panel (sides) + + // speeds.get(0) is the left side of the drive train, speeds.get(1) is the right side + ArrayList speeds = new ArrayList(); + + ArrayList>> pairs = GetContourPairs(); + if (pairs.size() > 0) { + + if (pairs.size() > 1) { + // + } else { + ArrayList> pair = SortContours(pairs.get(0), 0); + ArrayList contourA = pair.get(0); + ArrayList contourB = pair.get(1); + + double leftX = contourA.get(0) + (double)(contourA.get(2)/2); + double leftY = contourA.get(1) + (double)(contourA.get(3)/2); + + double rightX = contourB.get(0) + (double)(contourB.get(2)/2); + double rightY = contourB.get(1) + (double)(contourB.get(3)/2); + + NetworkTableEntry ntCamWidth = visionTable.getEntry("camWidth"); + NetworkTableEntry ntCamHeight = visionTable.getEntry("camHeight"); + double camWidth = ntCamWidth.getDouble(0); + double camHeight = ntCamHeight.getDouble(0); + + double cameraX = camWidth/2; + double pairX = leftX + (rightX-leftX)/2; + + double offset = pairX - cameraX; + + // Make speedScaleConstant bigger if you want robot to slow down over a longer distance, and smaller if you want it to slow down over a shorter distance + double speedScaleConstant = 8; + double speedScale = Math.min(speedScaleConstant / (contourA.get(2) + contourB.get(2))/2, 1.0); + double scaledSpeed = Math.min(speed * speedScale, 1.0); + + // Increase subScale to make adjustment less extreme // + double subScale = 3; + + if (Math.abs(offset) > 5) { + + if (offset > 0) { + // Angled too far to left + speeds.add(scaledSpeed); + + // Slow down right side + double scale = Math.min(1, subScale / Math.abs(offset)); + speeds.add(-(scaledSpeed * scale)); + } else { + // Angled too far to right + // Slow down left side + /*double scale = (Math.abs(offset) / (camWidth/2)); + speeds.add(scaledSpeed - scaledSpeed * scale);*/ + double scale = Math.min(1, subScale / Math.abs(offset)); + speeds.add(scaledSpeed * scale); + + speeds.add(-scaledSpeed); + + } + + } else { + speeds.add(scaledSpeed); + speeds.add(-scaledSpeed); + } + + + } + + } else { + // Set both sides to 0 + speeds.add(0.0); + speeds.add(0.0); + + } + + + if (speeds.size() != 2) { + speeds = new ArrayList(); + speeds.add(0.0); + speeds.add(0.0); + } + return speeds; + } } From 2c1b7438bf271d9ec095f4ada42dfcb558812a88 Mon Sep 17 00:00:00 2001 From: Tri Force Date: Sat, 19 Jan 2019 15:57:09 -0500 Subject: [PATCH 02/15] Working on vision! woo! --- .../java/frc/robot/commands/TeleopVision.java | 6 +- .../java/frc/robot/subsystems/DriveTrain.java | 128 +++++++++---- .../frc/robot/subsystems/VisionRocket.java | 170 +++++++++++++++--- src/main/java/org/team3236/AssistMode.java | 5 + .../java/org/team3236/DriveTrainMode.java | 5 + .../java/org/team3236/contours/Contour.java | 70 ++++++++ .../org/team3236/contours/ContourPair.java | 31 ++++ vendordeps/navx_frc.json | 33 ++++ 8 files changed, 386 insertions(+), 62 deletions(-) create mode 100644 src/main/java/org/team3236/AssistMode.java create mode 100644 src/main/java/org/team3236/DriveTrainMode.java create mode 100644 src/main/java/org/team3236/contours/Contour.java create mode 100644 src/main/java/org/team3236/contours/ContourPair.java create mode 100644 vendordeps/navx_frc.json diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java index 3071578..fe5c5a0 100644 --- a/src/main/java/frc/robot/commands/TeleopVision.java +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -10,7 +10,9 @@ import java.util.ArrayList; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.*; +import org.team3236.AssistMode; import frc.robot.CommandBase; public class TeleopVision extends Command { @@ -27,7 +29,9 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - ArrayList speeds = CommandBase.visionRocket.DriveToPair(0, 1.75); + ArrayList speeds = CommandBase.visionRocket.DriveAlongArc(AssistMode.HATCH, 0.4); + SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.GetAngle()); + CommandBase.drivetrain.Drive(speeds.get(0), speeds.get(1)); } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 07cfbaa..f97dc14 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -14,44 +14,102 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import com.kauailabs.navx.frc.*; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.SerialPort.*; + +import org.team3236.DriveTrainMode; /** * An example subsystem. You can replace me with your own Subsystem. */ public class DriveTrain extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - private WPI_TalonSRX LeftTalonA = new WPI_TalonSRX(RobotMap.LEFTTALONA); - private WPI_TalonSRX LeftTalonB = new WPI_TalonSRX(RobotMap.LEFTTALONB); - private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); - private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); - - public void InvertTalons() - { - RightTalonA.setInverted(true); - RightTalonB.setInverted(true); - } - - public void SetLeft(double speed){ - LeftTalonA.set(speed); - LeftTalonB.set(speed); - } - - public void SetRight(double speed){ - RightTalonA.set(speed); - RightTalonB.set(speed); - } - - public void Drive(double leftSpeed, double rightSpeed){ - SetLeft(leftSpeed); - SetRight(rightSpeed); - } - - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } - - + // Put methods for controlling this subsystem + // here. Call these from Commands. + private WPI_TalonSRX LeftTalonA = new WPI_TalonSRX(RobotMap.LEFTTALONA); + private WPI_TalonSRX LeftTalonB = new WPI_TalonSRX(RobotMap.LEFTTALONB); + private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); + private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); + + SerialPort Ultrasonic; + private static AHRS NavX = new AHRS(SPI.Port.kMXP); + private boolean autoLocked = false; + + private DriveTrainMode DriveMode = DriveTrainMode.HATCH; + + public void Initialize() { + // Set up the ultrasonic sensor // + try { + Ultrasonic = new SerialPort(9600, SerialPort.Port.kOnboard, 8, Parity.kNone, StopBits.kOne); + } catch (RuntimeException ex) { + throw ex; + } + Ultrasonic.setTimeout(2); + Ultrasonic.setReadBufferSize(6); + } + + public double GetDistance() { + + + return 0.0; + } + + public double GetAngle() { + return NavX.getAngle(); + } + + public double GetPitch() { + return NavX.getPitch(); + } + + public void ResetGyro() { + NavX.reset(); + } + + public void SetLeft(double speed) { + if (DriveMode == DriveTrainMode.HATCH) { + LeftTalonA.set(speed); + LeftTalonB.set(speed); + } else { + LeftTalonA.set(-speed); + LeftTalonB.set(-speed); + } + } + + public void SetRight(double speed) { + if (DriveMode == DriveTrainMode.HATCH) { + RightTalonA.set(-speed); + RightTalonB.set(-speed); + } else { + RightTalonA.set(speed); + RightTalonB.set(speed); + } + } + + public void Drive(double leftSpeed, double rightSpeed){ + SetLeft(leftSpeed); + SetRight(rightSpeed); + } + + public void LockAuto() { + autoLocked = true; + } + + public void UnlockAuto() { + autoLocked = false; + } + + public boolean IsAutoLocked() { + return autoLocked; + } + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } + + } diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index a21a77c..ca0a0ea 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -7,8 +7,15 @@ package frc.robot.subsystems; + import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.CommandBase; +import frc.robot.CommandBase.*; + +import org.team3236.contours.*; +import org.team3236.AssistMode; +import org.team3236.DriveTrainMode; import java.util.ArrayList; import java.util.Collections; @@ -28,7 +35,11 @@ public void initDefaultCommand() { private static NetworkTableInstance ntInst = NetworkTableInstance.getDefault(); private static NetworkTable visionTable = ntInst.getTable("contours"); - public ArrayList> GetContours() { + private static enum RocketSide { + RIGHT, LEFT + } + + public ArrayList GetContours() { NetworkTableEntry ntwidths = visionTable.getEntry("width"); NetworkTableEntry ntheights = visionTable.getEntry("height"); NetworkTableEntry ntxs = visionTable.getEntry("x"); @@ -53,17 +64,26 @@ public ArrayList> GetContours() { if (ratios.length < smallestLen) { smallestLen = ratios.length; } if (areas.length < smallestLen) { smallestLen = areas.length; } - ArrayList> contours = new ArrayList>(); + ArrayList contours = new ArrayList(); if (smallestLen > 0) { for (int i = 0; i < smallestLen; i++) { - ArrayList newContour = new ArrayList(); + /*ArrayList newContour = new ArrayList(); newContour.add((double)xs[i]); newContour.add((double)ys[i]); newContour.add((double)widths[i]); newContour.add((double)heights[i]); newContour.add((double)areas[i]); - newContour.add((double)ratios[i]); + newContour.add((double)ratios[i]);*/ + + Contour newContour = new Contour( + (double)xs[i], + (double)ys[i], + (double)widths[i], + (double)heights[i], + (double)areas[i], + (double)ratios[i] + ); // Add this contour to the main list of contours // contours.add(newContour); @@ -72,23 +92,23 @@ public ArrayList> GetContours() { return contours; } - public ArrayList> SortContours(ArrayList> contours, int index) { - Collections.sort(contours, new Comparator>() { + public ArrayList SortContours(ArrayList contours, int index) { + Collections.sort(contours, new Comparator() { @Override - public int compare(ArrayList a1, ArrayList a2) { - return a1.get(index).compareTo(a2.get(index)); + public int compare(Contour a1, Contour a2) { + return Double.compare(a1.get(index), a2.get(index)); } }); return contours; } - public ArrayList>> GetContourPairs() { - ArrayList> contours = GetContours(); + public ArrayList GetContourPairs() { + ArrayList contours = GetContours(); // Sort the contours! // contours = SortContours(contours, 1); - ArrayList>> pairs = new ArrayList>>(); + ArrayList pairs = new ArrayList(); for (int i = 0; i < contours.size()-1; i++) { for (int j = i+1; j < contours.size(); j++) { @@ -96,9 +116,7 @@ public ArrayList>> GetContourPairs() { double secondY = contours.get(j).get(1); if ((firstY/secondY) >= 0.9) { - ArrayList> newPair = new ArrayList>(); - newPair.add(contours.get(i)); - newPair.add(contours.get(j)); + ContourPair newPair = new ContourPair(contours.get(i), contours.get(j)); pairs.add(newPair); } @@ -108,27 +126,30 @@ public ArrayList>> GetContourPairs() { return pairs; } - public ArrayList DriveToPair(int mode, double speed) { - // mode 0 is cargo (center), mode 1 is the hatch panel (sides) + public ArrayList DriveToPair(AssistMode mode, double speed) { // speeds.get(0) is the left side of the drive train, speeds.get(1) is the right side ArrayList speeds = new ArrayList(); - ArrayList>> pairs = GetContourPairs(); + ArrayList pairs = GetContourPairs(); if (pairs.size() > 0) { if (pairs.size() > 1) { - // + if (mode == AssistMode.CARGOROCKET) { + + } else if (mode == AssistMode.HATCH) { + + } } else { - ArrayList> pair = SortContours(pairs.get(0), 0); - ArrayList contourA = pair.get(0); - ArrayList contourB = pair.get(1); + ArrayList pair = SortContours(pairs.get(0).getContours(), 0); + Contour contourA = pair.get(0); + Contour contourB = pair.get(1); - double leftX = contourA.get(0) + (double)(contourA.get(2)/2); - double leftY = contourA.get(1) + (double)(contourA.get(3)/2); + double leftX = contourA.getX() + (double)(contourA.getWidth()/2); + double leftY = contourA.getY() + (double)(contourA.getHeight()/2); - double rightX = contourB.get(0) + (double)(contourB.get(2)/2); - double rightY = contourB.get(1) + (double)(contourB.get(3)/2); + double rightX = contourB.getX() + (double)(contourB.getWidth()/2); + double rightY = contourB.getY() + (double)(contourB.getHeight()/2); NetworkTableEntry ntCamWidth = visionTable.getEntry("camWidth"); NetworkTableEntry ntCamHeight = visionTable.getEntry("camHeight"); @@ -142,7 +163,7 @@ public ArrayList DriveToPair(int mode, double speed) { // Make speedScaleConstant bigger if you want robot to slow down over a longer distance, and smaller if you want it to slow down over a shorter distance double speedScaleConstant = 8; - double speedScale = Math.min(speedScaleConstant / (contourA.get(2) + contourB.get(2))/2, 1.0); + double speedScale = Math.min(speedScaleConstant / (contourA.getWidth() + contourB.getWidth()/2), 1.0); double scaledSpeed = Math.min(speed * speedScale, 1.0); // Increase subScale to make adjustment less extreme // @@ -192,4 +213,101 @@ public ArrayList DriveToPair(int mode, double speed) { } return speeds; } + + public ArrayList DriveAlongArc(AssistMode mode, double speed) { + + ArrayList speeds = new ArrayList(); + + + double distanceToRocket = CommandBase.drivetrain.GetDistance(); + double gyroAngle = CommandBase.drivetrain.GetAngle(); + int gyroAngleInt = (int)Math.round(gyroAngle); + + double desiredAngle; + + double width = distanceToRocket * Math.cos(gyroAngle); + double depth = distanceToRocket * Math.sin(gyroAngle); + double moddedAngle = Math.floorMod(gyroAngleInt, 360); + + if (mode == AssistMode.CARGOROCKET) { + + } else if (mode == AssistMode.HATCH) { + if (moddedAngle >= 0 && moddedAngle < 90) { + desiredAngle = 60; + } else if (moddedAngle >= 90 && moddedAngle < 180) { + desiredAngle = 120; + } else if (moddedAngle >= 180 && moddedAngle < 270) { + desiredAngle = 240; + } else { + desiredAngle = 300; + } + + SmartDashboard.putNumber("Desired", desiredAngle); + SmartDashboard.putNumber("Modded", moddedAngle); + // Check to see if we can free up the drive train // + if (Math.abs(moddedAngle - desiredAngle) < 5) { + CommandBase.drivetrain.UnlockAuto(); + speeds.add(0.0); + speeds.add(0.0); + return speeds; + } + + double normalAngle, theta; + if (moddedAngle <= desiredAngle) { + normalAngle = desiredAngle - 90; + theta = 90 - (moddedAngle-normalAngle); + } else { + normalAngle = desiredAngle + 90; + theta = 90 - (normalAngle-moddedAngle); + } + double phi = 90 - theta; + + // Check if our current angle is within 10 degrees of our normal angle + if (Math.abs(moddedAngle - normalAngle%360) > 10) { + // Tell the drivetrain to spin // + double turnSpeed = 0.3; + if (moddedAngle > desiredAngle) { + speeds.add(turnSpeed); + speeds.add(-turnSpeed); + } else { + speeds.add(-turnSpeed); + speeds.add(turnSpeed); + } + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } + + // We can follow the arc now! // + double wheelBase = 23.0; + // New robot 2019 is 21.5;// + + double masterRadius = (depth/2) / Math.sin(theta); + double innerRadius = masterRadius - wheelBase; + double outerRadius = masterRadius + wheelBase; + double omega = speed/masterRadius; + + double innerSpeed = innerRadius * omega; + double outerSpeed = outerRadius * omega; + + if (moddedAngle >= 180) { + // The inside turning circle is on the left // + speeds.add(innerSpeed); + speeds.add(outerSpeed); + } else { + // The inside turning circle is on the right // + speeds.add(outerSpeed); + speeds.add(innerSpeed); + } + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } + + speeds.add(0.0); + speeds.add(0.0); + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; + } } diff --git a/src/main/java/org/team3236/AssistMode.java b/src/main/java/org/team3236/AssistMode.java new file mode 100644 index 0000000..570f9c7 --- /dev/null +++ b/src/main/java/org/team3236/AssistMode.java @@ -0,0 +1,5 @@ +package org.team3236; + +public enum AssistMode { + HATCH, CARGOSHIP, CARGOROCKET +} \ No newline at end of file diff --git a/src/main/java/org/team3236/DriveTrainMode.java b/src/main/java/org/team3236/DriveTrainMode.java new file mode 100644 index 0000000..c97140c --- /dev/null +++ b/src/main/java/org/team3236/DriveTrainMode.java @@ -0,0 +1,5 @@ +package org.team3236; + +public enum DriveTrainMode { + CARGO, HATCH +} \ No newline at end of file diff --git a/src/main/java/org/team3236/contours/Contour.java b/src/main/java/org/team3236/contours/Contour.java new file mode 100644 index 0000000..d197d3a --- /dev/null +++ b/src/main/java/org/team3236/contours/Contour.java @@ -0,0 +1,70 @@ +package org.team3236.contours; +/** + * @author: Eric Bernard + * + */ + + +public class Contour { + private double x, y, width, height, area, ratio; + + public Contour(double x, double y, double width, double height, double area, double ratio) { + this.x = x; + this.y = y; + this.width = width; + this.height = height; + this.area = area; + this.ratio = ratio; + } + + public double get(int index) { + double valToReturn = 0; + switch(index) { + case 0: { + valToReturn = this.x; + break; + } + case 1: { + valToReturn = this.y; + break; + } + case 2: { + valToReturn = this.width; + break; + } + case 3: { + valToReturn = this.height; + break; + } + case 4: { + valToReturn = this.area; + break; + } + case 5: { + valToReturn = this.ratio; + break; + } + } + + return valToReturn; + } + + public double getX() { + return this.x; + } + public double getY() { + return this.y; + } + public double getWidth() { + return this.width; + } + public double getHeight() { + return this.height; + } + public double getArea() { + return this.area; + } + public double getRatio() { + return this.ratio; + } +} \ No newline at end of file diff --git a/src/main/java/org/team3236/contours/ContourPair.java b/src/main/java/org/team3236/contours/ContourPair.java new file mode 100644 index 0000000..db5383b --- /dev/null +++ b/src/main/java/org/team3236/contours/ContourPair.java @@ -0,0 +1,31 @@ +package org.team3236.contours; +/** + * @author: Eric Bernard + * + */ + +import java.util.ArrayList; + +import org.team3236.contours.Contour; + +public class ContourPair { + private ArrayList pair = new ArrayList(); + + public ContourPair() {} + public ContourPair(Contour... contours) { + for (int i=0; i getContours() { + return pair; + } +} \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..80defba --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,33 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.344", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.344" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.344", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file From dd87e3366bcab84a88281da8b25c539d72e9502c Mon Sep 17 00:00:00 2001 From: Tri Force Date: Mon, 21 Jan 2019 15:19:32 -0500 Subject: [PATCH 03/15] kinda working not really. Will fix later --- src/main/java/frc/robot/Robot.java | 189 +++++++++--------- .../frc/robot/commands/ExampleCommand.java | 48 ----- .../java/frc/robot/commands/TeleopEric.java | 67 +++++++ .../java/frc/robot/commands/TeleopVision.java | 3 +- .../java/frc/robot/subsystems/DriveTrain.java | 54 +++-- .../frc/robot/subsystems/VisionRocket.java | 131 +++++++----- src/main/java/org/team3236/Length.java | 5 + .../java/org/team3236/kop/Ultrasonic.java | 46 +++++ 8 files changed, 328 insertions(+), 215 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java create mode 100644 src/main/java/frc/robot/commands/TeleopEric.java create mode 100644 src/main/java/org/team3236/Length.java create mode 100644 src/main/java/org/team3236/kop/Ultrasonic.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2a03350..fcb020e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.ExampleCommand; import frc.robot.commands.TeleopDefault; +import frc.robot.commands.TeleopEric; import frc.robot.commands.TeleopVision; import frc.robot.subsystems.ExampleSubsystem; @@ -25,113 +25,106 @@ * project. */ public class Robot extends TimedRobot { - public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); - public static CommandBase cmdBase; - public static TeleopVision teleop; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + public enum TeleopMode { + VISION, DEFAULT, ERIC + } - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override - public void robotInit() { - cmdBase = new CommandBase(); - //CommandBase.drivetrain.InvertTalons(); - teleop = new TeleopVision(); - m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); - // chooser.addOption("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); - } + // Create a dropdown box for them all // + SendableChooser teleopChooser = new SendableChooser(); - /** - * 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. - * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - } + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + @Override + public void robotInit() { + + } - /** - * 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() { - } + /** + * 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. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + } - @Override - public void disabledPeriodic() { - Scheduler.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() { + System.out.println("Disabled init"); + // Load all of the teleop modes into the chooser // + teleopChooser.setDefaultOption("Default Teleop", TeleopMode.DEFAULT); + teleopChooser.addOption("Vision Tracking Testing", TeleopMode.VISION); + teleopChooser.addOption("Eric's Teleop Testing", TeleopMode.ERIC); - /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. - */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); + // Add the chooser to the Shuffleboard // + SmartDashboard.putData("Teleop Mode", teleopChooser); + } - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ + @Override + public void disabledPeriodic() { + Scheduler.getInstance().run(); + } - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); - } - } + /** + * This autonomous (along with the chooser code above) shows how to select + * between different autonomous modes using the dashboard. The sendable + * chooser code works with the Java SmartDashboard. If you prefer the + * LabVIEW Dashboard, remove all of the chooser code and uncomment the + * getString code to get the auto name from the text box below the Gyro + * + *

You can add additional auto modes by adding additional commands to the + * chooser code above (like the commented example) or additional comparisons + * to the switch structure below with additional strings & commands. + */ + @Override + public void autonomousInit() { + + } - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - } + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + } - @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(); - } - teleop.start(); - } + @Override + public void teleopInit() { + // Get what the teleop chooser says // + TeleopMode selectedMode = teleopChooser.getSelected(); + if (selectedMode == TeleopMode.DEFAULT) { + (new TeleopDefault()).start(); + } else if (selectedMode == TeleopMode.ERIC) { + (new TeleopEric()).start(); + } else if (selectedMode == TeleopMode.VISION) { + (new TeleopVision()).start(); + } + } - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() { - Scheduler.getInstance().run(); - } + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + Scheduler.getInstance().run(); + } - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } } diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 83a3fcc..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; - -/** - * An example command. You can replace me with your own command. - */ -public class ExampleCommand extends Command { - public ExampleCommand() { - // Use requires() here to declare subsystem dependencies - requires(Robot.m_subsystem); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - } -} diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java new file mode 100644 index 0000000..83daed3 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.GenericHID.Hand; +import org.team3236.DriveTrainMode; +import frc.robot.CommandBase; + +public class TeleopEric extends Command { + + boolean canSwitchDriveMode = true; + public TeleopEric() { + + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + CommandBase.drivetrain.Initialize(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + // Check to see if we should switch drive modes // + boolean leftStickDown = CommandBase.controls.Driver.getStickButton(Hand.kLeft); + boolean rightStickDown = CommandBase.controls.Driver.getStickButton(Hand.kRight); + + if (leftStickDown && rightStickDown) { + if (canSwitchDriveMode) { + CommandBase.drivetrain.SwitchDriveMode(); + canSwitchDriveMode = false; + } + } else { + // So that the drive mode isn't switching over and over again while both buttons are pressed // + canSwitchDriveMode = true; + } + + double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); + double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); + + CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java index fe5c5a0..113a5b2 100644 --- a/src/main/java/frc/robot/commands/TeleopVision.java +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -24,12 +24,13 @@ public TeleopVision() { // Called just before this Command runs the first time @Override protected void initialize() { + CommandBase.drivetrain.Initialize(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - ArrayList speeds = CommandBase.visionRocket.DriveAlongArc(AssistMode.HATCH, 0.4); + ArrayList speeds = CommandBase.visionRocket.DriveAlongArc(AssistMode.HATCH, 0.3); SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.GetAngle()); CommandBase.drivetrain.Drive(speeds.get(0), speeds.get(1)); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index f97dc14..3f8b3ae 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -8,6 +8,7 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj.SerialPort.*; import org.team3236.DriveTrainMode; +import org.team3236.kop.Ultrasonic; /** * An example subsystem. You can replace me with your own Subsystem. */ @@ -33,27 +35,33 @@ public class DriveTrain extends Subsystem { private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); - SerialPort Ultrasonic; + Ultrasonic UltrasonicSensor; private static AHRS NavX = new AHRS(SPI.Port.kMXP); private boolean autoLocked = false; - private DriveTrainMode DriveMode = DriveTrainMode.HATCH; + private DriveTrainMode driveMode = DriveTrainMode.CARGO; public void Initialize() { // Set up the ultrasonic sensor // - try { - Ultrasonic = new SerialPort(9600, SerialPort.Port.kOnboard, 8, Parity.kNone, StopBits.kOne); - } catch (RuntimeException ex) { - throw ex; - } - Ultrasonic.setTimeout(2); - Ultrasonic.setReadBufferSize(6); - } + UltrasonicSensor = new Ultrasonic(9600); - public double GetDistance() { + // Reset the Gyro to 0 degrees // + this.ResetGyro(); + + // Put the drivemode on the display // + this.UpdateSmartDashboard(); + } + public void UpdateSmartDashboard() { + if (this.driveMode == DriveTrainMode.CARGO) { + SmartDashboard.putString("Drive Mode", "CARGO"); + } else { + SmartDashboard.putString("Drive Mode", "HATCH"); + } + } - return 0.0; + public String GetDistance() { + return ""; } public double GetAngle() { @@ -68,8 +76,26 @@ public void ResetGyro() { NavX.reset(); } + public void SetDriveMode(DriveTrainMode newMode) { + this.driveMode = newMode; + this.UpdateSmartDashboard(); + } + + public void SwitchDriveMode() { + if (this.driveMode == DriveTrainMode.HATCH) { + this.driveMode = DriveTrainMode.CARGO; + } else { + this.driveMode = DriveTrainMode.HATCH; + } + this.UpdateSmartDashboard(); + } + + public DriveTrainMode GetDriveMode() { + return this.driveMode; + } + public void SetLeft(double speed) { - if (DriveMode == DriveTrainMode.HATCH) { + if (driveMode == DriveTrainMode.HATCH) { LeftTalonA.set(speed); LeftTalonB.set(speed); } else { @@ -79,7 +105,7 @@ public void SetLeft(double speed) { } public void SetRight(double speed) { - if (DriveMode == DriveTrainMode.HATCH) { + if (driveMode == DriveTrainMode.HATCH) { RightTalonA.set(-speed); RightTalonB.set(-speed); } else { diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index ca0a0ea..8691acb 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -34,6 +34,12 @@ public void initDefaultCommand() { private static NetworkTableInstance ntInst = NetworkTableInstance.getDefault(); private static NetworkTable visionTable = ntInst.getTable("contours"); + private boolean drivingAlongArc = false; + private boolean discoveredDesiredAngle = false; + private int desiredAngle = 0; + private double masterRadius = 0; + private double theta = 0; + private int normalAngle = 0; private static enum RocketSide { RIGHT, LEFT @@ -219,54 +225,50 @@ public ArrayList DriveAlongArc(AssistMode mode, double speed) { ArrayList speeds = new ArrayList(); - double distanceToRocket = CommandBase.drivetrain.GetDistance(); + double distanceToRocket = 72.0; // In inches double gyroAngle = CommandBase.drivetrain.GetAngle(); int gyroAngleInt = (int)Math.round(gyroAngle); - double desiredAngle; - double width = distanceToRocket * Math.cos(gyroAngle); double depth = distanceToRocket * Math.sin(gyroAngle); double moddedAngle = Math.floorMod(gyroAngleInt, 360); + double moddedNormal; if (mode == AssistMode.CARGOROCKET) { } else if (mode == AssistMode.HATCH) { - if (moddedAngle >= 0 && moddedAngle < 90) { - desiredAngle = 60; - } else if (moddedAngle >= 90 && moddedAngle < 180) { - desiredAngle = 120; - } else if (moddedAngle >= 180 && moddedAngle < 270) { - desiredAngle = 240; - } else { - desiredAngle = 300; + + if (!discoveredDesiredAngle) { + if (moddedAngle >= 0 && moddedAngle < 90) { + desiredAngle = 60; + } else if (moddedAngle >= 90 && moddedAngle < 180) { + desiredAngle = 120; + } else if (moddedAngle >= 180 && moddedAngle < 270) { + desiredAngle = 240; + } else { + desiredAngle = 300; + } + if (moddedAngle <= desiredAngle) { + normalAngle = desiredAngle - 90; + theta = 90 - (moddedAngle-normalAngle); + } else { + normalAngle = desiredAngle + 90; + theta = 90 - (normalAngle-moddedAngle); + } + moddedNormal = Math.floorMod(normalAngle, 360); + discoveredDesiredAngle = true; } + SmartDashboard.putNumber("Desired", desiredAngle); SmartDashboard.putNumber("Modded", moddedAngle); - // Check to see if we can free up the drive train // - if (Math.abs(moddedAngle - desiredAngle) < 5) { - CommandBase.drivetrain.UnlockAuto(); - speeds.add(0.0); - speeds.add(0.0); - return speeds; - } - - double normalAngle, theta; - if (moddedAngle <= desiredAngle) { - normalAngle = desiredAngle - 90; - theta = 90 - (moddedAngle-normalAngle); - } else { - normalAngle = desiredAngle + 90; - theta = 90 - (normalAngle-moddedAngle); - } - double phi = 90 - theta; // Check if our current angle is within 10 degrees of our normal angle - if (Math.abs(moddedAngle - normalAngle%360) > 10) { + System.out.println("Normal: " + normalAngle); + if (!drivingAlongArc && Math.abs(moddedAngle - normalAngle) > 5) { // Tell the drivetrain to spin // double turnSpeed = 0.3; - if (moddedAngle > desiredAngle) { + if (moddedAngle < normalAngle) { speeds.add(turnSpeed); speeds.add(-turnSpeed); } else { @@ -276,32 +278,53 @@ public ArrayList DriveAlongArc(AssistMode mode, double speed) { SmartDashboard.putNumber("Speed Left", speeds.get(0)); SmartDashboard.putNumber("Speed Right", speeds.get(1)); return speeds; - } - - // We can follow the arc now! // - double wheelBase = 23.0; - // New robot 2019 is 21.5;// - - double masterRadius = (depth/2) / Math.sin(theta); - double innerRadius = masterRadius - wheelBase; - double outerRadius = masterRadius + wheelBase; - double omega = speed/masterRadius; - - double innerSpeed = innerRadius * omega; - double outerSpeed = outerRadius * omega; - - if (moddedAngle >= 180) { - // The inside turning circle is on the left // - speeds.add(innerSpeed); - speeds.add(outerSpeed); } else { - // The inside turning circle is on the right // - speeds.add(outerSpeed); - speeds.add(innerSpeed); + if (!drivingAlongArc) { + masterRadius = (depth/2) / Math.sin(theta); + } + drivingAlongArc = true; + // We can follow the arc now! // + double wheelBase = 23.0; + // New robot 2019 is 21.5;// + + double innerRadius = masterRadius - wheelBase; + double outerRadius = masterRadius + wheelBase; + double omega = speed/masterRadius; + + /*System.out.println("Ultrasonic " + CommandBase.drivetrain.GetDistance()); + System.out.println("Depth: " + depth); + System.out.println("Theta: " + theta); + System.out.println("Master rad: " + masterRadius); + System.out.println("Inner rad: " + innerRadius); + System.out.println("Outer rad: " + outerRadius); + System.out.println("Omega: " + omega);*/ + + double innerSpeed = -(innerRadius * omega); + double outerSpeed = -(outerRadius * omega); + + + if (Math.abs(gyroAngle - normalAngle) < 5) { + speeds.add(0.0); + speeds.add(0.0); + return speeds; + } + if (moddedAngle >= 180) { + // The inside turning circle is on the left // + speeds.add(innerSpeed); + speeds.add(outerSpeed); + } else { + // The inside turning circle is on the right // + speeds.add(outerSpeed); + speeds.add(innerSpeed); + } + + System.out.println("Speeds: I(" + innerSpeed + ") O(" + outerSpeed +")"); + SmartDashboard.putNumber("Speed Left", speeds.get(0)); + SmartDashboard.putNumber("Speed Right", speeds.get(1)); + return speeds; } - SmartDashboard.putNumber("Speed Left", speeds.get(0)); - SmartDashboard.putNumber("Speed Right", speeds.get(1)); - return speeds; + + } speeds.add(0.0); diff --git a/src/main/java/org/team3236/Length.java b/src/main/java/org/team3236/Length.java new file mode 100644 index 0000000..2879733 --- /dev/null +++ b/src/main/java/org/team3236/Length.java @@ -0,0 +1,5 @@ +package org.team3236; + +public enum Length { + INCH +} \ No newline at end of file diff --git a/src/main/java/org/team3236/kop/Ultrasonic.java b/src/main/java/org/team3236/kop/Ultrasonic.java new file mode 100644 index 0000000..e906da4 --- /dev/null +++ b/src/main/java/org/team3236/kop/Ultrasonic.java @@ -0,0 +1,46 @@ +package org.team3236.kop; + +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.SerialPort.Parity; +import edu.wpi.first.wpilibj.SerialPort.StopBits; + +public class Ultrasonic { + private SerialPort serialPort; + private Thread rangingThread; + private String distance = "0"; + + public Ultrasonic(int port) { + try { + serialPort = new SerialPort(port, SerialPort.Port.kOnboard, 8, Parity.kNone, StopBits.kOne); + } catch (RuntimeException ex) { + throw ex; + } + + serialPort.setTimeout(2); + serialPort.reset(); + + // Start the thread // + rangingThread = new Thread(this::rangingLoop); + rangingThread.start(); + + } + + public double getDistance() { + return 0.0; + } + + // Method to run in the thread // + private void rangingLoop() { + while (true) { + String response; + try { + response = serialPort.readString(5); + //System.out.println("Serial port: " + response); + } catch (StringIndexOutOfBoundsException ex) { + ex.printStackTrace(); + continue; + } + + } + } +} \ No newline at end of file From 763700e7ddef930df852a837aa7863d899dc094c Mon Sep 17 00:00:00 2001 From: Tri Force Date: Sat, 26 Jan 2019 14:19:05 -0500 Subject: [PATCH 04/15] Moving to faster laptop --- src/main/java/frc/robot/CommandBase.java | 1 + src/main/java/frc/robot/RobotMap.java | 3 + .../java/frc/robot/commands/TeleopEric.java | 8 ++- .../java/frc/robot/subsystems/DriveTrain.java | 17 ++--- .../java/frc/robot/subsystems/Elevator.java | 65 +++++++++++++++++++ src/main/java/org/team3236/Conversion.java | 50 ++++++++++++++ src/main/java/org/team3236/kop/MB1013.java | 39 +++++++++++ .../java/org/team3236/kop/Ultrasonic.java | 46 ------------- 8 files changed, 171 insertions(+), 58 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Elevator.java create mode 100644 src/main/java/org/team3236/Conversion.java create mode 100644 src/main/java/org/team3236/kop/MB1013.java delete mode 100644 src/main/java/org/team3236/kop/Ultrasonic.java diff --git a/src/main/java/frc/robot/CommandBase.java b/src/main/java/frc/robot/CommandBase.java index deef42e..0d2c8fe 100644 --- a/src/main/java/frc/robot/CommandBase.java +++ b/src/main/java/frc/robot/CommandBase.java @@ -13,5 +13,6 @@ public class CommandBase { public static VisionRocket visionRocket = new VisionRocket(); public static OI controls = new OI(); public static DriveTrain drivetrain = new DriveTrain(); + public static Elevator elevator = new Elevator(); } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index e968054..e5aec1e 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -26,6 +26,9 @@ public class RobotMap { public static int RIGHTTALONA = 2; public static int RIGHTTALONB = 3; + public static int ELEVATORTALON = 4; + public static int ELEVATORTALONENC = 5; + // If you are using multiple modules, make sure to define both the port // number and the module. For example you with a rangefinder: diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 83daed3..515847b 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.team3236.DriveTrainMode; import frc.robot.CommandBase; @@ -31,6 +32,7 @@ protected void execute() { // Check to see if we should switch drive modes // boolean leftStickDown = CommandBase.controls.Driver.getStickButton(Hand.kLeft); boolean rightStickDown = CommandBase.controls.Driver.getStickButton(Hand.kRight); + int dpad = CommandBase.controls.Driver.getPOV(); if (leftStickDown && rightStickDown) { if (canSwitchDriveMode) { @@ -42,10 +44,14 @@ protected void execute() { canSwitchDriveMode = true; } + SmartDashboard.putNumber("elevator", CommandBase.elevator.getRawEncoder()); + SmartDashboard.putNumber("Ultrasonic", CommandBase.drivetrain.GetDistance()); + double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); - CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); + //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); + CommandBase.elevator.set(leftSpeed/2); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 3f8b3ae..5b1fe5f 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -10,19 +10,15 @@ import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - import com.kauailabs.navx.frc.*; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.SerialPort.*; import org.team3236.DriveTrainMode; -import org.team3236.kop.Ultrasonic; +import org.team3236.Conversion; +import org.team3236.kop.MB1013; /** * An example subsystem. You can replace me with your own Subsystem. */ @@ -35,15 +31,13 @@ public class DriveTrain extends Subsystem { private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); - Ultrasonic UltrasonicSensor; + private static MB1013 ultrasonic = new MB1013(); private static AHRS NavX = new AHRS(SPI.Port.kMXP); private boolean autoLocked = false; private DriveTrainMode driveMode = DriveTrainMode.CARGO; public void Initialize() { - // Set up the ultrasonic sensor // - UltrasonicSensor = new Ultrasonic(9600); // Reset the Gyro to 0 degrees // this.ResetGyro(); @@ -60,8 +54,8 @@ public void UpdateSmartDashboard() { } } - public String GetDistance() { - return ""; + public double GetDistance() { + return ultrasonic.getDistance(); } public double GetAngle() { @@ -117,6 +111,7 @@ public void SetRight(double speed) { public void Drive(double leftSpeed, double rightSpeed){ SetLeft(leftSpeed); SetRight(rightSpeed); + } public void LockAuto() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..d422e6d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.RobotMap; +import org.team3236.Conversion; + +public class Elevator extends Subsystem { + + private WPI_TalonSRX talon, talonWithEncoder; + public Conversion conversion; + + public Elevator() { + talon = new WPI_TalonSRX(RobotMap.ELEVATORTALON); + talonWithEncoder = new WPI_TalonSRX(RobotMap.ELEVATORTALONENC); + + talonWithEncoder.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + conversion = new Conversion(40, Conversion.Units.IN); // Change the 40 later // + } + + public void zeroEncoder() { + talonWithEncoder.setSelectedSensorPosition(0); + } + + /** + * Gets the encoder value in ticks + * @return Distance of the encoder in ticks + */ + public int getRawEncoder() { + return -talonWithEncoder.getSelectedSensorPosition(); + } + + /** + * Gets the encoder value in inches + * @return Distance of the encoder in inches + */ + public double getEncoder() { + return conversion.getInches(getRawEncoder()); + } + + public void ascend(double speed) { + talon.set(speed); + talonWithEncoder.set(-speed); + } + + public void descend(double speed) { + talon.set(-speed); + talonWithEncoder.set(speed); + } + + public void set(double speed) { + if (speed > 0) { + + } else { + + } + } + + + + public void initDefaultCommand() {} +} \ No newline at end of file diff --git a/src/main/java/org/team3236/Conversion.java b/src/main/java/org/team3236/Conversion.java new file mode 100644 index 0000000..71d0d26 --- /dev/null +++ b/src/main/java/org/team3236/Conversion.java @@ -0,0 +1,50 @@ +package org.team3236; + +public class Conversion { + + public enum Units { + IN(1.0), FT(1/12), MM(1/25.4), CM(1/2.54), M(39.37); + + private double convVal; + + Units(double conversion) { + this.convVal = conversion; + } + + // Returns the conversion factor relative to 1 inch + public double getConversionFactor() { + return this.convVal; + } + } + + private double baseUnits; + private Units unitType; + + /** + * Creates a new conversion factor based on number of ticks per unit + * @param base the number of ticks + * @param unit a unit conversion factor + */ + public Conversion(double base, Units unit) { + baseUnits = base; + unitType = unit; + } + + /** + * Converts raw value into inches based on given units + * @param rawVal the raw value (i.e., ticks on an encoder) + * @return number of inches that is based on the units given on instantiation + */ + public double getInches(double rawVal) { + return (rawVal/this.baseUnits) * unitType.getConversionFactor(); + } + + /** + * Returns raw value based on conversion's units + * @param value the amount of units to convert + * @return number of ticks per distance unit + */ + public double getRawValue(double value) { + return value * baseUnits; + } +} \ No newline at end of file diff --git a/src/main/java/org/team3236/kop/MB1013.java b/src/main/java/org/team3236/kop/MB1013.java new file mode 100644 index 0000000..779e344 --- /dev/null +++ b/src/main/java/org/team3236/kop/MB1013.java @@ -0,0 +1,39 @@ +package org.team3236.kop; + +import edu.wpi.first.wpilibj.AnalogInput; +import org.team3236.Conversion; +// MaxBotix MB1013 Ultrasonic Sensor // +public class MB1013 { + + private AnalogInput ultrasonicPort; + private Conversion conversionFactor; + + // Default constructor, no arguments // + public MB1013() { + ultrasonicPort = new AnalogInput(1); + + ultrasonicPort.setAverageBits(50); + ultrasonicPort.setOversampleBits(3); + + conversionFactor = new Conversion(1, Conversion.Units.M); + AnalogInput.setGlobalSampleRate(44400); + } + + // Constructor with custom ports defined // + public MB1013(int ultrasonic, int baseline) { + ultrasonicPort = new AnalogInput(ultrasonic); + + AnalogInput.setGlobalSampleRate(44400); + } + + // Retuns in inches! // + public double getDistance() { + + double volts = ultrasonicPort.getAverageVoltage(); + //double distance = conversionFactor.getInches(volts); + double distance = volts * 1.14045; + + return distance; + } + +} \ No newline at end of file diff --git a/src/main/java/org/team3236/kop/Ultrasonic.java b/src/main/java/org/team3236/kop/Ultrasonic.java deleted file mode 100644 index e906da4..0000000 --- a/src/main/java/org/team3236/kop/Ultrasonic.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.team3236.kop; - -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.SerialPort.Parity; -import edu.wpi.first.wpilibj.SerialPort.StopBits; - -public class Ultrasonic { - private SerialPort serialPort; - private Thread rangingThread; - private String distance = "0"; - - public Ultrasonic(int port) { - try { - serialPort = new SerialPort(port, SerialPort.Port.kOnboard, 8, Parity.kNone, StopBits.kOne); - } catch (RuntimeException ex) { - throw ex; - } - - serialPort.setTimeout(2); - serialPort.reset(); - - // Start the thread // - rangingThread = new Thread(this::rangingLoop); - rangingThread.start(); - - } - - public double getDistance() { - return 0.0; - } - - // Method to run in the thread // - private void rangingLoop() { - while (true) { - String response; - try { - response = serialPort.readString(5); - //System.out.println("Serial port: " + response); - } catch (StringIndexOutOfBoundsException ex) { - ex.printStackTrace(); - continue; - } - - } - } -} \ No newline at end of file From 774e7155f3ec26fcbdf4b81257ba7816ca3c7642 Mon Sep 17 00:00:00 2001 From: Tri Force Date: Sat, 2 Feb 2019 10:26:43 -0500 Subject: [PATCH 05/15] moving laptops --- .../frc/robot/commands/TeleopDefault.java | 2 +- .../java/frc/robot/commands/TeleopEric.java | 5 +- .../java/frc/robot/commands/TeleopVision.java | 6 +- .../java/frc/robot/subsystems/DriveTrain.java | 63 ++++++------------- .../java/frc/robot/subsystems/Elevator.java | 26 +++++--- src/main/java/org/team3236/Length.java | 5 -- src/main/java/org/team3236/kop/MB1013.java | 39 ------------ 7 files changed, 40 insertions(+), 106 deletions(-) delete mode 100644 src/main/java/org/team3236/Length.java delete mode 100644 src/main/java/org/team3236/kop/MB1013.java diff --git a/src/main/java/frc/robot/commands/TeleopDefault.java b/src/main/java/frc/robot/commands/TeleopDefault.java index 26dbc13..20253e0 100644 --- a/src/main/java/frc/robot/commands/TeleopDefault.java +++ b/src/main/java/frc/robot/commands/TeleopDefault.java @@ -35,7 +35,7 @@ protected void execute() { CommandBase.visionRocket.GetContourPairs(); - CommandBase.drivetrain.Drive(lateralSpeed - forwardSpeed, lateralSpeed + forwardSpeed); + CommandBase.drivetrain.drive(lateralSpeed - forwardSpeed, lateralSpeed + forwardSpeed); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 515847b..2d0eb46 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -23,7 +23,7 @@ public TeleopEric() { // Called just before this Command runs the first time @Override protected void initialize() { - CommandBase.drivetrain.Initialize(); + } // Called repeatedly when this Command is scheduled to run @@ -36,7 +36,7 @@ protected void execute() { if (leftStickDown && rightStickDown) { if (canSwitchDriveMode) { - CommandBase.drivetrain.SwitchDriveMode(); + CommandBase.drivetrain.switchDriveMode(); canSwitchDriveMode = false; } } else { @@ -45,7 +45,6 @@ protected void execute() { } SmartDashboard.putNumber("elevator", CommandBase.elevator.getRawEncoder()); - SmartDashboard.putNumber("Ultrasonic", CommandBase.drivetrain.GetDistance()); double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java index 113a5b2..a8d4393 100644 --- a/src/main/java/frc/robot/commands/TeleopVision.java +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -24,16 +24,16 @@ public TeleopVision() { // Called just before this Command runs the first time @Override protected void initialize() { - CommandBase.drivetrain.Initialize(); + } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { ArrayList speeds = CommandBase.visionRocket.DriveAlongArc(AssistMode.HATCH, 0.3); - SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.GetAngle()); + SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.getAngle()); - CommandBase.drivetrain.Drive(speeds.get(0), speeds.get(1)); + CommandBase.drivetrain.drive(speeds.get(0), speeds.get(1)); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 5b1fe5f..99cc3a6 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -18,35 +18,27 @@ import org.team3236.DriveTrainMode; import org.team3236.Conversion; -import org.team3236.kop.MB1013; -/** - * An example subsystem. You can replace me with your own Subsystem. - */ public class DriveTrain extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. private WPI_TalonSRX LeftTalonA = new WPI_TalonSRX(RobotMap.LEFTTALONA); private WPI_TalonSRX LeftTalonB = new WPI_TalonSRX(RobotMap.LEFTTALONB); private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); - private static MB1013 ultrasonic = new MB1013(); private static AHRS NavX = new AHRS(SPI.Port.kMXP); - private boolean autoLocked = false; private DriveTrainMode driveMode = DriveTrainMode.CARGO; - public void Initialize() { + public DriveTrain() { // Reset the Gyro to 0 degrees // - this.ResetGyro(); + this.resetGyro(); // Put the drivemode on the display // - this.UpdateSmartDashboard(); + this.updateSmartDashboard(); } - public void UpdateSmartDashboard() { + public void updateSmartDashboard() { if (this.driveMode == DriveTrainMode.CARGO) { SmartDashboard.putString("Drive Mode", "CARGO"); } else { @@ -54,41 +46,37 @@ public void UpdateSmartDashboard() { } } - public double GetDistance() { - return ultrasonic.getDistance(); - } - - public double GetAngle() { + public double getAngle() { return NavX.getAngle(); } - public double GetPitch() { + public double getPitch() { return NavX.getPitch(); } - public void ResetGyro() { + public void resetGyro() { NavX.reset(); } - public void SetDriveMode(DriveTrainMode newMode) { + public void setDriveMode(DriveTrainMode newMode) { this.driveMode = newMode; - this.UpdateSmartDashboard(); + this.updateSmartDashboard(); } - public void SwitchDriveMode() { + public void switchDriveMode() { if (this.driveMode == DriveTrainMode.HATCH) { this.driveMode = DriveTrainMode.CARGO; } else { this.driveMode = DriveTrainMode.HATCH; } - this.UpdateSmartDashboard(); + this.updateSmartDashboard(); } - public DriveTrainMode GetDriveMode() { + public DriveTrainMode getDriveMode() { return this.driveMode; } - public void SetLeft(double speed) { + public void setLeft(double speed) { if (driveMode == DriveTrainMode.HATCH) { LeftTalonA.set(speed); LeftTalonB.set(speed); @@ -98,7 +86,7 @@ public void SetLeft(double speed) { } } - public void SetRight(double speed) { + public void setRight(double speed) { if (driveMode == DriveTrainMode.HATCH) { RightTalonA.set(-speed); RightTalonB.set(-speed); @@ -108,29 +96,14 @@ public void SetRight(double speed) { } } - public void Drive(double leftSpeed, double rightSpeed){ - SetLeft(leftSpeed); - SetRight(rightSpeed); + public void drive(double leftSpeed, double rightSpeed){ + setLeft(leftSpeed); + setRight(rightSpeed); } - public void LockAuto() { - autoLocked = true; - } - - public void UnlockAuto() { - autoLocked = false; - } - - public boolean IsAutoLocked() { - return autoLocked; - } - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } + public void initDefaultCommand() {} } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index d422e6d..4d0c1fb 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -12,6 +12,8 @@ public class Elevator extends Subsystem { private WPI_TalonSRX talon, talonWithEncoder; public Conversion conversion; + private static final double MAXHEIGHT = 4000; // In ticks! + public Elevator() { talon = new WPI_TalonSRX(RobotMap.ELEVATORTALON); talonWithEncoder = new WPI_TalonSRX(RobotMap.ELEVATORTALONENC); @@ -25,18 +27,10 @@ public void zeroEncoder() { talonWithEncoder.setSelectedSensorPosition(0); } - /** - * Gets the encoder value in ticks - * @return Distance of the encoder in ticks - */ public int getRawEncoder() { return -talonWithEncoder.getSelectedSensorPosition(); } - /** - * Gets the encoder value in inches - * @return Distance of the encoder in inches - */ public double getEncoder() { return conversion.getInches(getRawEncoder()); } @@ -47,8 +41,20 @@ public void ascend(double speed) { } public void descend(double speed) { - talon.set(-speed); - talonWithEncoder.set(speed); + // If the encoder is reading less than an inch from the bottom + if (getEncoder() < 1) { + talon.set(0); + talonWithEncoder.set(0); + } else { + talon.set(-speed); + talonWithEncoder.set(speed); + } + } + + public void goTo(double rawPosition, double rawSpeed) { + // Set the position to the top if its higher than the maxheight + double destination = Math.min(rawPosition, MAXHEIGHT); + double current = getRawEncoder(); } public void set(double speed) { diff --git a/src/main/java/org/team3236/Length.java b/src/main/java/org/team3236/Length.java deleted file mode 100644 index 2879733..0000000 --- a/src/main/java/org/team3236/Length.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.team3236; - -public enum Length { - INCH -} \ No newline at end of file diff --git a/src/main/java/org/team3236/kop/MB1013.java b/src/main/java/org/team3236/kop/MB1013.java deleted file mode 100644 index 779e344..0000000 --- a/src/main/java/org/team3236/kop/MB1013.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.team3236.kop; - -import edu.wpi.first.wpilibj.AnalogInput; -import org.team3236.Conversion; -// MaxBotix MB1013 Ultrasonic Sensor // -public class MB1013 { - - private AnalogInput ultrasonicPort; - private Conversion conversionFactor; - - // Default constructor, no arguments // - public MB1013() { - ultrasonicPort = new AnalogInput(1); - - ultrasonicPort.setAverageBits(50); - ultrasonicPort.setOversampleBits(3); - - conversionFactor = new Conversion(1, Conversion.Units.M); - AnalogInput.setGlobalSampleRate(44400); - } - - // Constructor with custom ports defined // - public MB1013(int ultrasonic, int baseline) { - ultrasonicPort = new AnalogInput(ultrasonic); - - AnalogInput.setGlobalSampleRate(44400); - } - - // Retuns in inches! // - public double getDistance() { - - double volts = ultrasonicPort.getAverageVoltage(); - //double distance = conversionFactor.getInches(volts); - double distance = volts * 1.14045; - - return distance; - } - -} \ No newline at end of file From 6fe488bea9cc8d15111e9029a735b4b3d435afbd Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Fri, 8 Feb 2019 16:17:05 +0000 Subject: [PATCH 06/15] Use Black Magic to make Vision work again --- CloneRepo.bat | 6 +++++ src/main/java/frc/robot/Robot.java | 6 ++++- .../robot/commands/TeleopTriggerControl.java | 7 +++--- .../java/frc/robot/commands/TeleopVision.java | 7 ++++-- .../java/frc/robot/subsystems/DriveTrain.java | 1 + .../java/frc/robot/subsystems/UltraSonic.java | 24 +++++++++++++++++++ .../frc/robot/subsystems/VisionRocket.java | 17 ++++++++----- vendordeps/navx_frc.json | 6 ++--- 8 files changed, 59 insertions(+), 15 deletions(-) create mode 100644 CloneRepo.bat create mode 100644 src/main/java/frc/robot/subsystems/UltraSonic.java diff --git a/CloneRepo.bat b/CloneRepo.bat new file mode 100644 index 0000000..8a6f229 --- /dev/null +++ b/CloneRepo.bat @@ -0,0 +1,6 @@ +del ./src/ +git init +git remote add origin https://github.com/FRC3236/DeepSpace.git +git fetch +git pull --rebase +git checkout -t origin/vision -b vision \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fcb020e..b3bcbe5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -103,7 +103,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { // Get what the teleop chooser says // - TeleopMode selectedMode = teleopChooser.getSelected(); + /*TeleopMode selectedMode = teleopChooser.getSelected(); if (selectedMode == TeleopMode.DEFAULT) { (new TeleopDefault()).start(); } else if (selectedMode == TeleopMode.ERIC) { @@ -111,6 +111,10 @@ public void teleopInit() { } else if (selectedMode == TeleopMode.VISION) { (new TeleopVision()).start(); } + else{ + new TeleopVision().start(); + } */ + new TeleopVision().start(); } /** diff --git a/src/main/java/frc/robot/commands/TeleopTriggerControl.java b/src/main/java/frc/robot/commands/TeleopTriggerControl.java index 39774ad..64a337d 100644 --- a/src/main/java/frc/robot/commands/TeleopTriggerControl.java +++ b/src/main/java/frc/robot/commands/TeleopTriggerControl.java @@ -39,18 +39,19 @@ protected void execute() { if(!forward_stop){ if (forward_acceleration){ - CommandBase.drivetrain.Drive(multiplier, -multiplier); + CommandBase.drivetrain.drive(multiplier, -multiplier); + } } if(!backward_stop){ if (backward_acceleration){ - CommandBase.drivetrain.Drive(-multiplier, multiplier); + CommandBase.drivetrain.drive(-multiplier, multiplier); } } if (forward_stop || backward_stop){ - CommandBase.drivetrain.Drive(0, 0); + CommandBase.drivetrain.drive(0, 0); } //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); diff --git a/src/main/java/frc/robot/commands/TeleopVision.java b/src/main/java/frc/robot/commands/TeleopVision.java index a8d4393..dbeccb8 100644 --- a/src/main/java/frc/robot/commands/TeleopVision.java +++ b/src/main/java/frc/robot/commands/TeleopVision.java @@ -30,10 +30,13 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - ArrayList speeds = CommandBase.visionRocket.DriveAlongArc(AssistMode.HATCH, 0.3); + System.out.println("Running teleop vision"); + ArrayList speeds = CommandBase.visionRocket.DriveToPair(AssistMode.CARGOSHIP, + 0.75); + System.out.println(speeds); SmartDashboard.putNumber("Gyro", CommandBase.drivetrain.getAngle()); + CommandBase.drivetrain.drive(-speeds.get(0), speeds.get(1)); - CommandBase.drivetrain.drive(speeds.get(0), speeds.get(1)); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 99cc3a6..2dfd069 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -27,6 +27,7 @@ public class DriveTrain extends Subsystem { private static AHRS NavX = new AHRS(SPI.Port.kMXP); + private DriveTrainMode driveMode = DriveTrainMode.CARGO; public DriveTrain() { diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java new file mode 100644 index 0000000..a2d45c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/UltraSonic.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.subsystems; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * Add your docs here. + */ +public class UltraSonic extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index 8691acb..83a9376 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -112,7 +112,7 @@ public ArrayList GetContourPairs() { ArrayList contours = GetContours(); // Sort the contours! // - contours = SortContours(contours, 1); + contours = SortContours(contours, 1); ArrayList pairs = new ArrayList(); @@ -128,7 +128,7 @@ public ArrayList GetContourPairs() { } } - } + } return pairs; } @@ -136,14 +136,15 @@ public ArrayList DriveToPair(AssistMode mode, double speed) { // speeds.get(0) is the left side of the drive train, speeds.get(1) is the right side ArrayList speeds = new ArrayList(); - ArrayList pairs = GetContourPairs(); + if (pairs.size() > 0) { if (pairs.size() > 1) { if (mode == AssistMode.CARGOROCKET) { - } else if (mode == AssistMode.HATCH) { + } + else if (mode == AssistMode.HATCH) { } } else { @@ -167,8 +168,12 @@ public ArrayList DriveToPair(AssistMode mode, double speed) { double offset = pairX - cameraX; + double width_X = (rightX-leftX); // Use this scale for distance + SmartDashboard.putNumber("Midpoint Distance:", width_X); + + // Make speedScaleConstant bigger if you want robot to slow down over a longer distance, and smaller if you want it to slow down over a shorter distance - double speedScaleConstant = 8; + double speedScaleConstant = 9; double speedScale = Math.min(speedScaleConstant / (contourA.getWidth() + contourB.getWidth()/2), 1.0); double scaledSpeed = Math.min(speed * speedScale, 1.0); @@ -226,7 +231,7 @@ public ArrayList DriveAlongArc(AssistMode mode, double speed) { double distanceToRocket = 72.0; // In inches - double gyroAngle = CommandBase.drivetrain.GetAngle(); + double gyroAngle = CommandBase.drivetrain.getAngle(); int gyroAngleInt = (int)Math.round(gyroAngle); double width = distanceToRocket * Math.cos(gyroAngle); diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index 80defba..49c7d70 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,7 +1,7 @@ { "fileName": "navx_frc.json", "name": "KauaiLabs_navX_FRC", - "version": "3.1.344", + "version": "3.1.366", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://repo1.maven.org/maven2/" @@ -11,7 +11,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-java", - "version": "3.1.344" + "version": "3.1.366" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-cpp", - "version": "3.1.344", + "version": "3.1.366", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, From d04c5f4d539a249adde2d20ef85ea55c5d2b7194 Mon Sep 17 00:00:00 2001 From: Eric Bernard Date: Fri, 8 Feb 2019 12:17:37 -0500 Subject: [PATCH 07/15] Gaston's equation added! --- .../java/frc/robot/commands/TeleopEric.java | 7 +- .../robot/commands/TeleopTriggerControl.java | 6 +- .../java/frc/robot/subsystems/Elevator.java | 82 ++++++++++++++++--- .../frc/robot/subsystems/VisionRocket.java | 2 +- 4 files changed, 79 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 2d0eb46..9e9190a 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -49,8 +49,11 @@ protected void execute() { double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); - //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); - CommandBase.elevator.set(leftSpeed/2); + if (CommandBase.controls.Driver.getAButton()) { + CommandBase.elevator.goTo(1500, 1); + } else { + CommandBase.elevator.set(-leftSpeed); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/TeleopTriggerControl.java b/src/main/java/frc/robot/commands/TeleopTriggerControl.java index 39774ad..64b39ac 100644 --- a/src/main/java/frc/robot/commands/TeleopTriggerControl.java +++ b/src/main/java/frc/robot/commands/TeleopTriggerControl.java @@ -39,18 +39,18 @@ protected void execute() { if(!forward_stop){ if (forward_acceleration){ - CommandBase.drivetrain.Drive(multiplier, -multiplier); + CommandBase.drivetrain.drive(multiplier, -multiplier); } } if(!backward_stop){ if (backward_acceleration){ - CommandBase.drivetrain.Drive(-multiplier, multiplier); + CommandBase.drivetrain.drive(-multiplier, multiplier); } } if (forward_stop || backward_stop){ - CommandBase.drivetrain.Drive(0, 0); + CommandBase.drivetrain.drive(0, 0); } //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 4d0c1fb..250ef50 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; import org.team3236.Conversion; @@ -12,7 +13,10 @@ public class Elevator extends Subsystem { private WPI_TalonSRX talon, talonWithEncoder; public Conversion conversion; - private static final double MAXHEIGHT = 4000; // In ticks! + private static final double HOLDINGCONSTANT = 0.25; // The speed at which the motors stall + // Increase/decrease the holding constant to + private static final double CAPTURERANGE = 0.7; + private static final int MAXHEIGHT = 4000; // In ticks public Elevator() { talon = new WPI_TalonSRX(RobotMap.ELEVATORTALON); @@ -20,7 +24,7 @@ public Elevator() { talonWithEncoder.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - conversion = new Conversion(40, Conversion.Units.IN); // Change the 40 later // + conversion = new Conversion(82.75, Conversion.Units.IN); // Change the 40 later // } public void zeroEncoder() { @@ -36,13 +40,22 @@ public double getEncoder() { } public void ascend(double speed) { - talon.set(speed); - talonWithEncoder.set(-speed); + double encoder = getRawEncoder(); + if (encoder / MAXHEIGHT > CAPTURERANGE && speed > 0.05) { + double captureHeight = MAXHEIGHT * CAPTURERANGE; + double motorSpeed = Math.min(gaston(MAXHEIGHT, speed, captureHeight), speed); + + talon.set(-motorSpeed); + talonWithEncoder.set(motorSpeed); + } else { + talon.set(-speed); + talonWithEncoder.set(speed); + } } - + public void descend(double speed) { // If the encoder is reading less than an inch from the bottom - if (getEncoder() < 1) { + if (getEncoder() < 15) { talon.set(0); talonWithEncoder.set(0); } else { @@ -51,17 +64,62 @@ public void descend(double speed) { } } - public void goTo(double rawPosition, double rawSpeed) { + // Gastons equation // + public double gaston(double destination, double rawSpeed) { + int encoder = getRawEncoder(); + + double holdVal = HOLDINGCONSTANT; + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 2; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + // Gaston's equation with custom offset + public double gaston(double destination, double rawSpeed, double offset) { + + double encoder = getRawEncoder() - offset; + destination = destination - offset; + + double holdVal = HOLDINGCONSTANT; + // Add more going down to increase accuracy + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 2; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + public void goTo(double destination, double rawSpeed) { // Set the position to the top if its higher than the maxheight - double destination = Math.min(rawPosition, MAXHEIGHT); - double current = getRawEncoder(); + destination = Math.min(destination, MAXHEIGHT); + double speed = gaston(destination, rawSpeed); + + SmartDashboard.putNumber("Elevator Speed", speed); + + talon.set(-speed); + talonWithEncoder.set(speed); } public void set(double speed) { - if (speed > 0) { - + if (speed >= 0) { + ascend(speed); } else { - + descend(speed); } } diff --git a/src/main/java/frc/robot/subsystems/VisionRocket.java b/src/main/java/frc/robot/subsystems/VisionRocket.java index 8691acb..a2dfb15 100644 --- a/src/main/java/frc/robot/subsystems/VisionRocket.java +++ b/src/main/java/frc/robot/subsystems/VisionRocket.java @@ -226,7 +226,7 @@ public ArrayList DriveAlongArc(AssistMode mode, double speed) { double distanceToRocket = 72.0; // In inches - double gyroAngle = CommandBase.drivetrain.GetAngle(); + double gyroAngle = CommandBase.drivetrain.getAngle(); int gyroAngleInt = (int)Math.round(gyroAngle); double width = distanceToRocket * Math.cos(gyroAngle); From d0ab67662319c75caceebf1a023b9fe13447a742 Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Fri, 8 Feb 2019 18:30:30 +0000 Subject: [PATCH 08/15] Add bugs --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/commands/TeleopEric.java | 32 +++++++- .../java/frc/robot/subsystems/Elevator.java | 82 ++++++++++++++++--- 3 files changed, 99 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b3bcbe5..9706dbc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,7 +114,7 @@ public void teleopInit() { else{ new TeleopVision().start(); } */ - new TeleopVision().start(); + new TeleopEric().start(); } /** diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 2d0eb46..4af68aa 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -46,11 +46,35 @@ protected void execute() { SmartDashboard.putNumber("elevator", CommandBase.elevator.getRawEncoder()); - double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); + //double leftSpeed = CommandBase.controls.Driver.getY(Hand.kLeft); + double leftSpeed = 0; double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); - //CommandBase.drivetrain.Drive(leftSpeed, rightSpeed); - CommandBase.elevator.set(leftSpeed/2); + // Lookie here + System.out.print(CommandBase.controls.Driver.getPOV()); + + + if (CommandBase.controls.Driver.getAButton()) { + System.out.println("???"); + CommandBase.elevator.goTo(1500, 1); + } + + /* and elif is neccecary here, because we believe that + flawed logic exists in the XboxController class which + fails to make GetAButton() false when B button is + pressed */ + + else if (CommandBase.controls.Driver.getBButton()) { + CommandBase.elevator.goTo(3000, 1); + } + + else { + // For safety reasons, we + // set the elevator motor to -0. + CommandBase.elevator.set(-leftSpeed); + } + + } // Make this return true when this Command no longer needs to run execute() @@ -69,4 +93,4 @@ protected void end() { @Override protected void interrupted() { } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 4d0c1fb..250ef50 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; import org.team3236.Conversion; @@ -12,7 +13,10 @@ public class Elevator extends Subsystem { private WPI_TalonSRX talon, talonWithEncoder; public Conversion conversion; - private static final double MAXHEIGHT = 4000; // In ticks! + private static final double HOLDINGCONSTANT = 0.25; // The speed at which the motors stall + // Increase/decrease the holding constant to + private static final double CAPTURERANGE = 0.7; + private static final int MAXHEIGHT = 4000; // In ticks public Elevator() { talon = new WPI_TalonSRX(RobotMap.ELEVATORTALON); @@ -20,7 +24,7 @@ public Elevator() { talonWithEncoder.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - conversion = new Conversion(40, Conversion.Units.IN); // Change the 40 later // + conversion = new Conversion(82.75, Conversion.Units.IN); // Change the 40 later // } public void zeroEncoder() { @@ -36,13 +40,22 @@ public double getEncoder() { } public void ascend(double speed) { - talon.set(speed); - talonWithEncoder.set(-speed); + double encoder = getRawEncoder(); + if (encoder / MAXHEIGHT > CAPTURERANGE && speed > 0.05) { + double captureHeight = MAXHEIGHT * CAPTURERANGE; + double motorSpeed = Math.min(gaston(MAXHEIGHT, speed, captureHeight), speed); + + talon.set(-motorSpeed); + talonWithEncoder.set(motorSpeed); + } else { + talon.set(-speed); + talonWithEncoder.set(speed); + } } - + public void descend(double speed) { // If the encoder is reading less than an inch from the bottom - if (getEncoder() < 1) { + if (getEncoder() < 15) { talon.set(0); talonWithEncoder.set(0); } else { @@ -51,17 +64,62 @@ public void descend(double speed) { } } - public void goTo(double rawPosition, double rawSpeed) { + // Gastons equation // + public double gaston(double destination, double rawSpeed) { + int encoder = getRawEncoder(); + + double holdVal = HOLDINGCONSTANT; + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 2; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + // Gaston's equation with custom offset + public double gaston(double destination, double rawSpeed, double offset) { + + double encoder = getRawEncoder() - offset; + destination = destination - offset; + + double holdVal = HOLDINGCONSTANT; + // Add more going down to increase accuracy + if (encoder > destination) { + holdVal -= .1; + rawSpeed /= 2; + } + + double speed = (((destination - encoder)/destination)*Math.abs(rawSpeed)) + holdVal; + + if (speed < 0) { + return Math.max(-1, speed); + } else { + return Math.min(1, speed); + } + } + + public void goTo(double destination, double rawSpeed) { // Set the position to the top if its higher than the maxheight - double destination = Math.min(rawPosition, MAXHEIGHT); - double current = getRawEncoder(); + destination = Math.min(destination, MAXHEIGHT); + double speed = gaston(destination, rawSpeed); + + SmartDashboard.putNumber("Elevator Speed", speed); + + talon.set(-speed); + talonWithEncoder.set(speed); } public void set(double speed) { - if (speed > 0) { - + if (speed >= 0) { + ascend(speed); } else { - + descend(speed); } } From 90073aa23651497b8e1fa6af95282b8c801a3d14 Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Fri, 8 Feb 2019 18:58:28 +0000 Subject: [PATCH 09/15] Add templates for better Operator control of Elevator --- src/main/java/frc/robot/commands/TeleopEric.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 4af68aa..3321611 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -51,8 +51,18 @@ protected void execute() { double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); // Lookie here - System.out.print(CommandBase.controls.Driver.getPOV()); - + if(CommandBase.controls.Driver.getYButton()){ + // Increment Elevator level here + System.out.println("Going up.."); + } + else if(CommandBase.controls.Driver.getBButton()){ + // Decrement Elevator level here + System.out.println("Going down..."); + } + else if(CommandBase.controls.Driver.getAButton()){ + // Actually move the elevator here + System.out.println("Doing stuff!"); + } if (CommandBase.controls.Driver.getAButton()) { System.out.println("???"); From d90816d0ce74ac4419212ccd63e81dd9b6603400 Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Sat, 9 Feb 2019 15:47:01 +0000 Subject: [PATCH 10/15] Add Elevator level selection --- src/main/java/frc/robot/RobotMap.java | 6 ++ .../java/frc/robot/commands/TeleopEric.java | 87 ++++++++++++++++--- .../java/frc/robot/subsystems/Elevator.java | 8 +- 3 files changed, 85 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index e5aec1e..2b38b95 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -28,6 +28,12 @@ public class RobotMap { public static int ELEVATORTALON = 4; public static int ELEVATORTALONENC = 5; + public static int ELEVATORLEVELONE = 500; + public static int ELEVATORLEVELTWO = 1200; + public static int ELEVATORLEVELTHREE = 3000; + + + // If you are using multiple modules, make sure to define both the port diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 3321611..04e9d5d 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -8,13 +8,16 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.team3236.DriveTrainMode; import frc.robot.CommandBase; +import frc.robot.RobotMap; public class TeleopEric extends Command { + int desiredElevatorLevel = 0; boolean canSwitchDriveMode = true; public TeleopEric() { @@ -23,7 +26,7 @@ public TeleopEric() { // Called just before this Command runs the first time @Override protected void initialize() { - + CommandBase.elevator.zeroEncoder(); } // Called repeatedly when this Command is scheduled to run @@ -50,31 +53,87 @@ protected void execute() { double leftSpeed = 0; double rightSpeed = CommandBase.controls.Driver.getY(Hand.kRight); - // Lookie here - if(CommandBase.controls.Driver.getYButton()){ + CommandBase.elevator.set(rightSpeed); + + + switch(desiredElevatorLevel){ + default: + case 0: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + + break; + case 1: + + SmartDashboard.putBoolean("Level 1", true); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + break; + + case 2: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", true); + SmartDashboard.putBoolean("Level 3", false); + break; + + case 3: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", true); + break; + } + + SmartDashboard.putNumber("Elevator Level", desiredElevatorLevel); + + if(CommandBase.controls.Driver.getYButtonPressed()){ // Increment Elevator level here - System.out.println("Going up.."); + if(desiredElevatorLevel < 3){ + desiredElevatorLevel++; + } } - else if(CommandBase.controls.Driver.getBButton()){ + else if(CommandBase.controls.Driver.getBButtonPressed()){ // Decrement Elevator level here - System.out.println("Going down..."); + if (desiredElevatorLevel >= 1){ + desiredElevatorLevel--; + } } + + else if(CommandBase.controls.Driver.getAButton()){ - // Actually move the elevator here - System.out.println("Doing stuff!"); + // Actually move the elevator + System.out.println(desiredElevatorLevel); + + switch(desiredElevatorLevel){ + default: + break; + + case 1: + CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELONE, 0.8); + break; + + case 2: + CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELTWO, 0.8); + break; + + case 3: + CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELTHREE, 0.8); + break; + } } - if (CommandBase.controls.Driver.getAButton()) { - System.out.println("???"); - CommandBase.elevator.goTo(1500, 1); - } + + //if (CommandBase.controls.Driver.getAButton()) { + // System.out.println("???"); + // CommandBase.elevator.goTo(1500, 1); + //} /* and elif is neccecary here, because we believe that flawed logic exists in the XboxController class which fails to make GetAButton() false when B button is pressed */ - else if (CommandBase.controls.Driver.getBButton()) { + /*(else if (CommandBase.controls.Driver.getBButton()) { CommandBase.elevator.goTo(3000, 1); } @@ -82,7 +141,7 @@ else if (CommandBase.controls.Driver.getBButton()) { // For safety reasons, we // set the elevator motor to -0. CommandBase.elevator.set(-leftSpeed); - } + } */ } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 250ef50..b32fe65 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -23,7 +23,8 @@ public Elevator() { talonWithEncoder = new WPI_TalonSRX(RobotMap.ELEVATORTALONENC); talonWithEncoder.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - + + zeroEncoder(); conversion = new Conversion(82.75, Conversion.Units.IN); // Change the 40 later // } @@ -67,7 +68,7 @@ public void descend(double speed) { // Gastons equation // public double gaston(double destination, double rawSpeed) { int encoder = getRawEncoder(); - + destination = Math.min(destination, MAXHEIGHT); double holdVal = HOLDINGCONSTANT; if (encoder > destination) { holdVal -= .1; @@ -87,6 +88,7 @@ public double gaston(double destination, double rawSpeed, double offset) { double encoder = getRawEncoder() - offset; destination = destination - offset; + destination = Math.min(destination, MAXHEIGHT); double holdVal = HOLDINGCONSTANT; // Add more going down to increase accuracy @@ -107,6 +109,8 @@ public double gaston(double destination, double rawSpeed, double offset) { public void goTo(double destination, double rawSpeed) { // Set the position to the top if its higher than the maxheight destination = Math.min(destination, MAXHEIGHT); + + System.out.println(destination); double speed = gaston(destination, rawSpeed); SmartDashboard.putNumber("Elevator Speed", speed); From 079d14befc37613a242bdb2935ce41265c33611d Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Sat, 9 Feb 2019 18:49:50 +0000 Subject: [PATCH 11/15] ... --- src/main/java/frc/robot/CommandBase.java | 1 + src/main/java/frc/robot/RobotMap.java | 15 +- .../java/frc/robot/commands/TeleopEric.java | 194 +++++++++++++----- src/main/java/frc/robot/subsystems/Arm.java | 35 ++++ 4 files changed, 184 insertions(+), 61 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Arm.java diff --git a/src/main/java/frc/robot/CommandBase.java b/src/main/java/frc/robot/CommandBase.java index 0d2c8fe..92d3630 100644 --- a/src/main/java/frc/robot/CommandBase.java +++ b/src/main/java/frc/robot/CommandBase.java @@ -14,5 +14,6 @@ public class CommandBase { public static OI controls = new OI(); public static DriveTrain drivetrain = new DriveTrain(); public static Elevator elevator = new Elevator(); + public static Arm arm = new Arm(); } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 2b38b95..60e1b42 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -26,12 +26,19 @@ public class RobotMap { public static int RIGHTTALONA = 2; public static int RIGHTTALONB = 3; + public static int ELEVATORTALON = 4; public static int ELEVATORTALONENC = 5; - public static int ELEVATORLEVELONE = 500; - public static int ELEVATORLEVELTWO = 1200; - public static int ELEVATORLEVELTHREE = 3000; - + + public static int HATCHLEVELONE = 500; + public static int HATCHLEVELTWO = 1200; + public static int HATCHLEVELTHREE = 3000; + + public static int CARGOLEVELONE = 600; + public static int CARGOLEVELTWO = 1000; + public static int CARGOLEVELTHREE = 2200; + public static int CARGOLEVELSHIP = 3500; + public static final int ACTUATOR = 7; diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 04e9d5d..9f60aec 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -14,10 +14,14 @@ import org.team3236.DriveTrainMode; import frc.robot.CommandBase; import frc.robot.RobotMap; +import frc.robot.subsystems.*; + public class TeleopEric extends Command { - int desiredElevatorLevel = 0; + int desiredElevatorLevelCargo = 0; + int desiredElevatorLevelHatch = 0; + boolean canSwitchDriveMode = true; public TeleopEric() { @@ -35,7 +39,10 @@ protected void execute() { // Check to see if we should switch drive modes // boolean leftStickDown = CommandBase.controls.Driver.getStickButton(Hand.kLeft); boolean rightStickDown = CommandBase.controls.Driver.getStickButton(Hand.kRight); - int dpad = CommandBase.controls.Driver.getPOV(); + if (CommandBase.controls.Driver.getBumperPressed(Hand.kLeft)) + { + CommandBase.drivetrain.switchDriveMode(); + } if (leftStickDown && rightStickDown) { if (canSwitchDriveMode) { @@ -56,96 +63,169 @@ protected void execute() { CommandBase.elevator.set(rightSpeed); - switch(desiredElevatorLevel){ - default: - case 0: - SmartDashboard.putBoolean("Level 1", false); - SmartDashboard.putBoolean("Level 2", false); - SmartDashboard.putBoolean("Level 3", false); + if(CommandBase.drivetrain.getDriveMode() == DriveTrainMode.CARGO){ + switch(desiredElevatorLevelCargo){ + default: + case 0: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); - break; - case 1: - SmartDashboard.putBoolean("Level 1", true); - SmartDashboard.putBoolean("Level 2", false); - SmartDashboard.putBoolean("Level 3", false); - break; + break; + case 1: - case 2: - SmartDashboard.putBoolean("Level 1", false); - SmartDashboard.putBoolean("Level 2", true); - SmartDashboard.putBoolean("Level 3", false); - break; + SmartDashboard.putBoolean("Level 1", true); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); + + break; - case 3: - SmartDashboard.putBoolean("Level 1", false); - SmartDashboard.putBoolean("Level 2", false); - SmartDashboard.putBoolean("Level 3", true); - break; + case 2: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", true); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", false); + + break; + + case 3: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", true); + SmartDashboard.putBoolean("Level 4", false); + break; + case 4: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + SmartDashboard.putBoolean("Level 4", true); + break; + } + if(CommandBase.controls.Driver.getYButtonPressed()){ + // Increment Elevator level here + if(desiredElevatorLevelCargo < 4){ + desiredElevatorLevelCargo++; + } + } + else if(CommandBase.controls.Driver.getBButtonPressed()){ + // Decrement Elevator level here + if (desiredElevatorLevelCargo >= 1){ + desiredElevatorLevelCargo--; + } + } + + + + + else if(CommandBase.controls.Driver.getAButton()){ + // Actually move the elevator + System.out.println(desiredElevatorLevelCargo); + + switch(desiredElevatorLevelCargo){ + default: + break; + + case 1: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELONE, 0.8); + break; + + case 2: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELTWO, 0.8); + break; + + case 3: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELTHREE, 0.8); + break; + case 4: + CommandBase.elevator.goTo(RobotMap.CARGOLEVELSHIP, 0.8); + break; + } + } } + else if(CommandBase.drivetrain.getDriveMode() == DriveTrainMode.HATCH){ + switch(desiredElevatorLevelHatch){ + default: + case 0: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); - SmartDashboard.putNumber("Elevator Level", desiredElevatorLevel); + + break; + case 1: + + SmartDashboard.putBoolean("Level 1", true); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + + break; + + case 2: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", true); + SmartDashboard.putBoolean("Level 3", false); + + break; + + case 3: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", true); + break; + case 4: + SmartDashboard.putBoolean("Level 1", false); + SmartDashboard.putBoolean("Level 2", false); + SmartDashboard.putBoolean("Level 3", false); + break; + } + if(CommandBase.controls.Driver.getYButtonPressed()){ // Increment Elevator level here - if(desiredElevatorLevel < 3){ - desiredElevatorLevel++; - } + if(desiredElevatorLevelHatch < 3){ + desiredElevatorLevelHatch++; + } } else if(CommandBase.controls.Driver.getBButtonPressed()){ // Decrement Elevator level here - if (desiredElevatorLevel >= 1){ - desiredElevatorLevel--; + if (desiredElevatorLevelHatch >= 1){ + desiredElevatorLevelHatch--; } } else if(CommandBase.controls.Driver.getAButton()){ // Actually move the elevator - System.out.println(desiredElevatorLevel); + System.out.println(desiredElevatorLevelHatch); - switch(desiredElevatorLevel){ + switch(desiredElevatorLevelHatch){ default: break; case 1: - CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELONE, 0.8); + CommandBase.elevator.goTo(RobotMap.HATCHLEVELONE, 0.8); break; case 2: - CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELTWO, 0.8); + CommandBase.elevator.goTo(RobotMap.HATCHLEVELTWO, 0.8); break; case 3: - CommandBase.elevator.goTo(RobotMap.ELEVATORLEVELTHREE, 0.8); + CommandBase.elevator.goTo(RobotMap.HATCHLEVELTHREE, 0.8); break; } } - - - //if (CommandBase.controls.Driver.getAButton()) { - // System.out.println("???"); - // CommandBase.elevator.goTo(1500, 1); - //} - - /* and elif is neccecary here, because we believe that - flawed logic exists in the XboxController class which - fails to make GetAButton() false when B button is - pressed */ - - /*(else if (CommandBase.controls.Driver.getBButton()) { - CommandBase.elevator.goTo(3000, 1); - } - - else { - // For safety reasons, we - // set the elevator motor to -0. - CommandBase.elevator.set(-leftSpeed); - } */ + } + double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kRight) - CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft); + + CommandBase.arm.setArm(actuatorSpeed); - } + // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..7d38b60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.subsystems; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.kauailabs.navx.frc.*; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.SerialPort.*; +import org.team3236.DriveTrainMode; +import org.team3236.Conversion; +import edu.wpi.first.wpilibj.AnalogPotentiometer; + +/** + * Add your docs here. + */ + +public class Arm extends Subsystem { + private WPI_TalonSRX Actuator = new WPI_TalonSRX(RobotMap.ACTUATOR); + private AnalogPotentiometer potentiometer = new AnalogPotentiometer(0); + public void setArm(double speed){ + Actuator.set(speed); + } + + + @Override + public void initDefaultCommand() {} +} From 864be5d40e1e7c08733cb51c9d541fdce32a3886 Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Wed, 13 Feb 2019 23:22:28 +0000 Subject: [PATCH 12/15] Thursday stuff --- src/main/java/frc/robot/RobotMap.java | 8 +-- .../java/frc/robot/commands/TeleopEric.java | 9 ++- src/main/java/frc/robot/subsystems/Arm.java | 55 ++++++++++++++++++- .../java/frc/robot/subsystems/DriveTrain.java | 27 +++++---- 4 files changed, 77 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 5acfbe0..402bd64 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -21,10 +21,10 @@ public class RobotMap { public static int LEFTXBOX = 0; public static int RIGHTXBOX = 1; - public static int LEFTTALONA = 0; - public static int LEFTTALONB = 1; - public static int RIGHTTALONA = 2; - public static int RIGHTTALONB = 3; + public static int LEFTVICTORA = 0; + public static int LEFTVICTORB = 1; + public static int RIGHTVICTORA = 2; + public static int RIGHTVICTORB = 3; public static int ELEVATORTALON = 4; diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 5a911d3..28ee7e1 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -209,12 +209,11 @@ else if (CommandBase.controls.Driver.getAButton()) { } } - double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kRight) - CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft); - SmartDashboard.putNumber("ACTUATOR", CommandBase.arm.get()); + double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft) - CommandBase.controls.Driver.getTriggerAxis(Hand.kRight); + SmartDashboard.putBoolean("ACTUATOR", CommandBase.arm.getSensor(1)); SmartDashboard.putNumber("ACTUATOR RATE", CommandBase.arm.getRate()); - if (CommandBase.arm.get() < 9) { - CommandBase.arm.setArm(actuatorSpeed); - } + //CommandBase.arm.setArm(actuatorSpeed); + CommandBase.arm.gotoSensor(1); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index fb9ace5..38dc34c 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -18,6 +18,7 @@ import org.team3236.Conversion; import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.AnalogPotentiometer; /** @@ -28,7 +29,11 @@ public class Arm extends Subsystem { private WPI_TalonSRX actuator; private AnalogPotentiometer potentiometer; private AnalogInput potPort = new AnalogInput(0); - + private DigitalInput hallSensor0 = new DigitalInput(7); + private DigitalInput hallSensor1 = new DigitalInput(8); + private DigitalInput hallSensor2 = new DigitalInput(9); + static int currentPosition = 0; + public Arm() { //potPort.setGlobalSampleRate(samplesPerSecond); @@ -41,12 +46,60 @@ public Arm() { public double get() { return Math.ceil(potPort.getAverageVoltage() * 1000)/10; } + public boolean getSensor(int sensor){ + switch(sensor){ + default: + // + case 0: + return !hallSensor0.get(); + case 1: + return !hallSensor1.get(); + case 2: + return !hallSensor2.get(); + } + } public double getRate() { return potPort.getAverageBits(); } + public void gotoSensor(int sensor){ + boolean sensorStatus = getSensor(sensor); + if(sensorStatus == false){ + /* if we are behind or on front of the sensor */ + + switch(currentPosition){ + default: + // + case(0): + // Extend sensor to destination + setArm(0.5); + case(1): + if(sensor == 2){ + /* Extend arm */ + setArm(0.5); + } + else{ + setArm(-0.5); + } + case(2): + // Retract sensor to destination + setArm(-0.5); + } + /*We are behind destination sensor*/ + + setArm(0.5); // extend arm + + } + else if(sensorStatus == true){ + currentPosition = sensor; + return; + } + } + public void setArm(double speed){ + /* Negative values retract the actuator; + Positive values extend the actuator; */ actuator.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 2dfd069..76884dd 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -10,7 +10,10 @@ import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.RobotMap; + +import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.kauailabs.navx.frc.*; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; @@ -20,10 +23,10 @@ import org.team3236.Conversion; public class DriveTrain extends Subsystem { - private WPI_TalonSRX LeftTalonA = new WPI_TalonSRX(RobotMap.LEFTTALONA); - private WPI_TalonSRX LeftTalonB = new WPI_TalonSRX(RobotMap.LEFTTALONB); - private WPI_TalonSRX RightTalonA = new WPI_TalonSRX(RobotMap.RIGHTTALONA); - private WPI_TalonSRX RightTalonB = new WPI_TalonSRX(RobotMap.RIGHTTALONB); + private WPI_VictorSPX LeftVictorA = new WPI_VictorSPX(RobotMap.LEFTVICTORA); + private WPI_VictorSPX LeftVictorB = new WPI_VictorSPX(RobotMap.LEFTVICTORB); + private WPI_VictorSPX RightVictorA = new WPI_VictorSPX(RobotMap.RIGHTVICTORA); + private WPI_VictorSPX RightVictorB = new WPI_VictorSPX(RobotMap.RIGHTVICTORB); private static AHRS NavX = new AHRS(SPI.Port.kMXP); @@ -79,21 +82,21 @@ public DriveTrainMode getDriveMode() { public void setLeft(double speed) { if (driveMode == DriveTrainMode.HATCH) { - LeftTalonA.set(speed); - LeftTalonB.set(speed); + LeftVictorA.set(speed); + LeftVictorB.set(speed); } else { - LeftTalonA.set(-speed); - LeftTalonB.set(-speed); + LeftVictorA.set(-speed); + LeftVictorB.set(-speed); } } public void setRight(double speed) { if (driveMode == DriveTrainMode.HATCH) { - RightTalonA.set(-speed); - RightTalonB.set(-speed); + RightVictorA.set(-speed); + RightVictorB.set(-speed); } else { - RightTalonA.set(speed); - RightTalonB.set(speed); + RightVictorA.set(speed); + RightVictorB.set(speed); } } From adffbd4d7eb2dbdf8d99a334fb8b49f188a8e0c5 Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Thu, 14 Feb 2019 18:18:13 +0000 Subject: [PATCH 13/15] Rewrite the Accuator subsystem --- .../java/frc/robot/commands/TeleopEric.java | 21 ++- src/main/java/frc/robot/subsystems/Arm.java | 169 ++++++++++++++---- 2 files changed, 154 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 28ee7e1..e9c2a11 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -23,6 +23,8 @@ public class TeleopEric extends Command { int desiredElevatorLevelHatch = 0; boolean canSwitchDriveMode = true; + boolean armActive = true; + public TeleopEric() { } @@ -212,12 +214,21 @@ else if (CommandBase.controls.Driver.getAButton()) { double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft) - CommandBase.controls.Driver.getTriggerAxis(Hand.kRight); SmartDashboard.putBoolean("ACTUATOR", CommandBase.arm.getSensor(1)); SmartDashboard.putNumber("ACTUATOR RATE", CommandBase.arm.getRate()); - //CommandBase.arm.setArm(actuatorSpeed); - CommandBase.arm.gotoSensor(1); - + ///CommandBase.arm.setArm(actuatorSpeed); + + // Icky + // /* + if(armActive){ + int currentArmStatus = CommandBase.arm.gotoSensor(1); + if(currentArmStatus == 0){ + armActive = false; + } + else{ + System.out.println("Errors occured..."); + } + } + } - - // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 38dc34c..21db60a 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -20,7 +20,8 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.AnalogPotentiometer; - +import java.util.Arrays; +import java.util.ArrayList; /** * Add your docs here. */ @@ -32,7 +33,12 @@ public class Arm extends Subsystem { private DigitalInput hallSensor0 = new DigitalInput(7); private DigitalInput hallSensor1 = new DigitalInput(8); private DigitalInput hallSensor2 = new DigitalInput(9); - static int currentPosition = 0; + static int previousPosition = -1; + + boolean sensor0Status; + boolean sensor1Status; + boolean sensor2Status; + boolean ActuatorInTransit; public Arm() { //potPort.setGlobalSampleRate(samplesPerSecond); @@ -62,45 +68,146 @@ public boolean getSensor(int sensor){ public double getRate() { return potPort.getAverageBits(); } + public int gotoSensor(int destinationSensor){ + // Update Sensors + sensor0Status = getSensor(0); + sensor1Status = getSensor(1); + sensor2Status = getSensor(2); + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); + SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); + SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + SmartDashboard.putNumber("Last known sensor value: ", previousPosition); + boolean thisSensorStatus = getSensor(destinationSensor); // True when we are at given sensor + System.out.print("Desintation: "+destinationSensor); + System.out.print("Status" + sensor0Status + sensor1Status + sensor2Status); + if (thisSensorStatus) + { + return 0; + } - public void gotoSensor(int sensor){ - boolean sensorStatus = getSensor(sensor); - if(sensorStatus == false){ - /* if we are behind or on front of the sensor */ - - switch(currentPosition){ - default: - // - case(0): - // Extend sensor to destination - setArm(0.5); - case(1): - if(sensor == 2){ - /* Extend arm */ - setArm(0.5); - } - else{ - setArm(-0.5); - } - case(2): - // Retract sensor to destination - setArm(-0.5); + else + // Find direction we need to move; Initate movement. + + { + // If this is first actuator movement and we are not already fully extended + if(previousPosition == -1 + && !((!sensor0Status) + && (!sensor1Status) + && (!sensor2Status))){ + /* First movement; Actuator is in default position; move forwards to destination */ + setArm(0.5, destinationSensor); + return 0; + } + else if(previousPosition == 0){ + /* Actuator has moved before, but re-initialized at some point; + move forwards to destination */ + + if(destinationSensor != 0){ + setArm(0.5, destinationSensor); + return 0; + } + + else{ + return 0; // Dont move from sensor 0 to sensor 0; that doesn't make sense. + } + } + else if(previousPosition == 1){ + /* Actuator has moved before; We are in middle position; + Move in a direction derived from value of desinationSensor towards the desination. */ + + if(destinationSensor < previousPosition){ + /* Actuator is moving from sensor1 to sensor0; move backwards to destination. */ + setArm(-0.5, destinationSensor); + return 0; + } + else if(destinationSensor == previousPosition){ + /* Actuator is moving from sensor 1 to sensor 1; Don't move. */ + return 0; + } + else if(destinationSensor > previousPosition){ + /* Actuator is moving from sensor 1 to sensor 2; move forwards to destination */ + setArm(0.5, destinationSensor); + } + } + else if(previousPosition == 2){ + /* Actuator has moved before; Actuator is in extended position; + Move backwards to destination; */ + + if(destinationSensor != 2){ + setArm(-0.5, destinationSensor); + return 0; + } + else{ + // Don't move from sensor 2 to sensor 2; That doesn't make sense + return 0; + } + } + else if ((!sensor0Status) && (!sensor1Status) && (!sensor2Status)){ + /* (0,0,0) must be fully extended past last encoder */ + // Move backwards to destination + System.out.print("Gaston is confused... :/"); + setArm(-0.5, destinationSensor); + return 0; } - /*We are behind destination sensor*/ - - setArm(0.5); // extend arm } - else if(sensorStatus == true){ - currentPosition = sensor; - return; - } + /* This code will not be reachable under normal conditions; + It serves as a mechanism to detect movement error. */ + return -1; + + } public void setArm(double speed){ /* Negative values retract the actuator; Positive values extend the actuator; */ + + // Update sensors + sensor0Status = getSensor(0); + sensor1Status = getSensor(1); + sensor2Status = getSensor(2); + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); + SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); + SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + + actuator.set(-speed); + } + + public void setArm(double speed, int destinationSensor){ + /* Negative values retract the actuator; + Positive values extend the actuator; */ + + // Update sensors + sensor0Status = getSensor(0); + sensor1Status = getSensor(1); + sensor2Status = getSensor(2); + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); + SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); + SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + + // Move arm in speed calculated by gotoSensor() + System.out.println("Moving Arm to desination..."); + System.out.println("(Destionation = Sensor "+destinationSensor+")"); + System.out.println("Speed: "+ speed); actuator.set(speed); + System.out.println("Completed Arm Motion;"); + + // Update sensors again + sensor0Status = getSensor(0); + sensor1Status = getSensor(1); + sensor2Status = getSensor(2); + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); + SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); + SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + + // If we are at destinaiton, return; + if(getSensor(destinationSensor) == true){ + System.out.print("Arrived at destination!"); + return; + } + else{ + System.out.println("Gaston encountered an error... Gaston is sad :("); + } } From 3e127162083d74f00c21427479bdd344bac4f04f Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Thu, 14 Feb 2019 22:12:55 +0000 Subject: [PATCH 14/15] Fix Manual Actuator Failsafe --- .../java/frc/robot/commands/TeleopEric.java | 25 ++- src/main/java/frc/robot/subsystems/Arm.java | 192 +++++------------- 2 files changed, 58 insertions(+), 159 deletions(-) diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index e9c2a11..6e31558 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -212,23 +212,22 @@ else if (CommandBase.controls.Driver.getAButton()) { } double actuatorSpeed = CommandBase.controls.Driver.getTriggerAxis(Hand.kLeft) - CommandBase.controls.Driver.getTriggerAxis(Hand.kRight); + if (actuatorSpeed > 0.7){ + actuatorSpeed = 0.7; + } + if (actuatorSpeed < -0.7){ + actuatorSpeed = -0.7; + } SmartDashboard.putBoolean("ACTUATOR", CommandBase.arm.getSensor(1)); SmartDashboard.putNumber("ACTUATOR RATE", CommandBase.arm.getRate()); - ///CommandBase.arm.setArm(actuatorSpeed); - - // Icky - // /* - if(armActive){ - int currentArmStatus = CommandBase.arm.gotoSensor(1); - if(currentArmStatus == 0){ - armActive = false; - } - else{ - System.out.println("Errors occured..."); - } + CommandBase.arm.setArm(actuatorSpeed); + + if (CommandBase.controls.Driver.getBumperPressed(Hand.kRight)){ + CommandBase.elevator.set(1); } - } + + // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 21db60a..3f6ef79 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -35,10 +35,8 @@ public class Arm extends Subsystem { private DigitalInput hallSensor2 = new DigitalInput(9); static int previousPosition = -1; - boolean sensor0Status; - boolean sensor1Status; - boolean sensor2Status; - boolean ActuatorInTransit; + + static private boolean ActuatorInTransit; public Arm() { //potPort.setGlobalSampleRate(samplesPerSecond); @@ -53,161 +51,63 @@ public double get() { return Math.ceil(potPort.getAverageVoltage() * 1000)/10; } public boolean getSensor(int sensor){ - switch(sensor){ - default: - // - case 0: - return !hallSensor0.get(); - case 1: - return !hallSensor1.get(); - case 2: - return !hallSensor2.get(); + if(sensor == 0){ + return !hallSensor0.get(); + } + else if(sensor == 1){ + return !hallSensor1.get(); + } + else { + return !hallSensor2.get(); } } public double getRate() { return potPort.getAverageBits(); } - public int gotoSensor(int destinationSensor){ - // Update Sensors - sensor0Status = getSensor(0); - sensor1Status = getSensor(1); - sensor2Status = getSensor(2); - SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); - SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); - SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); - SmartDashboard.putNumber("Last known sensor value: ", previousPosition); - boolean thisSensorStatus = getSensor(destinationSensor); // True when we are at given sensor - System.out.print("Desintation: "+destinationSensor); - System.out.print("Status" + sensor0Status + sensor1Status + sensor2Status); - if (thisSensorStatus) - { - return 0; - } - - else - // Find direction we need to move; Initate movement. - - { - // If this is first actuator movement and we are not already fully extended - if(previousPosition == -1 - && !((!sensor0Status) - && (!sensor1Status) - && (!sensor2Status))){ - /* First movement; Actuator is in default position; move forwards to destination */ - setArm(0.5, destinationSensor); - return 0; - } - else if(previousPosition == 0){ - /* Actuator has moved before, but re-initialized at some point; - move forwards to destination */ - - if(destinationSensor != 0){ - setArm(0.5, destinationSensor); - return 0; - } - - else{ - return 0; // Dont move from sensor 0 to sensor 0; that doesn't make sense. - } - } - else if(previousPosition == 1){ - /* Actuator has moved before; We are in middle position; - Move in a direction derived from value of desinationSensor towards the desination. */ - - if(destinationSensor < previousPosition){ - /* Actuator is moving from sensor1 to sensor0; move backwards to destination. */ - setArm(-0.5, destinationSensor); - return 0; - } - else if(destinationSensor == previousPosition){ - /* Actuator is moving from sensor 1 to sensor 1; Don't move. */ - return 0; - } - else if(destinationSensor > previousPosition){ - /* Actuator is moving from sensor 1 to sensor 2; move forwards to destination */ - setArm(0.5, destinationSensor); - } - } - else if(previousPosition == 2){ - /* Actuator has moved before; Actuator is in extended position; - Move backwards to destination; */ - - if(destinationSensor != 2){ - setArm(-0.5, destinationSensor); - return 0; - } - else{ - // Don't move from sensor 2 to sensor 2; That doesn't make sense - return 0; - } - } - else if ((!sensor0Status) && (!sensor1Status) && (!sensor2Status)){ - /* (0,0,0) must be fully extended past last encoder */ - // Move backwards to destination - System.out.print("Gaston is confused... :/"); - setArm(-0.5, destinationSensor); - return 0; - } - - } - /* This code will not be reachable under normal conditions; - It serves as a mechanism to detect movement error. */ - return -1; - - - } public void setArm(double speed){ - /* Negative values retract the actuator; - Positive values extend the actuator; */ - // Update sensors - sensor0Status = getSensor(0); - sensor1Status = getSensor(1); - sensor2Status = getSensor(2); - SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); - SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); - SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); - - actuator.set(-speed); - } - - public void setArm(double speed, int destinationSensor){ - /* Negative values retract the actuator; - Positive values extend the actuator; */ - - // Update sensors - sensor0Status = getSensor(0); - sensor1Status = getSensor(1); - sensor2Status = getSensor(2); - SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); - SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); - SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); + boolean sensor0Status = getSensor(0); + boolean sensor1Status = getSensor(1); + boolean sensor2Status = getSensor(2); + + if((speed < 0 && sensor2Status) || (speed > 0 && sensor0Status)){ + /* speed = 1; sensor2Status = True; + (speed < 0 && sensor2Status) = 0 & 1 = 1 = False + (speed > 0 && sensor0Status) = 1 & 0 = False + False || False = False + */ + System.out.println("Sensors (0):"); + System.out.println(sensor0Status); + System.out.println(sensor2Status); + + System.out.println(";"); + actuator.set(speed); + } + else if (!sensor0Status && !sensor2Status){ + /* + 1 & 0 = 0 = False + */ + System.out.println("Sensors (1):"); + System.out.println(!sensor0Status); + System.out.println(!sensor2Status); + + System.out.println(";"); + actuator.set(speed); + } + else{ + System.out.println("Sensors (2):"); + System.out.println(sensor0Status); + System.out.println(sensor2Status); - // Move arm in speed calculated by gotoSensor() - System.out.println("Moving Arm to desination..."); - System.out.println("(Destionation = Sensor "+destinationSensor+")"); - System.out.println("Speed: "+ speed); - actuator.set(speed); - System.out.println("Completed Arm Motion;"); - - // Update sensors again - sensor0Status = getSensor(0); - sensor1Status = getSensor(1); - sensor2Status = getSensor(2); + System.out.println(";"); + actuator.set(0); + } + SmartDashboard.putBoolean("Hall Sensor 0:", sensor0Status); SmartDashboard.putBoolean("Hall Sensor 1:", sensor1Status); SmartDashboard.putBoolean("Hall Sensor 2:", sensor2Status); - - // If we are at destinaiton, return; - if(getSensor(destinationSensor) == true){ - System.out.print("Arrived at destination!"); - return; - } - else{ - System.out.println("Gaston encountered an error... Gaston is sad :("); - } } From afdca54e1744c5240ecc30cf379bace729fb1a3d Mon Sep 17 00:00:00 2001 From: Hexic Pyth Date: Thu, 14 Feb 2019 23:24:54 +0000 Subject: [PATCH 15/15] ... --- .../java/frc/robot/commands/TeleopEric.java | 16 ++++ src/main/java/frc/robot/subsystems/Arm.java | 78 +++++++++++++++---- 2 files changed, 77 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/TeleopEric.java b/src/main/java/frc/robot/commands/TeleopEric.java index 6e31558..4ca07ad 100644 --- a/src/main/java/frc/robot/commands/TeleopEric.java +++ b/src/main/java/frc/robot/commands/TeleopEric.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.*; + public class TeleopEric extends Command { int desiredElevatorLevelCargo = 0; @@ -225,6 +226,21 @@ else if (CommandBase.controls.Driver.getAButton()) { if (CommandBase.controls.Driver.getBumperPressed(Hand.kRight)){ CommandBase.elevator.set(1); } + + if (CommandBase.arm.getActivity() == false){ + if (CommandBase.controls.Operator.getYButtonPressed()){ + CommandBase.arm.goToSensor(2); + } + if (CommandBase.controls.Operator.getBButtonPressed()){ + CommandBase.arm.goToSensor(1); + } + if (CommandBase.controls.Operator.getAButtonPressed()){ + CommandBase.arm.goToSensor(0); + } + } + else{ + return; + } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 3f6ef79..8ff9342 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.AnalogPotentiometer; import java.util.Arrays; +import java.beans.DesignMode; import java.util.ArrayList; /** * Add your docs here. @@ -33,8 +34,8 @@ public class Arm extends Subsystem { private DigitalInput hallSensor0 = new DigitalInput(7); private DigitalInput hallSensor1 = new DigitalInput(8); private DigitalInput hallSensor2 = new DigitalInput(9); - static int previousPosition = -1; - + static int previousSensor = 0; + static boolean isRunning = false; static private boolean ActuatorInTransit; @@ -66,6 +67,14 @@ public double getRate() { return potPort.getAverageBits(); } + public void toggleActivity(){ + isRunning = !isRunning; + } + + public boolean getActivity(){ + return isRunning; + } + public void setArm(double speed){ // Update sensors boolean sensor0Status = getSensor(0); @@ -78,30 +87,15 @@ public void setArm(double speed){ (speed > 0 && sensor0Status) = 1 & 0 = False False || False = False */ - System.out.println("Sensors (0):"); - System.out.println(sensor0Status); - System.out.println(sensor2Status); - - System.out.println(";"); actuator.set(speed); } else if (!sensor0Status && !sensor2Status){ /* 1 & 0 = 0 = False */ - System.out.println("Sensors (1):"); - System.out.println(!sensor0Status); - System.out.println(!sensor2Status); - - System.out.println(";"); actuator.set(speed); } else{ - System.out.println("Sensors (2):"); - System.out.println(sensor0Status); - System.out.println(sensor2Status); - - System.out.println(";"); actuator.set(0); } @@ -111,6 +105,56 @@ else if (!sensor0Status && !sensor2Status){ } + + public void setArm(double speed, int destinationSensor){ + toggleActivity(); + while (!getSensor(destinationSensor)){ + setArm(speed); + } + previousSensor = destinationSensor; + toggleActivity(); + } + public void goToSensor(int destinationSensor){ + if(previousSensor == 2) + { + if (destinationSensor == 2){ + /* do nothing */ + } + else{ + setArm(-0.7, destinationSensor); + } + + } + + else if (previousSensor == 1){ + if (destinationSensor == 0){ + setArm(-0.7, destinationSensor); + } + else if (destinationSensor == 1){ + /* Don't move */ + } + else if (destinationSensor == 2){ + setArm(+0.7, destinationSensor); + } + + } + else if (previousSensor == 0){ + if (destinationSensor == 0){ + /* Don't move */ + } + else if (destinationSensor == 1){ + setArm(+0.7, destinationSensor); + } + else if (destinationSensor == 2){ + setArm(+0.7, destinationSensor); + } + } + + + + } + + @Override public void initDefaultCommand() {} }