class AcquireCube(Command):
def __init__(self, drive_speed, timeout, drive: Drivetrain,
intake: Intake):
super().__init__("AcquireCube", timeout)
self.drive_speed = drive_speed
self.drive = drive
self.intake = intake
self.requires(intake)
self.requires(drive)
self.state = 0
self.timer = Timer()
def initialize(self):
self.state = 0
self.intake.set_arm_state(ArmState.DOWN)
self.intake.set_grab_state(GrabState.OUT)
self.timer.start()
def execute(self):
if self.state == 0:
self.drive.arcade_drive(0, 0)
if self.timer.get() > 0.2:
self.state = 1
elif self.state == 1:
self.intake.intake()
self.drive.arcade_drive(self.drive_speed, 0)
def isFinished(self):
return self.intake.has_acquired_cube() or self.isTimedOut()
def end(self):
self.intake.set_grab_state(GrabState.IN)
self.intake.set_arm_state(ArmState.UP)
self.intake.run_intake(0)
self.drive.arcade_drive(0, 0)
def __init__(self, port):
spi = SPI(port)
spi.setClockRate(4000000) # 4 MHz (rRIO max, gyro can go high)
spi.setClockActiveHigh()
spi.setChipSelectActiveLow()
spi.setMSBFirst()
self._spi = spi
self._command = [0x00] * DATA_SIZE
self._data = [0x00] * DATA_SIZE
self._command[0] = READ_COMMAND
self._accumulated_angle = 0.0
self._current_rate = 0.0
self._accumulated_offset = 0.0
self._rate_offset = 0.0
self._last_time = 0
self._current_time = 0
self._calibration_timer = Timer()
self._calibration_timer.start()
self._update_timer = Timer()
self._update_timer.start()
#
# self._update_thread = Thread(self.update, daemon=True)
# self._update_thread.start()
self.update()
def __init__(self, intake: Intake, new_state: ArmState):
super().__init__("MoveIntakeCommand")
self.intake = intake
self.new_state = new_state
self.old_state = None
self.timer = Timer()
self.requires(intake)
def robotInit(self):
print('Starting gyro init...')
self.myGyro = ADIS16448_IMU(ADIS16448_IMU.IMUAxis.kZ, SPI.Port.kMXP, 4)
print('Gyro init completed.')
self.statusTimer = Timer()
self.statusTimer.start()
class MoveElevatorToInitialPosition(Command):
def __init__(self):
super().__init__('MoveElevatorToInitialPosition')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = -robotmap.ElevatorSpeed #Negated
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._timer = Timer()
def initialize(self):
self._elevator.stop()
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
self._timer.reset()
self._timer.start()
def execute(self):
self._elevator.move(self._speed)
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
def isFinished(self):
if self._timer.get() > 1.0:
return True
else:
return False
def interrupted(self):
self.end()
def end(self):
self._elevator.stop()
class MoveLiftOneBoxHeight(Command):
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.total_timer = Timer()
self.direction = d
# Called just before this Command runs the first time
def initialize(self):
self.total_timer.reset()
self.total_timer.start()
# Called repeatedly when this Command is scheduled to run
def execute(self):
setpoint = self.robot.lift.getCurrentSetpoint()
if self.direction == Direction.UP:
setpoint = setpoint + robotmap.BOX_HEIGHT_INCHES
elif self.direction == Direction.DOWN:
setpoint = setpoint - robotmap.BOX_HEIGHT_INCHES
self.robot.lift.setMotionMagicSetpoint(setpoint)
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
return True
def __init__(self, robot, angle):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.angle = angle
self.timer = Timer()
def __init__(self, delay_period):
""" :param delay_period: The amount of time to do a delay """
self.timer = Timer()
self.delay_period = delay_period
self.timer.start()
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.m_setpoint = d #inches
self.total_timer = Timer()
def __init__(self):
self.trajectories: trajectories.Trajectories = trajectories.Trajectories(
)
self.ramsete = RamseteController(self.BETA, self.ZETA)
self.desired_trajectory: Trajectory = None
self.timer = Timer()
self.is_tracking = False
class AutoSetLiftPosition(Command):
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.m_setpoint = d #inches
self.total_timer = Timer()
# Called just before this Command runs the first time
def initialize(self):
self.total_timer.reset()
self.total_timer.start()
self.robot.lift.setMotionMagicSetpoint(self.m_setpoint)
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
returnval = False
if self.total_timer.get() > 1.0:
if self.robot.lift.isMotionMagicNearTarget():
returnval = True
elif self.total_timer > 9.0:
returnval = True
return returnval
# Called when another command which requires one or more of the same
# subsystems is scheduled to run
def interrupted(self):
self.robot.lift.hold()
self.robot.debug.Stop()
def __init__ (self, intake: Intake):
super().__init__ ()
self.requires(intake)
self.intake = intake
self.last_arm_state = ArmState.UP
self.arm_timer = Timer()
self.arm_timer_latch = False
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.total_timer = Timer()
self.direction = d
class PreciseDelay(object):
"""
Used to synchronize a timing loop.
Usage:
delay = PreciseDelay(time_to_delay)
while something:
# do things here
delay.wait()
"""
def __init__(self, delay_period):
""" :param delay_period: The amount of time to do a delay """
self.timer = Timer()
self.delay_period = delay_period
self.timer.start()
def wait(self):
""" Waits until the delay period has passed """
# we must *always* yield here, so other things can run
Timer.delay(0.001)
while not self.timer.hasPeriodPassed(self.delay_period):
Timer.delay(0.001)
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
self.target_chooser = sendablechooser.SendableChooser()
self.target_chooser.setDefaultOption(TargetHeight.LOW.name,
TargetHeight.LOW)
self.target_chooser.addOption(TargetHeight.MIDDLE.name,
TargetHeight.MIDDLE)
self.target_chooser.addOption(TargetHeight.HIGH.name,
TargetHeight.HIGH)
self.left_right_chooser = sendablechooser.SendableChooser()
self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name,
RightLeft.RIGHT)
self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT)
self.hab_level_chooser = sendablechooser.SendableChooser()
self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name,
HabLevel.LEVEL1)
self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2)
SmartDashboard.putData("TargetChooser", self.target_chooser)
SmartDashboard.putData("LeftRightChooser", self.left_right_chooser)
SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser)
self.chosen_target = TargetHeight.LOW
self.left_right = RightLeft.RIGHT
self.hab_level = HabLevel.LEVEL1
class Auto3():
CLAW_DEPLOY = 1
DRIVE_OFF_PLATFORM = 2
LIFT_DEPLOY = 3
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
def init(self):
self.logger.info("Initializing auto 3")
self.state = 0
self.timer.start()
def iterate(self):
if self.state == 0:
self.state = 1
elif self.state == 1:
self.state = 2
elif self.state == 2:
self.state = 3
else:
pass
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count + 1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def __init__(self, robot):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.pMotionProfiler = PFLJustDriveForward(robot)
self.pTimer = Timer()
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count+1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def move_arm_to_30(self):
'''while(self.arm.getPOT() <= 30):
self.arm.armAuto(1,0,30,rate=0.7)
self.arm.armAuto(0,0,30)
'''
Timer.delay(3)
self.next_state('drive_forward_step_3')
def move_arm_to_0(self):
'''while(self.arm.getPOT() >= 0.5):
self.arm.armAuto(0,1,0,rate=0.5)
self.arm.armAuto(0,0,0.5)
'''
Timer.delay(3)
self.next_state('drive_forward_step_2')
def timePassed(self, seconds):
if self._timer <= 0:
self._timer = Timer.getFPGATimestamp()
if Timer.getFPGATimestamp() - self._timer >= seconds:
self._timer = -1
return True
else:
return False
def __init__(self):
super().__init__('MoveElevatorToInitialPosition')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = -robotmap.ElevatorSpeed #Negated
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._timer = Timer()
def wait(self):
""" Waits until the delay period has passed """
# we must *always* yield here, so other things can run
Timer.delay(0.001)
while not self.timer.hasPeriodPassed(self.delay_period):
Timer.delay(0.001)
def __init__(self, position):
super().__init__('MoveElevatorDown')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = robotmap.ElevatorSpeed
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._targetPosition = position
self._timer = Timer()
def __init__(self, drive_speed, timeout, drive: Drivetrain,
intake: Intake):
super().__init__("AcquireCube", timeout)
self.drive_speed = drive_speed
self.drive = drive
self.intake = intake
self.requires(intake)
self.requires(drive)
self.state = 0
self.timer = 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 get(self, input):
if input == self.last_input:
time = Timer.getFPGATimestamp()
if time - self.last_timestamp <= self.debounce_period:
return -1
else:
self.last_timestamp = time
return input
else:
self.last_input = input
self.last_timestamp = Timer.getFPGATimestamp()
return input
def get(self, input):
if input == self.last_input:
time = Timer.getFPGATimestamp()
if time - self.last_timestamp <= self.debounce_period:
return -1
else:
self.last_timestamp = time
return input
else:
self.last_input = input
self.last_timestamp = Timer.getFPGATimestamp()
return input
def __init__(self):
super().__init__()
self._l_motor = Talon(hardware.intake_left)
self._r_motor = Talon(hardware.intake_right)
self._intake_piston = Solenoid(hardware.intake_solenoid)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
self._left_intake_amp = CircularBuffer(25)
self._right_intake_amp = CircularBuffer(25)
self._pulse_timer = Timer()
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp(), 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **",
flush=True)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **"
)
# note I made this a bit more complicated to demonstrate the flexibility of the button approach
# two button approach
if self.command == 'stop':
self.robot.shooter.stop_flywheel()
self.shooter_enabled = False
elif self.command == 'spin':
self.robot.shooter.set_flywheel(velocity=self.velocity)
self.shooter_enabled = True
else:
# toggle button approach
if self.shooter_enabled:
self.robot.shooter.stop_flywheel()
self.shooter_enabled = False
else:
self.robot.shooter.set_flywheel(velocity=self.velocity)
self.shooter_enabled = True
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
if self.source == 'dashboard':
self.setpoint = SmartDashboard.getNumber('z_angle', 1)
# may want to break if no valid setpoint is passed
self.start_angle = self.robot.drivetrain.navx.getAngle()
if self.absolute: # trust the navx to be correctly oriented to the field
self.setpoint = self.setpoint - self.start_angle
if self.setpoint > 180:
self.setpoint = -(360 - self.setpoint)
if self.setpoint < -180:
self.setpoint = (360 + self.setpoint)
self.error = 0
self.prev_error = 0
def get_output(self, current_input: float, setpoint: float) -> float:
'''Get PID output for process
current_input:
The current PID input
setpoint:
Desired output of process/input to PID
'''
# Current time in seconds
current_time = Timer.getFPGATimestamp()
# Time elapsed since last update
time_change = current_time - self._previous_time
# The current error
current_error = self._get_continuous_error(setpoint - current_input)
self._integral_term += self._coefs.i * (current_error * time_change)
self._integral_term = clamp(self._integral_term, self._output_max,
self._output_min)
# Protect againsts ZeroDivisionError caused
# by time resolution in simulator
if time_change <= 0.0:
time_change = 0.005
derivative = (current_input - self._previous_input) / time_change
self._previous_input = current_input
self._previous_time = current_time
output = ((self._coefs.p * current_error) + self._integral_term +
(self._coefs.d * derivative))
return clamp(output, self._output_max, self._output_min)
def searching(self) -> None:
"""
The vision system does not have a target, we try to find one using odometry
"""
if self.is_manual_aiming:
self.next_state_now("manual_aiming")
if self.target_estimator.is_ready():
# print(f"searching -> tracking {self.vision.get_vision_data()}")
self.time_target_lost = None
self.next_state("tracking")
else:
# Scan starting straight downrange.
time_now = Timer.getFPGATimestamp()
# Start a scan only if it's been a minimum time since we lost the target
if (self.time_target_lost is None or
(time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN):
# Start a scan only if it's been a minimum time since we started
# a scan. This allows the given scan to grow enough to find the
# target so that we don't just start the scan over every cycle.
if (self.time_of_last_scan is None
or (time_now - self.time_of_last_scan) >
self.MIN_SCAN_PERIOD):
self.turret.scan(-self.chassis.get_heading())
self.time_of_last_scan = time_now
def stop( self ):
current = Timer.getMsClock( )
self.offset = current - self.offset
self.time = self.offset
self.running = False
def start( self ):
if self.running:
return
self.offset = Timer.getMsClock( )
self.running = True
def restart( self ):
self.offset = Timer.getMsClock( )
self.time = 0.0
self.running = True
def __init__(self):
self.debounce_period = 0.5
self.last_input = -1
self.last_timestamp = Timer.getFPGATimestamp()
def time(self):
return Timer.getFPGATimestamp()
def getTimeMS( self ):
current = Timer.getMsClock( )
current -= self.offset
current += self.time
return float( current )
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
class driveTrain(Component) :
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
# Adding components to the LiveWindow (testing)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor)
'''
def log(self):
#The log method puts interesting information to the SmartDashboard. (like velocity information)
'''
#no longer implemented because of change of hardware
wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity())
'''
wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition())
wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition())
wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition())
wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition())
'''
wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57))
'''
# drive forward function
def drive_forward(self, speed) :
self.drive.tankDrive(speed, speed, True)
# manual drive function for Tank Drive
def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT):
#self.lfmotor.setCloseLoopRampRate(1)
#self.lbmotor.setCloseLoopRampRate(1)
#self.rfmotor.setCloseLoopRampRate(1)
#self.rbmotor.setCloseLoopRampRate(1)
if (leftB == True): #Straight Button
rightSpeed = leftSpeed
if (rightB == True): #Slow Button
#leftSpeed = leftSpeed/1.75
#rightSpeed = rightSpeed/1.75
if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning
leftSpeed = leftSpeed/1.75
rightSpeed = rightSpeed/1.75
# Fast button
if(rightT == True):
#self.lfmotor.setCloseLoopRampRate(24)
#self.lbmotor.setCloseLoopRampRate(24)
#self.rfmotor.setCloseLoopRampRate(24)
#self.rbmotor.setCloseLoopRampRate(24)
leftSpeed = leftSpeed*(1.75)
rightSpeed = rightSpeed*(1.75)
if(leftT == True):
leftSpeed = 0.1
rightSpeed = 0.1
# Creating margin for error when using the joysticks, as they're quite sensitive
if abs(rightSpeed) < 0.04 :
rightSpeed = 0
if abs(leftSpeed) < 0.04 :
leftSpeed = 0
if self.CONTROL_TYPE:
self.pidRightFront.setSetpoint(rightSpeed)
self.pidRightBack.setSetpoint(rightSpeed)
self.pidLeftFront.setSetpoint(leftSpeed)
self.pidLeftBack.setSetpoint(leftSpeed)
else:
self.lfmotor.set(leftSpeed*512)
self.rfmotor.set(rightSpeed*512)
self.lbmotor.set(leftSpeed*512)
self.rbmotor.set(rightSpeed*512)
#autononmous tank drive (to remove a need for a slow, striaght, or fast button)
def autonTankDrive(self, leftSpeed, rightSpeed):
self.log()
#self.drive.tankDrive(leftSpeed, rightSpeed, True)
self.rfmotor.set(rightSpeed)
self.rbmotor.set(rightSpeed*(-1))
self.lfmotor.set(leftSpeed)
self.lbmotor.set(leftSpeed*(-1))
# stop function
def drive_stop(self) :
self.drive.tankDrive(0,0)
# fucntion to reset the PID's and encoder values
def reset(self):
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
if self.CONTROL_TYPE:
self.LEFTFRONTCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE= 0
self.RIGHTBACKCUMULATIVE = 0
self.pidLeftBack.setSetpoint(0)
self.pidLeftFront.setSetpoint(0)
self.pidRightBack.setSetpoint(0)
self.pidRightFront.setSetpoint(0)
# def getDistance(self)
# return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE)))
def turn_angle(self, degrees):
desired_inches = self.INCHES_PER_DEGREE * degrees
if degrees < 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(0.4, -0.4)
elif degrees > 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(-0.4, 0.4)
# Enable PID Controllers
def enablePIDs(self):
'''
#No longer required because we swapped from analog encoders to magnetic encoders
self.pidLeftFront.enable()
self.pidLeftBack.enable()
self.pidRightFront.enable()
self.pidRightBack.enable()
'''
# Disable PID Controllers
def disablePIDs(self):
'''
#see explaination above
self.pidLeftFront.disable()
self.pidLeftBack.disable()
self.pidRightFront.disable()
self.pidRightBack.disable()
'''
def getAutonDistance(self):
return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4
#detirmines how many ticks the encoder has processed
def getMotorDistance(self, motor, cumulativeDistance):
currentRollovers = 0 #number of times the encoder has gone from 1023 to 0
previousValue = cumulativeDistance #variable for comparison
currentValue = motor.getEncPosition() #variable for comparison
if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0
currentRollovers += 1 #notes the rollover
return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks
#converts ticks from getMotorDistance into inches
def convertEncoderRaw(self, selectedEncoderValue):
return selectedEncoderValue * self.INCHES_PER_REV
