diff --git a/WiredCats2014/nbproject/private/private.xml b/WiredCats2014/nbproject/private/private.xml
index 93e0583..51ca675 100644
--- a/WiredCats2014/nbproject/private/private.xml
+++ b/WiredCats2014/nbproject/private/private.xml
@@ -1,10 +1,10 @@
-
-
-
-
-
- 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
-
-
-
+
+
+
+
+
+ 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 922c93a..27c35ce 100644
--- a/WiredCats2014/nbproject/project.bak.xml
+++ b/WiredCats2014/nbproject/project.bak.xml
@@ -3,89 +3,91 @@
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
-
-
- WiredCats2415
-
- ${user.home}/.sunspotfrc.properties
- build.properties
- ${sunspot.home}/default.properties
-
-
-
-
- java
- src
-
-
-
-
- jar-app
-
-
- clean
-
-
- deploy
- run
-
-
- clean
- jar-app
-
-
- deploy
- debug-run
-
-
- javadoc
-
-
-
- folder
- build
- jar-app
-
-
-
-
-
- src
-
-
- build.xml
-
-
-
-
-
-
-
-
-
-
-
- deploy
-
-
-
- jar-deploy
-
-
-
-
-
-
-
-
- src
- ${sunspot.home}\lib\squawk.jar
- ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar
- build
- 1.4
-
-
-
-
\ No newline at end of file
+
+ -->
+
+ org.netbeans.modules.ant.freeform
+
+
+ WiredCats2014
+
+ ${user.home}/.sunspotfrc.properties
+ build.properties
+ ${sunspot.home}/default.properties
+
+
+
+
+ java
+ src
+
+
+
+
+ jar-app
+
+
+ clean
+
+
+ deploy
+ run
+
+
+ clean
+ jar-app
+
+
+ deploy
+ debug-run
+
+
+ javadoc
+
+
+
+ folder
+ build
+ jar-app
+
+
+
+
+
+ src
+
+
+ build.xml
+
+
+
+
+
+
+
+
+
+
+
+ deploy
+
+
+
+ jar-deploy
+
+
+
+
+
+
+
+
+ src
+ ${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 27c35ce..91b63c9 100644
--- a/WiredCats2014/nbproject/project.xml
+++ b/WiredCats2014/nbproject/project.xml
@@ -83,8 +83,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/commands/AutonomousCommands/CommandStraightDrive.java b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java
index 2332516..d77a73c 100644
--- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java
+++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/commands/AutonomousCommands/CommandStraightDrive.java
@@ -6,6 +6,7 @@
package edu.wpi.first.wpilibj.templates.commands.AutonomousCommands;
+import Utilities.WiredVector;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
/**
@@ -15,18 +16,67 @@
*/
public class CommandStraightDrive extends CommandBase {
+ //sinusoidal motion profiling.
+
+ private WiredVector speeds;
+
+ //In polar coordinates
+ private float destination;
+ private float currPosition;
+ private float startingPosition;
+
public CommandStraightDrive(float distance){
+ speeds = new WiredVector();
+ currPosition = 0;
+ destination = distance;
+ startingPosition = drivesubsystem.getEncoderValue();
+
}
protected void initialize() {
- }
+
+ }
protected void execute() {
+
+
+ currPosition = drivesubsystem.getEncoderValue();
+ float currVelocity = drivesubsystem.getSpeed();
+
+ float power = drivesubsystem.turnPID.pid(getDesiredVelocity(), currVelocity);
+
+ //STRAIGHT DRIVE CODE YET TO BE CODED.
+ //TODO
+ drivesubsystem.setLeftRight(power, power);
+ }
+
+ public float getDesiredVelocity(){
+ if ( currPosition < .2*destination){
+ return (float)(drivesubsystem.MAX_VELOCITY/(.2*destination))*currPosition;
+ } else if ( currPosition > .8*destination){
+ return -(float)((drivesubsystem.MAX_VELOCITY/(.2*destination))*(currPosition-(.8f*destination)));
+ } else {
+ return drivesubsystem.MAX_VELOCITY;
+ }
+ }
+
+ /**
+ * 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();
}
protected boolean isFinished() {
- return false;
+ return destination - currPosition < 0;
}
protected void end() {
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 2943e60..f9791ef 100644
--- a/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java
+++ b/WiredCats2014/src/edu/wpi/first/wpilibj/templates/subsystems/SubSystemDrive.java
@@ -3,15 +3,12 @@
import Utilities.ChezyGyro;
import Utilities.PID;
-import edu.wpi.first.wpilibj.Gyro;
-import edu.wpi.first.wpilibj.Accelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.CommandArcadeDrive;
-import edu.wpi.first.wpilibj.templates.commands.CommandTankDrive;
/**
*
@@ -22,7 +19,7 @@ public class SubSystemDrive extends Subsystem {
public static final int TICKS_PER_REVOLUTION = 120;
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;
+ public static final float TICKS_TO_FEET = WHEEL_CIRCUMFERENCE_FEET / TICKS_PER_REVOLUTION;
private Talon left = new Talon(RobotMap.DRIVE_LEFT_MOTOR_1);
private Talon left2 = new Talon(RobotMap.DRIVE_LEFT_MOTOR_2);
@@ -45,6 +42,8 @@ public class SubSystemDrive extends Subsystem {
public PID straightPID;
public PID turnPID;
+ public float MAX_VELOCITY = 16;
+
public void init(){
gyro.initGyro();
leftEncoder.start();
@@ -79,6 +78,12 @@ public void setLowSpeed(){
highSpeedSolenoid.set(false);
}
+ public float getEncoderValue(){
+ int ticks = (leftEncoder.getRaw() + -rightEncoder.getRaw());
+ ticks /= 2;
+ return (TICKS_TO_FEET * ticks);
+ }
+
/**
* Returns the speed of the robot in feet per second.
* @return
@@ -86,7 +91,7 @@ public void setLowSpeed(){
public float getSpeed(){
float ticks = (float)(leftEncoder.getRate() + -rightEncoder.getRate());
ticks /= 2;
- return TICKS_TO_FEET_PER_SECOND * ticks;
+ return TICKS_TO_FEET * ticks;
}
public float getAngle(){