예제 #1
0
 public void setPositionGoal(double distance, double angle, double speed) {
   profile.setMaxVelocity(speed);
   profile.setTimeToMaxV(.2);
   straightController.setGoal(distance);
   straightController.enable();
   turnController.setGoal(angle);
   turnController.enable();
 }
예제 #2
0
 public void setSpeedGoal(double speed, double angle) {
   profile.setMaxVelocity(Math.abs(speed));
   profile.setTimeToMaxV(0.001);
   straightController.setGoal(speed < 0 ? -1000 : 1000);
   straightController.enable();
   turnController.setGoal(angle);
   turnController.enable();
 }
예제 #3
0
 public void setStraightProfile(MotionProfile profile) {
   straightController.setProfile(profile);
 }
예제 #4
0
 public void resetControllers() {
   straightController.setProfile(profile);
 }
예제 #5
0
 public boolean onTarget() {
   return straightController.onTarget() && turnController.onTarget();
 }
예제 #6
0
 public void setTurnGoal(double angle) {
   straightController.disable();
   lastStraight = 0;
   turnController.setGoal(angle);
   turnController.enable();
 }
예제 #7
0
 public void updatePositionGoal(double distance) {
   straightController.setGoalRaw(distance);
 }
예제 #8
0
 public void disableControllers() {
   straightController.disable();
   turnController.disable();
   setLeftRightPower(0, 0);
 }
예제 #9
0
 public void shift(boolean highGear) {
   isHighGear = highGear;
   shifter.set(!isHighGear);
   straightController.setGains(highGear ? highStraightGains : lowStraightGains);
   turnController.setGains(highGear ? highTurnGains : lowTurnGains);
 }