Exemplo n.º 1
0
 public void autoDrive() {
   double speed = 0;
   if (distPID.isEnable()) {
     speed = distPID.get();
   }
   double turn = 0;
   if (turnPID.isEnable()) {
     turn = turnPID.get();
   }
   if (speed > 0) {
     drive.drive(speed, turn);
   } else {
     if (turn > 0) {
       // turn right
       tankDrive(turn, -turn);
     } else if (turn < 0) {
       tankDrive(-turn, turn);
     }
   }
   SmartDashboard.putDouble("left pwm", leftJag.get());
   SmartDashboard.putDouble("right pwm", rightJag.get());
   SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
   SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
   SmartDashboard.putDouble("turn error", turnPID.getError());
   // System.out.println("driving " + speed + " " + turn);
 }
Exemplo n.º 2
0
 public boolean isTurnDone() {
   System.out.println(
       "is turn done "
           + turnPID.onTarget()
           + " deg="
           + RobotMap.roundtoTwo(gyro.getAngle())
           + " out="
           + RobotMap.roundtoTwo(turnPID.get())
           + " err="
           + RobotMap.roundtoTwo(turnPID.getError()));
   return turnPID.onTarget();
 }