def __init__(self, angle):
self.sd = NetworkTables.getTable("SmartDashboard")
# PID constants
# kp = self.sd.getNumber("rod/kp")
# ki = self.sd.getNumber("rod/ki")
# kd = self.sd.getNumber("rod/kd")
# kf = self.sd.getNumber("rod/kf")
kp = 0.006
ki = 0.0001
kd = 0.02
kf = 0
ktolerance = 1.0 # tolerance of 1.0 degree
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf,
"Rotate to angle {}".format(angle))
self.requires(subsystems.motors)
self.initial_angle = None
self.rate = 1.0
turn_controller = self.getPIDController()
turn_controller.setInputRange(-180, 180)
turn_controller.setOutputRange(-1.0, 1.0)
turn_controller.setAbsoluteTolerance(ktolerance)
turn_controller.setContinuous(True)
turn_controller.setSetpoint(angle)
self.logger = logging.getLogger('robot')
self.drive = RectifiedDrive(30, 0.05)
def __init__(self, timeout=20, power=0.12):
self.sd = NetworkTables.getTable("SmartDashboard")
# NetworkTables variables for tuning
kp = self.sd.getNumber("rod/kp")
ki = self.sd.getNumber("rod/ki")
kd = self.sd.getNumber("rod/kd")
kf = self.sd.getNumber("rod/kf")
ktolerance = self.sd.getNumber("rod/ktolerance")
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf, "Drive To Rod")
self.requires(subsystems.motors)
turnController = self.getPIDController()
turnController.setInputRange(-1.0, 1.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(ktolerance)
turnController.setContinuous(True)
turnController.setSetpoint(0.5) # want rod to be at center
self.drive = RectifiedDrive(30, 0.05)
self.timeout = timeout
self.logger = logging.getLogger("robot")
self.is_autonomous = self.sd.getBoolean("isautonomous")
self.is_lost = False # can't find the rod
self.power = power
self.last_output = 0 # for if the rod is lost
class Rotate(PIDCommand):
"""
This command uses the NavX to rotate to the specified angle.
"""
def __init__(self, angle):
self.sd = NetworkTables.getTable("SmartDashboard")
# PID constants
# kp = self.sd.getNumber("rod/kp")
# ki = self.sd.getNumber("rod/ki")
# kd = self.sd.getNumber("rod/kd")
# kf = self.sd.getNumber("rod/kf")
kp = 0.006
ki = 0.0001
kd = 0.02
kf = 0
ktolerance = 1.0 # tolerance of 1.0 degree
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf,
"Rotate to angle {}".format(angle))
self.requires(subsystems.motors)
self.initial_angle = None
self.rate = 1.0
turn_controller = self.getPIDController()
turn_controller.setInputRange(-180, 180)
turn_controller.setOutputRange(-1.0, 1.0)
turn_controller.setAbsoluteTolerance(ktolerance)
turn_controller.setContinuous(True)
turn_controller.setSetpoint(angle)
self.logger = logging.getLogger('robot')
self.drive = RectifiedDrive(30, 0.05)
def initialize(self):
self.initial_angle = navx.ahrs.getAngle()
def returnPIDInput(self):
return navx.ahrs.getAngle() - self.initial_angle
def usePIDOutput(self, output):
self.rate = output
# subsystems.motors.robot_drive.setLeftRightMotorOutputs(output, -output)
self.drive.rectified_drive(0, output)
def isFinished(self):
# stop command if rate set to less than 0.02 or if it has been 2.5 seconds
if abs(self.rate) < 0.02 or self.timeSinceInitialized() > 2.5:
self.logger.info("Done rotating {}.".format(self.rate))
return True
return False
def end(self):
# set outputs to 0 on end
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
def __init__(self, power, timeoutInSeconds):
super().__init__('Set Speed %d' % power, timeoutInSeconds)
self.requires(subsystems.motors)
self.power = power
self.drive = RectifiedDrive(0, 0.05)
self.timer = wpilib.Timer()
def __init__(self):
super().__init__('Follow Joystick')
self.requires(subsystems.motors)
self.logger = logging.getLogger("robot")
self.drive = RectifiedDrive(25, 0.05, squared_inputs=True)
self.sd = NetworkTables.getTable("SmartDashboard")
self.timer = Timer()
self.timer.start()
def __init__(self, dist):
super().__init__('Driving forward %d inches' % dist)
self.requires(subsystems.motors)
self.drive = RectifiedDrive(0, 0.05)
self.timer = wpilib.Timer()
self.desired_dist = dist * 1024 / 18.85 # convert from inches to edges
self.last_position = None
self.dist_traveled = 0 # in edges
self.logger = logging.getLogger("robot")
class DriveForward(Command):
"""
Drives forward the given distance in inches.
"""
def __init__(self, dist):
super().__init__('Driving forward %d inches' % dist)
self.requires(subsystems.motors)
self.drive = RectifiedDrive(0, 0.05)
self.timer = wpilib.Timer()
self.desired_dist = dist * 1024 / 18.85 # convert from inches to edges
self.last_position = None
self.dist_traveled = 0 # in edges
self.logger = logging.getLogger("robot")
def initialize(self):
self.timer.start()
self.last_position = subsystems.motors.left_motor.getPosition()
def execute(self):
self.drive.rectified_drive(0.1, 0)
self.timer.delay(0.05)
def isFinished(self):
# timeout after 10 seconds
if self.timeSinceInitialized() > 3:
return True
position = subsystems.motors.left_motor.getPosition()
self.logger.info("Encoder position: {}".format(position))
if position < self.last_position:
self.dist_traveled += 1024 - self.last_position + position
else:
self.dist_traveled += position - self.last_position
if self.dist_traveled >= self.desired_dist:
return True
self.last_position = position
return False
def end(self):
# set outputs to 0 on end
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
class FollowJoystick(Command):
"""
This command will read the joysticks' y-axes and uses tank drive.
"""
def __init__(self):
super().__init__('Follow Joystick')
self.requires(subsystems.motors)
self.logger = logging.getLogger("robot")
self.drive = RectifiedDrive(25, 0.05, squared_inputs=True)
self.sd = NetworkTables.getTable("SmartDashboard")
self.timer = Timer()
self.timer.start()
def execute(self):
if self.timer.hasPeriodPassed(0.05): # period of 0.05 seconds
# tank drive
# subsystems.motors.robot_drive.tankDrive(oi.joystick, robotmap.joystick.left_port, oi.joystick,
# robotmap.joystick.right_port, True)
# arcade drive
# subsystems.motors.robot_drive.arcadeDrive(oi.joystick)
# rectified arcade drive
power = -oi.joystick.getRawAxis(
robotmap.joystick.forwardAxis) * 0.75
angular_vel = oi.joystick.getRawAxis(
robotmap.joystick.steeringAxis)
if self.sd.containsKey("slowmode") and self.sd.getBoolean(
"slowmode"):
power *= 0.75
angular_vel *= 0.75
if abs(angular_vel) < 0.1: # tolerance
angular_vel = 0
self.drive.rectified_drive(power, angular_vel)
def end(self):
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
def __init__(self, timeout=20):
self.sd = NetworkTables.getTable("SmartDashboard")
self.sd.putBoolean("lockonRunning", True)
# PID constants
# kp = 0.01
# ki = 0.005
# kd = 0.002
# kf = 0.0
# ktolerance = 0.02
# NetworkTables variables for tuning
kp = self.sd.getNumber("rod/kp")
ki = self.sd.getNumber("rod/ki")
kd = self.sd.getNumber("rod/kd")
kf = self.sd.getNumber("rod/kf")
ktolerance = self.sd.getNumber("rod/ktolerance")
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf, "Lock On")
self.requires(subsystems.motors)
turnController = self.getPIDController()
turnController.setInputRange(-1.0, 1.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(ktolerance)
turnController.setContinuous(True)
turnController.setSetpoint(0.5) # want rod to be at center
self.timeout = timeout
self.drive = RectifiedDrive(30, 0.05)
self.logger = logging.getLogger("robot")
self.is_lost = False # can't find the rod
self.last_rod_pos = 0
class SetSpeed(TimedCommand):
"""
Spins the motors at the given power for a given number of seconds and then stops.
"""
def __init__(self, power, timeoutInSeconds):
super().__init__('Set Speed %d' % power, timeoutInSeconds)
self.requires(subsystems.motors)
self.power = power
self.drive = RectifiedDrive(0, 0.05)
self.timer = wpilib.Timer()
def initialize(self):
self.timer.start()
def execute(self):
self.drive.rectified_drive(self.power, 0)
self.timer.delay(0.05)
def end(self):
# set outputs to 0 on end
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
class LockOn(PIDCommand):
"""
This command dynamically transfers control between the driver and computer vision for driving to the rod
"""
def __init__(self, timeout=20):
self.sd = NetworkTables.getTable("SmartDashboard")
self.sd.putBoolean("lockonRunning", True)
# PID constants
# kp = 0.01
# ki = 0.005
# kd = 0.002
# kf = 0.0
# ktolerance = 0.02
# NetworkTables variables for tuning
kp = self.sd.getNumber("rod/kp")
ki = self.sd.getNumber("rod/ki")
kd = self.sd.getNumber("rod/kd")
kf = self.sd.getNumber("rod/kf")
ktolerance = self.sd.getNumber("rod/ktolerance")
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf, "Lock On")
self.requires(subsystems.motors)
turnController = self.getPIDController()
turnController.setInputRange(-1.0, 1.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(ktolerance)
turnController.setContinuous(True)
turnController.setSetpoint(0.5) # want rod to be at center
self.timeout = timeout
self.drive = RectifiedDrive(30, 0.05)
self.logger = logging.getLogger("robot")
self.is_lost = False # can't find the rod
self.last_rod_pos = 0
def returnPIDInput(self):
rod_pos = camera.get_rod_pos()
if rod_pos is None:
self.logger.critical("Couldn't find the rod!")
self.is_lost = True
self.sd.putBoolean("lockonRunning", False)
RumbleController(0.5).start()
return 0.5
else:
self.last_rod_pos = rod_pos[0]
self.sd.putNumber("rod/actual", rod_pos[0])
return rod_pos[0]
def usePIDOutput(self, output):
power = -oi.joystick.getRawAxis(
robotmap.joystick.forwardAxis
) # get human command for steering and speed
angular_vel = oi.joystick.getRawAxis(robotmap.joystick.steeringAxis)
if self.is_lost or abs(
angular_vel) >= 0.5: # if we're lost or the human is insistent
self.drive.rectified_drive(power, angular_vel)
else: # combine human and computer vision control
self.drive.rectified_drive(
power, -output * abs(0.5 - self.last_rod_pos) +
(1.0 - abs(0.5 - self.last_rod_pos)) * angular_vel)
def isFinished(self):
# timeout
if self.timeSinceInitialized() > self.timeout:
return True
return False
class DriveToRod(PIDCommand):
"""
This command will find the rod and drive the robot towards it.
"""
def __init__(self, timeout=20, power=0.12):
self.sd = NetworkTables.getTable("SmartDashboard")
# NetworkTables variables for tuning
kp = self.sd.getNumber("rod/kp")
ki = self.sd.getNumber("rod/ki")
kd = self.sd.getNumber("rod/kd")
kf = self.sd.getNumber("rod/kf")
ktolerance = self.sd.getNumber("rod/ktolerance")
# initialize PID controller with a period of 0.05 seconds
super().__init__(kp, ki, kd, 0.05, kf, "Drive To Rod")
self.requires(subsystems.motors)
turnController = self.getPIDController()
turnController.setInputRange(-1.0, 1.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(ktolerance)
turnController.setContinuous(True)
turnController.setSetpoint(0.5) # want rod to be at center
self.drive = RectifiedDrive(30, 0.05)
self.timeout = timeout
self.logger = logging.getLogger("robot")
self.is_autonomous = self.sd.getBoolean("isautonomous")
self.is_lost = False # can't find the rod
self.power = power
self.last_output = 0 # for if the rod is lost
def returnPIDInput(self):
if oi.controller.getBumper(
GenericHID.Hand.kLeft): # return control back to controller
FollowJoystick().start()
return 0.5
rod_pos = camera.get_rod_pos()
if rod_pos is None:
self.logger.critical("Couldn't find the rod!")
self.is_lost = True
if not self.is_autonomous: # return control to controller if not in autonomous
self.logger.critical("Returning control to the controller!")
FollowJoystick().start()
RumbleController(0.5).start()
return 0.5
else:
self.sd.putNumber("rod/actual", rod_pos[0])
return rod_pos[0]
def usePIDOutput(self, output):
if self.is_lost: # if lost, slowly spin in circle
if self.last_output > 0: # keep turning right
subsystems.motors.robot_drive.setLeftRightMotorOutputs(
0.1, -0.1)
else: # keep turning left
subsystems.motors.robot_drive.setLeftRightMotorOutputs(
-0.1, 0.1)
else:
self.drive.rectified_drive(self.power, -output)
self.last_output = output
def isFinished(self):
# timeout
if self.timeSinceInitialized() > self.timeout:
return True
# stop if the rod is hitting the switch
if not switches.gear_mech_switch.get():
self.logger.info("switch pressed")
ControlGearMech(False).start()
return True
return False
def end(self):
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)