projects
/
3501
/
2015-FRC-Spark
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
finish MoveDistance command with acceleration function
[3501/2015-FRC-Spark]
/
src
/
org
/
usfirst
/
frc3501
/
RiceCatRobot
/
commands
/
MoveDistance.java
diff --git
a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
index 88827f985666456683e4ac627c3a19ae8927de38..2243bc7a88e1912eebcbfb646033f4a341956988 100644
(file)
--- a/
src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
+++ b/
src/org/usfirst/frc3501/RiceCatRobot/commands/MoveDistance.java
@@
-1,43
+1,48
@@
package org.usfirst.frc3501.RiceCatRobot.commands;
import org.usfirst.frc3501.RiceCatRobot.Robot;
package org.usfirst.frc3501.RiceCatRobot.commands;
import org.usfirst.frc3501.RiceCatRobot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
import edu.wpi.first.wpilibj.command.Command;
public class MoveDistance extends Command {
import edu.wpi.first.wpilibj.command.Command;
public class MoveDistance extends Command {
- double distance;
-
- public MoveDistance(double distance
)
{
+ double distance
, minSpeed, maxSpeed
;
+
+ public MoveDistance(double distance
, double minSpeed, double maxSpeed)
{
requires(Robot.driveTrain);
this.distance = distance;
requires(Robot.driveTrain);
this.distance = distance;
+ this.minSpeed = minSpeed;
+ this.maxSpeed = maxSpeed;
}
}
-
- protected void initialize(){
-
+
+ protected void initialize()
{
+ Robot.driveTrain.resetEncoders();
}
@Override
protected void execute() {
}
@Override
protected void execute() {
- // TODO Auto-generated method stub
-
+ double speed = 4 * (minSpeed - maxSpeed) *
+ Math.pow((Robot.driveTrain.getAverageSpeed() / distance - 0.5), 2)
+ + maxSpeed;
+ Robot.driveTrain.setMotorSpeeds(speed, speed);
}
@Override
protected boolean isFinished() {
}
@Override
protected boolean isFinished() {
- // TODO Auto-generated method stub
+ if (Robot.driveTrain.getLeftDistance() > distance
+ && Robot.driveTrain.getRightDistance() > distance)
+ return true;
return false;
}
return false;
}
-
+
@Override
protected void end() {
@Override
protected void end() {
- // TODO Auto-generated method stub
-
+ Robot.driveTrain.stop();
}
@Override
protected void interrupted() {
}
@Override
protected void interrupted() {
- // TODO Auto-generated method stub
-
+ end();
}
}
}
}