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):
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
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, 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
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
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 __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, angle):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.angle = angle
self.timer = Timer()
def __init__(self, robot):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.pMotionProfiler = PFLJustDriveForward(robot)
self.pTimer = Timer()
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()
def __init__(self):
self.armMotor = ctre.wpi_talonsrx.WPI_TalonSRX(armMotor)
self.armTimer = Timer()
self.bottomStopSwitch = wpilib.DigitalInput(bottomStop)
self.topStopSwitch = wpilib.DigitalInput(topStop)
self.bendSwitchOne = wpilib.DigitalInput(2)
self.bendSwitchTwo = wpilib.DigitalInput(3)
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 __init__(self):
super().__init__('MoveArmToInitialPosition')
self._arm = self.getRobot().arm
self.requires(self._arm)
self._logger = self.getRobot().logger
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._speed = -robotmap.ArmSpeed
self._timer = Timer()
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 __init__(self, beak_solenoid: DoubleSolenoid, diag_solenoid: DoubleSolenoid,
driver_station: DriverStation = None, timer: Timer = Timer(), cooldown: float = 0.5):
self.timer = timer
self.cooldown = cooldown
self.driver_station = driver_station
self.beak_solenoid = beak_solenoid
self.beak_state = False
self.beak_last = 0
self.diag_solenoid = diag_solenoid
self.diag_state = False
self.diag_last = 0
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 __init__(self, fnom, name=None):
super().__init__(name)
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.fnom = fnom
self.trajectory_points = read_trajectories(self.fnom)
#assert self.trajectory_points[0][0] == self.period
self.i = 0
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.target_heading = 0
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def robotInit(self):
self.chandler = XboxController(0)
self.meet = XboxController(1)
self.drive = drive.Drive() # So redundant omg
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer() # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
quickdebug.add_printables(self, ('match_time', Timer.getMatchTime))
quickdebug.init()
def __init__(self):
self.timer = Timer()
self.driver_station = DriverStation(controller=XboxController(0))
# self.gyroscope = AHRS.create_i2c()
# self.left_encoder = Encoder()
# self.right_encoder = Encoder()
self.drivetrain = TankDrivetrain(timer=self.timer, left_motors=[TalonSRX(10), TalonSRX(6)],
right_motors=[TalonSRX(12), TalonSRX(18)])
self.pneumatics = Pneumatics(compressor=Compressor(0), start_compressor=True, timer=self.timer)
self.beak = BeakMechanism(beak_solenoid=DoubleSolenoid(0, 4, 5), diag_solenoid=DoubleSolenoid(0, 0, 1),
driver_station=self.driver_station, timer=self.timer, cooldown=0.5)
# self.odometry = EncoderOdometry(left_encoder=self.left_encoder, right_encoder=self.right_encoder, gyro=self.gyroscope)
'''
def __init__(self,
compressor: Compressor,
closed_loop: bool = False,
vent_solenoid: Solenoid or None = None,
pressure_sensor: AnalogInput or None = None,
timer: Timer = Timer(),
start_compressor=True):
self.timer = timer
self.compressor = compressor
self.compressor.setClosedLoopControl(on=closed_loop)
self.vent_solenoid = vent_solenoid
self.pressure_sensor = pressure_sensor
self.pressure_observations = deque(maxlen=10)
self.last_interval = 0.0
self.obs_interval_sec = 1.0
if start_compressor:
self.start_compressor()
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
if not self.robot.isSimulation():
with open("/home/lvuser/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
else:
with open("/home/ubuntu/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
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
def robotInit(self):
"""
Init Robot
"""
# Robot Components
# Constructor params are PWM Ports on the RIO
self.drivetrain = drive.Drivetrain(self, 1,2,3,4)
self.intake = intake.Intake(0, self)
self.popper = popper.Popper(0,0)
self.front_lift = lift.Lift(0,1,2)
self.rear_lift = lift.Lift(0,5,4)
self.imu = imu.IMU(2)
self.encoders = encoders.Encoders()
CameraServer.launch("subsystems/camera.py:main")
self.oi = OI(self)
#self.drivecommand = DriveCommandGroup()
self.timer = Timer()
class Drive(Component):
_left_pwm = 0
_right_pwm = 0
# Cheesy Drive Stuff
quickstop_accumulator = 0
sensitivity = .9
old_wheel = 0
# Gyro & encoder stuff
gyro_timer = Timer()
driving_angle = False
driving_distance = False
gyro_goal = 0
gyro_tolerance = 1 # Degree
encoder_goal = 0
encoder_tolerance = 1 # Inch
def __init__(self):
super().__init__()
self.l_motor = SyncGroup(Talon, hardware.drive_left)
self.r_motor = SyncGroup(Talon, hardware.drive_right)
self._gyro_p = 0.12
self._gyro_d = 0.005
self._prev_gyro_error = 0
self.encoder_left_offset = 0
self.encoder_right_offset = 0
def stop(self):
"""Disables EVERYTHING. Only use in case of critical failure."""
self.l_motor.set(0)
self.r_motor.set(0)
def cheesy_drive(self, wheel, throttle, quickturn,
low_gear): # TODO replace low gear with slowdown analog
"""
Written partially by 254, ported to python & modified by 865.
:param wheel: The speed that the robot should turn in the X direction. 1 is right [-1.0..1.0]
:param throttle: The speed that the robot should drive in the Y direction. -1 is forward. [-1.0..1.0]
:param quickturn: If the robot should drive arcade-drive style
"""
if low_gear:
throttle *= 0.6 # sloow down
neg_inertia = wheel - self.old_wheel
self.old_wheel = wheel
if not low_gear:
wheel = util.sin_scale(wheel, 0.8, passes=2)
else:
wheel = util.sin_scale(wheel, 0.8, passes=3)
if not low_gear:
neg_inertia_scalar = 5
else:
if wheel * neg_inertia > 0:
neg_inertia_scalar = 2.5
else:
if abs(wheel) > .65:
neg_inertia_scalar = 5
else:
neg_inertia_scalar = 3
wheel += neg_inertia * neg_inertia_scalar
if quickturn:
if abs(throttle) < 0.2:
alpha = .1
self.quickstop_accumulator = (
1 -
alpha) * self.quickstop_accumulator + alpha * util.limit(
wheel, 1.0) * 5
over_power = 1
angular_power = wheel
else:
over_power = 0
angular_power = abs(
throttle
) * wheel * self.sensitivity - self.quickstop_accumulator
self.quickstop_accumulator = util.wrap_accumulator(
self.quickstop_accumulator)
right_pwm = left_pwm = throttle
left_pwm += angular_power
right_pwm -= angular_power
if left_pwm > 1:
right_pwm -= over_power * (left_pwm - 1)
left_pwm = 1
elif right_pwm > 1:
left_pwm -= over_power * (right_pwm - 1)
right_pwm = 1
elif left_pwm < -1:
right_pwm += over_power * (-1 - left_pwm)
left_pwm = -1
elif right_pwm < -1:
left_pwm += over_power * (-1 - right_pwm)
right_pwm = -1
self._left_pwm = left_pwm
self._right_pwm = right_pwm
def tank_drive(self, left, right):
# Applies a bit of exponential scaling to improve control at low speeds
self._left_pwm = math.copysign(math.pow(left, 2), left)
self._right_pwm = math.copysign(math.pow(right, 2), right)
# Stuff for encoder driving
def set_distance_goal(self, goal):
self.encoder_left_offset = hardware.drive_left_encoder.get()
self.encoder_right_offset = hardware.drive_right_encoder.get()
self.gyro_goal = hardware.gyro.getAngle()
self.encoder_goal = goal
self.driving_distance = True
self.driving_angle = False
def drive_distance(self): # TODO Make this work
l_error = util.limit(
self.encoder_goal - hardware.drive_left_encoder.getDistance(), 0.5)
r_error = util.limit(
self.encoder_goal - hardware.drive_right_encoder.getDistance(),
0.5)
l_speed = l_error + util.limit(self.gyro_error() * self._gyro_p * 0.5,
0.3)
r_speed = r_error - util.limit(self.gyro_error() * self._gyro_p * 0.5,
0.3)
self._left_pwm = util.limit(l_speed, 0.5)
self._right_pwm = util.limit(r_speed, 0.5)
def at_distance_goal(self):
l_error = self.encoder_goal + self.encoder_left_offset - hardware.drive_left_encoder.getDistance(
)
r_error = self.encoder_goal + self.encoder_right_offset - hardware.drive_right_encoder.getDistance(
)
return abs(l_error) < self.encoder_tolerance and abs(
r_error) < self.encoder_tolerance
# Stuff for Gyro driving
def set_angle_goal(self, goal):
self.gyro_timer.stop()
self.gyro_timer.reset()
self.gyro_goal = goal
self.driving_angle = True
self.driving_distance = False
def turn_angle(self):
error = self.gyro_error()
result = error * self._gyro_p + (
(error - self._prev_gyro_error) / 0.025) * self._gyro_d
self._left_pwm = result
self._right_pwm = -result
self._prev_gyro_error = error
def at_angle_goal(self):
on = abs(self.gyro_error) < self.gyro_tolerance
if on:
if not self.gyro_timer.running:
self.gyro_timer.start()
if self.gyro_timer.hasPeriodPassed(.3):
return True
return False
def gyro_error(self):
""" Returns gyro error wrapped from -180 to 180 """
raw_error = self.gyro_goal - hardware.gyro.getAngle()
wrapped_error = raw_error - 360 * round(raw_error / 360)
return wrapped_error
def update(self):
self.l_motor.set(self._left_pwm)
self.r_motor.set(-self._right_pwm)
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
