From dd95eda58d8e1016f902ac3f123adf7d554010a9 Mon Sep 17 00:00:00 2001 From: Bruce Crane Date: Sat, 8 Feb 2014 13:36:14 -0500 Subject: [PATCH 1/3] Updated RobotMap - Updated robotmap - cleaned up subsystem drive. --- WiredCats2014/nbproject/project.bak.xml | 18 ++++++------ WiredCats2014/nbproject/project.xml | 7 ++--- .../wpi/first/wpilibj/templates/RobotMap.java | 18 ++++++++---- .../templates/subsystems/SubSystemDrive.java | 29 ++++--------------- 4 files changed, 29 insertions(+), 43 deletions(-) diff --git a/WiredCats2014/nbproject/project.bak.xml b/WiredCats2014/nbproject/project.bak.xml index 6da2d76..9d8855f 100644 --- a/WiredCats2014/nbproject/project.bak.xml +++ b/WiredCats2014/nbproject/project.bak.xml @@ -1,14 +1,14 @@ - + + org.netbeans.modules.ant.freeform - WiredCats2014 + WiredCats2415 ${user.home}/.sunspotfrc.properties build.properties @@ -82,8 +82,8 @@ src - ${sunspot.home}\lib\squawk.jar - ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar + ${sunspot.home}/lib/squawk.jar + ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar build 1.4 diff --git a/WiredCats2014/nbproject/project.xml b/WiredCats2014/nbproject/project.xml index 594e6e5..237fa58 100644 --- a/WiredCats2014/nbproject/project.xml +++ b/WiredCats2014/nbproject/project.xml @@ -3,8 +3,7 @@ This is a sample netbeans project file for a Sun Spot Application project. You may edit it freely, it doesn't affect the ant-powered build. ---> - +--> org.netbeans.modules.ant.freeform @@ -82,8 +81,8 @@ src - ${sunspot.home}/lib/squawk.jar - ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar + ${sunspot.home}\lib\squawk.jar + ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar build 1.4 diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java index 17c53a6..bd9924c 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java @@ -14,14 +14,20 @@ public class RobotMap { public static final int DRIVER = 1; - public static final int DRIVE_LEFT = 2; - public static final int DRIVE_RIGHT = 1; - public static final int DRIVE_GYRO = 2; + public static final int DRIVE_LEFT_MOTOR_1 = 1; + public static final int DRIVE_LEFT_MOTOR_2 = 4; + public static final int DRIVE_RIGHT_MOTOR_1 = 2; + public static final int DRIVE_RIGHT_MOTOR_2 = 3; + public static final int DRIVE_GYRO = 1; public static final int DRIVE_ACCEL = 10; + public static final int DRIVE_LOW_SPEED_SOLENOID = 1; + public static final int DRIVE_HIGH_SPEED_SOLENOID = 2; - public static final int ARM_MOTOR = 3; - public static final int ARM_ENCODER_A = 9; - public static final int ARM_ENCODER_B = 8; + public static final int ARM_MOTOR = 9; + public static final int DRIVE_LEFT_ENCODER_A = 6; + public static final int DRIVE_LEFT_ENCODER_B = 7; + public static final int DRIVE_RIGHT_ENCODER_A = 8; + public static final int DRIVE_RIGHT_ENCODER_B = 9; // If you are using multiple modules, make sure to define both the port diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java index 7458368..0fa0df2 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java @@ -19,7 +19,7 @@ public class SubSystemDrive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. public static final int TICKS_PER_ROTATION = 240; - + public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((2*2*Math.PI)/12); public static final float TICKS_TO_FEET = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_ROTATION; @@ -34,7 +34,7 @@ public class SubSystemDrive extends Subsystem { RobotMap.DRIVE_LEFT_ENCODER_B); Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, RobotMap.DRIVE_RIGHT_ENCODER_B); - + Solenoid lowSpeedSolenoid = new Solenoid(RobotMap.DRIVE_LOW_SPEED_SOLENOID); Solenoid highSpeedSolenoid = new Solenoid(RobotMap.DRIVE_HIGH_SPEED_SOLENOID); @@ -57,7 +57,7 @@ public void setRight(double power){ this.right.set(-power); right2.set(-power); } - + public void setHighSpeed(){ highSpeedSolenoid.set(true); lowSpeedSolenoid.set(false); @@ -78,7 +78,7 @@ public void setLowSpeed(){ highSpeedSolenoid.set(false); } - public float getGyroAngle(){ + public float getGyroAngle(){ return (float)gyro.getAngle(); } @@ -86,30 +86,11 @@ public float getGyroAngle(){ // return (float)gyro. // } // - public void setHighSpeed(){ - lowSpeedSolenoid.set(false); - highSpeedSolenoid.set(true); - } - - public void setLowSpeed(){ - lowSpeedSolenoid.set(true); - highSpeedSolenoid.set(false); - } - + public boolean isHighSpeed(){ return highSpeedSolenoid.get(); } - /** - * Returns the robots speed in ft/s. - * @return - */ - public float getSpeed(){ - float ticks = (float)(leftEncoder.getRate() + rightEncoder.getRate()); - ticks /= 2; - return TICKS_TO_FEET * ticks; - } - public void resetGyro(){ gyro.reset(); } From 9437b2ed50210ddb0e008e292ba9c0566ebe9c4f Mon Sep 17 00:00:00 2001 From: Bruce Crane Date: Sat, 8 Feb 2014 16:49:29 -0500 Subject: [PATCH 2/3] Automatic Transmission - Successfully interfaced with drive encoders. - added functionality to poll for robot speed. - added automatic transmission - if robot velocity dips too low, shift down. - if robot velocity passes upper band, shift up. - all that crap that happened when merging has been resolved. - added command ChangeSpeed. --- .gitignore | 1 + WiredCats2014/nbproject/private/private.xml | 7 +-- WiredCats2014/nbproject/project.bak.xml | 15 +++-- WiredCats2014/src/Utilities/ChezyGyro.java | 8 +++ WiredCats2014/src/Utilities/WiredVector.java | 14 +++-- .../wpi/first/wpilibj/templates/RobotMap.java | 27 ++++----- .../wpilibj/templates/WiredCatsRobot.java | 2 +- .../commands/CommandArcadeDrive.java | 59 +++++++++++++++---- .../templates/commands/CommandBase.java | 6 +- .../commands/CommandChangeSpeed.java | 39 ++++++++++++ .../templates/subsystems/SubSystemDrive.java | 19 +++--- 11 files changed, 142 insertions(+), 55 deletions(-) create mode 100644 WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java diff --git a/.gitignore b/.gitignore index d37aceb..06ea241 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /TestAutotune/nbproject/private/ /WiredCats2415/nbproject/private/ WiredCats2014/nbproject/project.bak.xml +/WiredCats2014/build/ \ No newline at end of file diff --git a/WiredCats2014/nbproject/private/private.xml b/WiredCats2014/nbproject/private/private.xml index 12a3e99..93e0583 100644 --- a/WiredCats2014/nbproject/private/private.xml +++ b/WiredCats2014/nbproject/private/private.xml @@ -3,11 +3,8 @@ - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/Utilities/TXTReader.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java - file:/C:/Users/WiredCats/Documents/NetBeansProjects/WiredCats2014/WiredCats2415/src/Utilities/GamePad.java + file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java + file:/C:/Users/WiredCats/Documents/GitHub/WiredCats2014/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java diff --git a/WiredCats2014/nbproject/project.bak.xml b/WiredCats2014/nbproject/project.bak.xml index 9d8855f..6df50d7 100644 --- a/WiredCats2014/nbproject/project.bak.xml +++ b/WiredCats2014/nbproject/project.bak.xml @@ -1,10 +1,9 @@ - - + org.netbeans.modules.ant.freeform @@ -82,8 +81,8 @@ src - ${sunspot.home}/lib/squawk.jar - ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar + ${sunspot.home}\lib\squawk.jar + ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar build 1.4 diff --git a/WiredCats2014/src/Utilities/ChezyGyro.java b/WiredCats2014/src/Utilities/ChezyGyro.java index aba72ab..e7d0dad 100644 --- a/WiredCats2014/src/Utilities/ChezyGyro.java +++ b/WiredCats2014/src/Utilities/ChezyGyro.java @@ -157,6 +157,14 @@ public double getAngle() { return scaledValue; } } + /** + * Returns the angular velocity of the robot. + * @author Sam Crane + * @return + */ + public double getAngularVelocity(){ + return m_analog.getAverageValue(); + } /** * Set the gyro type based on the sensitivity. diff --git a/WiredCats2014/src/Utilities/WiredVector.java b/WiredCats2014/src/Utilities/WiredVector.java index c3f4e89..8c35c28 100644 --- a/WiredCats2014/src/Utilities/WiredVector.java +++ b/WiredCats2014/src/Utilities/WiredVector.java @@ -24,26 +24,30 @@ public WiredVector() { * This function adds a value to the Blackbox's list of values * @param input Value to be added */ - public void addVal(int input) { + public void addVal(float input) { valueList.addElement(new OurInteger(input)); } - public int getVal(int index){ + public float getVal(int index){ return((OurInteger)valueList.elementAt(index)).i; } - public int lastElement(){ + public float lastElement(){ return ((OurInteger)valueList.lastElement()).i; } + public void removeFirst(){ + if (valueList.elementAt(0) != null) valueList.removeElementAt(0); + } + public int size(){ return valueList.size(); } private class OurInteger { - public int i; + public float i; - public OurInteger(int i){ + public OurInteger(float i){ this.i = i; } } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java index bd9924c..74bcd47 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/RobotMap.java @@ -14,24 +14,19 @@ public class RobotMap { public static final int DRIVER = 1; + //PWM public static final int DRIVE_LEFT_MOTOR_1 = 1; - public static final int DRIVE_LEFT_MOTOR_2 = 4; - public static final int DRIVE_RIGHT_MOTOR_1 = 2; - public static final int DRIVE_RIGHT_MOTOR_2 = 3; + public static final int DRIVE_LEFT_MOTOR_2 = 2; + public static final int DRIVE_RIGHT_MOTOR_1 = 3; + public static final int DRIVE_RIGHT_MOTOR_2 = 4; + //analog public static final int DRIVE_GYRO = 1; - public static final int DRIVE_ACCEL = 10; +// public static final int DRIVE_ACCEL = 10; public static final int DRIVE_LOW_SPEED_SOLENOID = 1; public static final int DRIVE_HIGH_SPEED_SOLENOID = 2; - - public static final int ARM_MOTOR = 9; - public static final int DRIVE_LEFT_ENCODER_A = 6; - public static final int DRIVE_LEFT_ENCODER_B = 7; - public static final int DRIVE_RIGHT_ENCODER_A = 8; - public static final int DRIVE_RIGHT_ENCODER_B = 9; - - - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static final int rangefinderPort = 1; - // public static final int rangefinderModule = 1; + //digital + public static final int DRIVE_LEFT_ENCODER_A = 1; + public static final int DRIVE_LEFT_ENCODER_B = 2; + public static final int DRIVE_RIGHT_ENCODER_A = 3; + public static final int DRIVE_RIGHT_ENCODER_B = 4; } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java index b3ace3d..5fd6edd 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/WiredCatsRobot.java @@ -54,7 +54,7 @@ public void teleopInit() { // continue until interrupted by another command, remove // this line or comment it out. //autonomousCommand.cancel(); - + CommandBase.resources.getFromFile("wiredCatsConfig.txt"); } /** diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java index 2c5309a..f11bcdd 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java @@ -3,6 +3,7 @@ * and open the template in the editor. */ package edu.wpi.first.wpilibj.templates.commands; +import Utilities.WiredVector; import edu.wpi.first.wpilibj.command.Scheduler; import java.lang.Math; /** @@ -11,19 +12,29 @@ */ public class CommandArcadeDrive extends CommandBase{ - public static float PRIMARY_TURN_COEFFICIENT = 0.8f; - public static float SECONDARY_TURN_COEFFICIENT = 1.0f; + private float primaryTurnCoefficient = 0.8f; + private float SECONDARY_TURN_COEFFICIENT = 1.0f; + private float jsDeadband = 0.06f; + private float interpolationBias = 0.7f; + private float LOW_SPEED_MAX = 12; //ft/s - double DEADBAND = 0.06; + private float UPPER_SHIFT_LIMIT = .8f * LOW_SPEED_MAX; + private float LOWER_SHIFT_LIMIT = 4; + + private WiredVector speeds; public CommandArcadeDrive() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); requires(drivesubsystem); + speeds = new WiredVector(); } // Called just before this Command runs the first time protected void initialize() { + primaryTurnCoefficient = (float)resources.getValue("primaryTurnCoefficient"); + jsDeadband = (float)resources.getValue("jsDeadband"); + interpolationBias = (float)resources.getValue("interpolationBias"); } // Called repeatedly when this Command is scheduled to run @@ -32,23 +43,51 @@ protected void execute() { double y = jsdriver.leftY(); double x = jsdriver.rightX(); - if(x <= -DEADBAND && x >= DEADBAND) x = 0; - if(y <= -DEADBAND && y >= DEADBAND) y = 0; + System.out.println(drivesubsystem.getSpeed()); + + if(Math.abs(x) < jsDeadband) x = 0; + if(Math.abs(y) < jsDeadband) y = 0; + + y = interpolationBias*y + (1-interpolationBias)*y*y*y; double left; double right; - if(Math.abs(y) <= DEADBAND){ - left = y - SECONDARY_TURN_COEFFICIENT*x; - right = y + SECONDARY_TURN_COEFFICIENT*x; + if(Math.abs(y) <= jsDeadband){ + left = y + SECONDARY_TURN_COEFFICIENT*x; + right = y - SECONDARY_TURN_COEFFICIENT*x; }else{ - left = y - PRIMARY_TURN_COEFFICIENT*x; - right = y + PRIMARY_TURN_COEFFICIENT*x; + left = y + primaryTurnCoefficient*x; + right = y - primaryTurnCoefficient*x; + } + + //shifting code. + float avgSpd = getAverageSpeed(); + if (avgSpd > UPPER_SHIFT_LIMIT && !drivesubsystem.isHighSpeed()){ + drivesubsystem.setHighSpeed(); + } else if (avgSpd < LOWER_SHIFT_LIMIT && drivesubsystem.isHighSpeed()){ + drivesubsystem.setLowSpeed(); } + + //System.out.println("[WiredCats] Gyroscope: " + drivesubsystem.getAngle()); drivesubsystem.setLeft(left); drivesubsystem.setRight(right); } + + /** + * Returns the average current speed of the drivetrain in feet/second. + */ + public float getAverageSpeed(){ + //TODO + float sum = 0; + speeds.addVal(drivesubsystem.getSpeed()); + for (int i = 0; i < speeds.size(); i++){ + sum+= speeds.getVal(i); + } + if (speeds.size() > 10) speeds.removeFirst(); + return sum / speeds.size(); + } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java index c2461ad..76b74f9 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java @@ -30,8 +30,10 @@ public static void init() { // which commands extend), subsystems are not guaranteed to be // yet. Thus, their requires() statements may grab null pointers. Bad // news. Don't move it. - jsdriver.a_button.whenReleased(new CommandTankDrive()); - jsdriver.b_button.whenReleased(new CommandArcadeDrive()); + resources.getFromFile("wiredCatsConfig.txt"); + jsdriver.b_button.whenReleased(new CommandChangeSpeed()); + + drivesubsystem.init(); // Show what command your subsystem is running on the SmartDashboard //SmartDashboard.putData("Autotune PID", new CommandAutotunePID()); } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java new file mode 100644 index 0000000..a1daefa --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java @@ -0,0 +1,39 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands; + +/** + * + * @author WiredCats + */ +public class CommandChangeSpeed extends CommandBase{ + + protected void initialize() { + if (drivesubsystem.isHighSpeed()){ + drivesubsystem.setLowSpeed(); + } else { + drivesubsystem.setHighSpeed(); + } + } + + protected void execute() { + + } + + protected boolean isFinished() { + return true; + } + + protected void end() { + + } + + protected void interrupted() { + + } + +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java index 0fa0df2..3d76163 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java @@ -18,10 +18,10 @@ public class SubSystemDrive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - public static final int TICKS_PER_ROTATION = 240; + public static final int TICKS_PER_REVOLUTION = 120; public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((2*2*Math.PI)/12); - public static final float TICKS_TO_FEET = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_ROTATION; + public static final float TICKS_TO_FEET_PER_SECOND = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION; Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1); @@ -29,7 +29,7 @@ public class SubSystemDrive extends Subsystem { Talon right = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_1); Talon right2 = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_2); ChezyGyro gyro = new ChezyGyro(RobotMap.DRIVE_GYRO); - Accelerometer accel = new Accelerometer(RobotMap.DRIVE_ACCEL); + //Accelerometer accel = new Accelerometer(RobotMap.DRIVE_ACCEL); Encoder leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODER_A, RobotMap.DRIVE_LEFT_ENCODER_B); Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, @@ -40,6 +40,8 @@ public class SubSystemDrive extends Subsystem { public void init(){ gyro.initGyro(); + leftEncoder.start(); + rightEncoder.start(); } public void initDefaultCommand() { @@ -49,12 +51,12 @@ public void initDefaultCommand() { public void setLeft(double power){ - this.left.set(power); + left.set(power); left2.set(power); } public void setRight(double power){ - this.right.set(-power); + right.set(-power); right2.set(-power); } @@ -68,9 +70,10 @@ public void setHighSpeed(){ * @return */ public float getSpeed(){ - float ticks = (float)(leftEncoder.getRate() + rightEncoder.getRate()); +// System.out.println(leftEncoder.getRate() + ", " + rightEncoder.getRate()); + float ticks = (float)(leftEncoder.getRate() + -rightEncoder.getRate()); ticks /= 2; - return TICKS_TO_FEET * ticks; + return TICKS_TO_FEET_PER_SECOND * ticks; } public void setLowSpeed(){ @@ -78,7 +81,7 @@ public void setLowSpeed(){ highSpeedSolenoid.set(false); } - public float getGyroAngle(){ + public float getAngle(){ return (float)gyro.getAngle(); } From 3856cfb564ad8b2b0b54bd65dfd43ac7b9e2068b Mon Sep 17 00:00:00 2001 From: Bruce Crane Date: Sun, 9 Feb 2014 16:30:36 -0500 Subject: [PATCH 3/3] Autonomous Commit - Began setting up autonomous. - Began working on creating autonomous driving commands. - Added a wait command to wait for a set duration. - Began looking into sinusoidal motion profiling. --- WiredCats2014/src/Utilities/ChezyGyro.java | 3 + .../CommandStraightDrive.java | 37 ++++++++++ .../AutonomousCommands/CommandTurn.java | 46 +++++++++++++ .../AutonomousCommands/CommandWait.java | 42 ++++++++++++ .../commands/CommandArcadeDrive.java | 31 ++++----- .../templates/commands/CommandBase.java | 2 +- ...mandChangeSpeed.java => CommandShift.java} | 2 +- .../templates/commands/CommandTankDrive.java | 3 +- .../templates/subsystems/SubSystemDrive.java | 67 ++++++++++--------- 9 files changed, 182 insertions(+), 51 deletions(-) create mode 100644 WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java create mode 100644 WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java create mode 100644 WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java rename WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/{CommandChangeSpeed.java => CommandShift.java} (92%) diff --git a/WiredCats2014/src/Utilities/ChezyGyro.java b/WiredCats2014/src/Utilities/ChezyGyro.java index e7d0dad..8d1289d 100644 --- a/WiredCats2014/src/Utilities/ChezyGyro.java +++ b/WiredCats2014/src/Utilities/ChezyGyro.java @@ -163,6 +163,9 @@ public double getAngle() { * @return */ public double getAngularVelocity(){ + if (m_analog == null){ + return 0.0; + } return m_analog.getAverageValue(); } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java new file mode 100644 index 0000000..2332516 --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java @@ -0,0 +1,37 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * Drives in a straight line from whatever orientation + * the robot is in upon execution of the command. + * @author WiredCats + */ +public class CommandStraightDrive extends CommandBase { + + public CommandStraightDrive(float distance){ + + } + + protected void initialize() { + } + + protected void execute() { + } + + protected boolean isFinished() { + return false; + } + + protected void end() { + } + + protected void interrupted() { + } +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java new file mode 100644 index 0000000..52f39d2 --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandTurn.java @@ -0,0 +1,46 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * + * @author WiredCats + */ +public class CommandTurn extends CommandBase { + + /** + * Turns this many degrees from the current orientation. + * @param goal the desired orientation. + */ + public CommandTurn(float goal){ + + } + + protected void initialize() { + + } + + protected void execute() { + + } + + protected boolean isFinished() { + //TODO + return false; + } + + protected void end() { + + } + + protected void interrupted() { + + } + +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java new file mode 100644 index 0000000..981439c --- /dev/null +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandWait.java @@ -0,0 +1,42 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.templates.commands.CommandBase; + +/** + * + * @author WiredCats + */ +public class CommandWait extends CommandBase{ + + private double time; + private Timer t; + + public CommandWait(double time){ + this.time = time; + } + + protected void initialize() { + t = new Timer(); + t.start(); + } + + protected void execute() { + } + + protected boolean isFinished() { + return t.get() > time; + } + + protected void end() { + } + + protected void interrupted() { + } +} diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java index f11bcdd..97f34a7 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandArcadeDrive.java @@ -3,9 +3,8 @@ * and open the template in the editor. */ package edu.wpi.first.wpilibj.templates.commands; +import Utilities.PID; import Utilities.WiredVector; -import edu.wpi.first.wpilibj.command.Scheduler; -import java.lang.Math; /** * * @author benbarber @@ -13,13 +12,11 @@ public class CommandArcadeDrive extends CommandBase{ private float primaryTurnCoefficient = 0.8f; - private float SECONDARY_TURN_COEFFICIENT = 1.0f; private float jsDeadband = 0.06f; private float interpolationBias = 0.7f; - private float LOW_SPEED_MAX = 12; //ft/s - private float UPPER_SHIFT_LIMIT = .8f * LOW_SPEED_MAX; - private float LOWER_SHIFT_LIMIT = 4; + private float upperShiftLimit = 7.0f; // ft/s + private float lowerShiftLimit = 4.0f; // ft/s private WiredVector speeds; @@ -35,9 +32,10 @@ protected void initialize() { primaryTurnCoefficient = (float)resources.getValue("primaryTurnCoefficient"); jsDeadband = (float)resources.getValue("jsDeadband"); interpolationBias = (float)resources.getValue("interpolationBias"); + upperShiftLimit = (float)resources.getValue("upperShiftLimit"); + lowerShiftLimit = (float)resources.getValue("lowerShiftLimit"); } - // Called repeatedly when this Command is scheduled to run protected void execute() { double y = jsdriver.leftY(); @@ -54,8 +52,8 @@ protected void execute() { double right; if(Math.abs(y) <= jsDeadband){ - left = y + SECONDARY_TURN_COEFFICIENT*x; - right = y - SECONDARY_TURN_COEFFICIENT*x; + left = y + x; + right = y - x; }else{ left = y + primaryTurnCoefficient*x; right = y - primaryTurnCoefficient*x; @@ -63,16 +61,15 @@ protected void execute() { //shifting code. float avgSpd = getAverageSpeed(); - if (avgSpd > UPPER_SHIFT_LIMIT && !drivesubsystem.isHighSpeed()){ + if (avgSpd > upperShiftLimit && !drivesubsystem.isHighSpeed()){ drivesubsystem.setHighSpeed(); - } else if (avgSpd < LOWER_SHIFT_LIMIT && drivesubsystem.isHighSpeed()){ + } else if (avgSpd < lowerShiftLimit && drivesubsystem.isHighSpeed()){ drivesubsystem.setLowSpeed(); } //System.out.println("[WiredCats] Gyroscope: " + drivesubsystem.getAngle()); - drivesubsystem.setLeft(left); - drivesubsystem.setRight(right); + drivesubsystem.setLeftRight(left,right); } /** @@ -98,9 +95,13 @@ protected boolean isFinished() { protected void end() { } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run + /** + * Called when arcade drive is interrupted by + * another command. Make sure to power down any + * motors to avoid any nasty loss of control. + */ protected void interrupted() { System.out.println("Arcade Drive interrupted."); + drivesubsystem.setLeftRight(0,0); } } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java index 76b74f9..de5c5cb 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandBase.java @@ -31,7 +31,7 @@ public static void init() { // yet. Thus, their requires() statements may grab null pointers. Bad // news. Don't move it. resources.getFromFile("wiredCatsConfig.txt"); - jsdriver.b_button.whenReleased(new CommandChangeSpeed()); + jsdriver.b_button.whenReleased(new CommandShift()); drivesubsystem.init(); // Show what command your subsystem is running on the SmartDashboard diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java similarity index 92% rename from WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java rename to WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java index a1daefa..c4cb1d3 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandChangeSpeed.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandShift.java @@ -10,7 +10,7 @@ * * @author WiredCats */ -public class CommandChangeSpeed extends CommandBase{ +public class CommandShift extends CommandBase{ protected void initialize() { if (drivesubsystem.isHighSpeed()){ diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java index aba7676..9d312db 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/CommandTankDrive.java @@ -33,8 +33,7 @@ protected void execute() { float actualLeftMotor = (float)actualMotor(theorMotor(theorJS(actualLeftJS))); float actualRightMotor = (float)actualMotor(theorMotor(theorJS(actualRightJS))); - drivesubsystem.setLeft(actualLeftMotor); - drivesubsystem.setRight(actualRightMotor); + drivesubsystem.setLeftRight(actualLeftMotor,actualRightMotor); } diff --git a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java index 3d76163..2943e60 100644 --- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java +++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java @@ -2,6 +2,7 @@ package edu.wpi.first.wpilibj.templates.subsystems; import Utilities.ChezyGyro; +import Utilities.PID; import edu.wpi.first.wpilibj.Gyro; import edu.wpi.first.wpilibj.Accelerometer; import edu.wpi.first.wpilibj.Encoder; @@ -19,29 +20,36 @@ public class SubSystemDrive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. public static final int TICKS_PER_REVOLUTION = 120; - - public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((2*2*Math.PI)/12); + public static final int WHEEL_RADIUS = 2; + public static final float WHEEL_CIRCUMFERENCE_FEET = (float)((WHEEL_RADIUS*2*Math.PI)/12); public static final float TICKS_TO_FEET_PER_SECOND = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION; - - Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1); - Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2); - Talon right = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_1); - Talon right2 = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_2); - ChezyGyro gyro = new ChezyGyro(RobotMap.DRIVE_GYRO); + private Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1); + private Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2); + private Talon right = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_1); + private Talon right2 = new Talon(RobotMap.DRIVE_RIGHT_MOTOR_2); + private ChezyGyro gyro = new ChezyGyro(RobotMap.DRIVE_GYRO); //Accelerometer accel = new Accelerometer(RobotMap.DRIVE_ACCEL); - Encoder leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODER_A, + private Encoder leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_ENCODER_A, RobotMap.DRIVE_LEFT_ENCODER_B); - Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, + private Encoder rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_ENCODER_A, RobotMap.DRIVE_RIGHT_ENCODER_B); + private Solenoid lowSpeedSolenoid = new Solenoid(RobotMap.DRIVE_LOW_SPEED_SOLENOID); + private Solenoid highSpeedSolenoid = new Solenoid(RobotMap.DRIVE_HIGH_SPEED_SOLENOID); - Solenoid lowSpeedSolenoid = new Solenoid(RobotMap.DRIVE_LOW_SPEED_SOLENOID); - Solenoid highSpeedSolenoid = new Solenoid(RobotMap.DRIVE_HIGH_SPEED_SOLENOID); + public PID lowStraightPID = new PID(0.5f,0,0); + public PID highStraightPID = new PID(0.5f,0,0); + public PID lowTurnPID = new PID(0.5f,0,0); + public PID highTurnPID = new PID(0.5f,0,0); + public PID straightPID; + public PID turnPID; + public void init(){ gyro.initGyro(); leftEncoder.start(); rightEncoder.start(); + setLowSpeed(); } public void initDefaultCommand() { @@ -49,20 +57,26 @@ public void initDefaultCommand() { setDefaultCommand(new CommandArcadeDrive()); } - - public void setLeft(double power){ - left.set(power); - left2.set(power); - } - - public void setRight(double power){ - right.set(-power); - right2.set(-power); + /** + * Sets the left and right side of the drivetrain. + * @param l + * @param r + */ + public void setLeftRight(double l, double r){ + left.set(l); + left2.set(l); + right.set(-r); + right2.set(-r); } public void setHighSpeed(){ highSpeedSolenoid.set(true); lowSpeedSolenoid.set(false); + } + + public void setLowSpeed(){ + lowSpeedSolenoid.set(true); + highSpeedSolenoid.set(false); } /** @@ -70,25 +84,14 @@ public void setHighSpeed(){ * @return */ public float getSpeed(){ -// System.out.println(leftEncoder.getRate() + ", " + rightEncoder.getRate()); float ticks = (float)(leftEncoder.getRate() + -rightEncoder.getRate()); ticks /= 2; return TICKS_TO_FEET_PER_SECOND * ticks; } - public void setLowSpeed(){ - lowSpeedSolenoid.set(true); - highSpeedSolenoid.set(false); - } - public float getAngle(){ return (float)gyro.getAngle(); } - -// public float getAngularVelocity(){ -// return (float)gyro. -// } -// public boolean isHighSpeed(){ return highSpeedSolenoid.get();