projects
/
3501
/
2017steamworks
/ commitdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
| commitdiff |
tree
raw
|
patch
|
inline
| side by side (parent:
bf921ec
)
fix bugs for TurnForAngle and tune constants
author
Cindy Zhang
<cindyzyx9@gmail.com>
Wed, 1 Feb 2017 04:06:09 +0000
(20:06 -0800)
committer
Cindy Zhang
<cindyzyx9@gmail.com>
Wed, 1 Feb 2017 04:06:09 +0000
(20:06 -0800)
src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
patch
|
blob
|
blame
|
history
src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
patch
|
blob
|
blame
|
history
src/org/usfirst/frc/team3501/robot/utils/PIDController.java
patch
|
blob
|
blame
|
history
diff --git
a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
index 86176f15fe9e3b03ebaf0ac447af00f597f1cb17..f9f5c7f0e285676a21b0548baa6d3a80df75879c 100755
(executable)
--- a/
src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
+++ b/
src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java
@@
-28,11
+28,14
@@
public class TurnForAngle extends Command {
private double gyroI;
private double gyroD;
private double gyroI;
private double gyroD;
+ private double zeroAngle;
+
public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
this.direction = direction;
this.maxTimeOut = maxTimeOut;
public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
this.direction = direction;
this.maxTimeOut = maxTimeOut;
- this.target = angle;
+ this.target = Math.abs(angle);
+ this.zeroAngle = driveTrain.getAngle();
this.gyroP = driveTrain.defaultGyroP;
this.gyroI = driveTrain.defaultGyroI;
this.gyroP = driveTrain.defaultGyroP;
this.gyroI = driveTrain.defaultGyroI;
@@
-46,7
+49,6
@@
public class TurnForAngle extends Command {
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
- this.driveTrain.resetGyro();
this.gyroController.setSetPoint(this.target);
}
this.gyroController.setSetPoint(this.target);
}
@@
-55,14
+57,14
@@
public class TurnForAngle extends Command {
double xVal = 0;
xVal = this.gyroController
double xVal = 0;
xVal = this.gyroController
- .calcPID(
this.driveTrain.getAngle() - this.driveTrain.getZeroAngle(
));
+ .calcPID(
Math.abs(this.driveTrain.getAngle() - this.zeroAngle
));
- double leftDrive;
- double rightDrive;
+ double leftDrive
= 0
;
+ double rightDrive
= 0
;
if (direction == Constants.Direction.RIGHT) {
leftDrive = xVal;
rightDrive = -xVal;
if (direction == Constants.Direction.RIGHT) {
leftDrive = xVal;
rightDrive = -xVal;
- } else {
+ } else
if (direction == Constants.Direction.LEFT)
{
leftDrive = -xVal;
rightDrive = xVal;
}
leftDrive = -xVal;
rightDrive = xVal;
}
diff --git
a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index eb9a4420a62024423e7a176f6d7bbbbaea6d4d95..1ba8c8c41847bbf86319ab576e4373476a6844e8 100644
(file)
--- a/
src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/
src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@
-11,9
+11,9
@@
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- public static double driveP = 0.00
8
, driveI = 0.001, driveD = -0.002;
- public static double defaultGyroP = 0.00
6, defaultGyroI = 0.00000
,
- defaultGyroD = -0.00
0
;
+ public static double driveP = 0.00
6
, driveI = 0.001, driveD = -0.002;
+ public static double defaultGyroP = 0.00
4, defaultGyroI = 0.0013
,
+ defaultGyroD = -0.00
5
;
private double gyroZero = 0;
public static final double WHEEL_DIAMETER = 6; // inches
private double gyroZero = 0;
public static final double WHEEL_DIAMETER = 6; // inches
@@
-125,7
+125,7
@@
public class DriveTrain extends Subsystem {
}
public void resetGyro() {
}
public void resetGyro() {
- this.
gyroZero = this.getAngle
();
+ this.
imu.reset
();
}
public double getZeroAngle() {
}
public double getZeroAngle() {
diff --git
a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
index b8ec465626547a08aafc13f3609b1b087d7a48fd..c6ab0fb7b14d632bd987714b890525c025273221 100644
(file)
--- a/
src/org/usfirst/frc/team3501/robot/utils/PIDController.java
+++ b/
src/org/usfirst/frc/team3501/robot/utils/PIDController.java
@@
-161,7
+161,6
@@
public class PIDController {
// close enough to target
if (currError <= this.doneRange) {
this.doneCycleCount++;
// close enough to target
if (currError <= this.doneRange) {
this.doneCycleCount++;
- System.out.println(doneCycleCount);
}
// not close enough to target
else {
}
// not close enough to target
else {