class ShootLeftSideGearPlace(RightSideGearPlace):
'Place robot 15in from string 90deg to string'
MODE_NAME = 'Shoot left side gear place'
DEFAULT = False
DIRECTION = -1
drive_back_distance = tunable(-4)
at_tower_angle = tunable(40)
shooter = shooter.Shooter
x_ctrl = XPosController
@state
def rotate_back(self):
"""
IF MODIFIED: Function in RightGearPlace must be modified too"""
self.angle_ctrl.align_to(self.at_tower_angle)
if self.angle_ctrl.is_aligned():
self.drive.set_raw_rcw(0.0)
self.next_state('sit_and_shoot')
self.shooter.shoot()
@timed_state(duration=8, next_state='finish')
def sit_and_shoot(self):
self.shooter.shoot()
class DriveStraightWithGyroTest(VictisAuto):
MODE_NAME = 'Drive_Straight_With_Gyro_Test'
DEFAULT = False
drive = swervedrive.SwerveDrive
fc_y_ctrl = FCYPosController
fc_x_ctrl = FCXPosController
moving_angle_ctrl = MovingAngleController
fc_tracker = FCPositionTracker
drive_distance_feet = tunable(-5) # Feet
align_to = tunable(0) # Degrees
@timed_state(duration=10.0, first=True, next_state='failed_distance')
def drive_distance(self, initial_call):
if initial_call:
self.drive.field_centric = True
self.moving_angle_ctrl.reset_angle()
self.fc_tracker.enable()
self.moving_angle_ctrl.align_to(self.align_to)
self.fc_x_ctrl.move_to(self.drive_distance_feet)
if self.fc_x_ctrl.is_at_location():
self.next_state('finish')
@state
def failed_distance(self):
self.next_state('finish')
@state
def finish(self):
self.fc_tracker.disable()
self.done()
class ChevalDeFrise(StatefulAutonomous):
DEFAULT = False
"""This autonomous utilizes the ultrasonic sensor mounted on the front
of the robot to tell when we are ready to lower the arms"""
intake = intake.Arm
drive = drive.Drive
ultrasonic = wpilib.AnalogInput
targetDistance = tunable(.13)
driveOnDistance = tunable(1)
driveOffDistance = tunable(4)
def drive_to_cheval(self):
"""Drives forward toward the cheval"""
self.drive.move(.4, 0)
if self.ultrasonic.getVoltage() < self.targetDistance:
self.next_state('lower_arms')
@state
def lower_arms(self, initial_call):
"""Lowers arms onto cheval"""
if initial_call:
self.intake.set_arm_bottom()
if self.intake.on_target():
self.next_state('drive_on')
@state
def drive_on(self, initial_call):
"""Drives forward onto the cheval"""
if initial_call:
self.drive.reset_drive_encoders()
if self.drive.drive_distance(self.driveOnDistance):
self.next_state('raise_arms')
@state
def raise_arms(self, initial_call):
"""Raises arms to protect them when coming down"""
self.intake.set_arm_top()
self.next_state('drive_off')
@state
def drive_off(self, initial_call):
"""Drives off cheval"""
if initial_call:
self.drive.reset_drive_encoders()
if self.drive.drive_distance(self.driveOffDistance * 12):
self.next_state('transition')
class DriveLeftTest(VictisAuto):
MODE_NAME = 'Drive_Left_Test'
DEFAULT = False
drive = swervedrive.SwerveDrive
x_ctrl = XPosController
drive_distance_feet = tunable(-5)
@timed_state(duration=10.0, first=True, next_state='failed_distance')
def drive_distance(self, initial_call):
if initial_call:
self.drive.field_centric = False
self.sd = NetworkTable.getTable('SmartDashboard')
self.drive.enable_position_prediction()
self.x_ctrl.move_to(self.drive_distance_feet)
if self.x_ctrl.is_at_location():
self.next_state('finish')
@state
def failed_distance(self):
self.next_state('finish')
@state
def finish(self):
self.drive.disable_position_prediction()
self.done()
class GyroTest(VictisAuto):
MODE_NAME = 'Gyro_Test'
DEFAULT = False
drive = swervedrive.SwerveDrive
angle_ctrl = AngleController
align_to = tunable(-30) # Degrees
@timed_state(duration=10.0, first=True, next_state='failed_align')
def align(self, initial_call):
if initial_call:
self.angle_ctrl.reset_angle()
self.drive.allow_reverse = True
self.drive.threshold_input_vectors = False
self.angle_ctrl.align_to(self.align_to)
if self.angle_ctrl.is_aligned():
self.next_state('finish')
@state
def failed_align(self):
self.next_state('finish')
@state
def finish(self):
self.drive.set_raw_rcw(0.0)
self.drive.wait_for_align = False
self.drive.threshold_input_vectors = True
self.drive.disable_position_prediction()
self.done()
class DriveStraightTest(VictisAuto):
MODE_NAME = 'Drive_Straight_Test'
DEFAULT = False
drive = swervedrive.SwerveDrive
tracker = PositionTracker
y_ctrl = YPosController
drive_distance_feet = tunable(5)
@timed_state(duration=10.0, first=True, next_state='failed_distance')
def drive_distance(self, initial_call):
if initial_call:
self.drive.field_centric = False
self.sd = NetworkTable.getTable('SmartDashboard')
self.tracker.enable()
self.y_ctrl.move_to(self.drive_distance_feet)
self.sd.putNumber('TESTING: ', self.y_ctrl.get_position())
if self.y_ctrl.is_at_location():
self.next_state('finish')
@state
def failed_distance(self):
self.next_state('finish')
@state
def finish(self):
self.tracker.disable()
self.done()
class SideGearPlace(VictisAuto):
MODE_NAME = 'Drive Forward'
DEFAULT = False
# Injection
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
shooter = shooter.Shooter
x_ctrl = pos_controller.XPosController
y_ctrl = pos_controller.YPosController
fc_y_ctrl = pos_controller.FCYPosController
fc_x_ctrl = pos_controller.FCXPosController
angle_ctrl = angle_controller.AngleController
moving_angle_ctrl = angle_controller.MovingAngleController
tracker = position_tracker.PositionTracker
fc_tracker = position_tracker.FCPositionTracker
s_direction = tunable(
1, subtable='side'
) # When set to 1 this will run gear place on the right side
# Distances
# Warning: this is a field centric mode; all positions are relative to starting position
s_out_y = tunable(12, subtable='side')
s_out_x = tunable(0, subtable='side')
#################################################
# This portion of the code is for placing gears #
#################################################
@timed_state(duration=10, next_state='failed', first=True)
def drive_out(self):
self.fc_y_ctrl.move_to(self.s_out_y)
self.fc_x_ctrl.move_to(self.s_out_x)
self.moving_angle_ctrl.align_to(0)
if self.fc_y_ctrl.is_at_location() and self.fc_x_ctrl.is_at_location():
self.next_state('finish')
class Shooter(StateMachine):
belt_motor = wpilib.spark.Spark
shooter_motor = ctre.CANTalon
belt_speed = 0
shooter_speed = 0
max_shooter_speed = tunable(-0.9)
def stop(self):
self.shooter_speed = 0
self.belt_speed = 0
self.done()
def shoot(self):
self.engage()
def force_spin(self):
self.shooter_speed = self.max_shooter_speed
def force_feed(self):
self.belt_speed = -1
@timed_state(duration=1.0, first=True, next_state='feed_shoot')
def spinup(self):
print('spinning up')
self.shooter_speed = self.max_shooter_speed
@state
def feed_shoot(self):
self.shooter_speed = self.max_shooter_speed
self.belt_speed = -1
def execute(self):
super().execute()
self.belt_motor.set(self.belt_speed)
self.shooter_motor.set(self.shooter_speed)
self.belt_speed = 0
self.shooter_speed = 0
class ShootMiddleGearPlace(MiddleGearPlace):
MODE_NAME = 'Shoot middle Gear Place'
DEFAULT = False
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
shooter = shooter.Shooter
x_ctrl = XPosController
y_ctrl = YPosController
angle_ctrl = AngleController
moving_angle_ctrl = MovingAngleController
drive_back_distance = tunable(-3)
strafe_tower_distance = tunable(-4)
at_tower_angle = tunable(55)
@timed_state(duration=5, next_state='failed')
def drive_back(self, initial_call):
if initial_call:
self.tracker.reset()
self.y_ctrl.move_to(self.drive_back_distance)
self.moving_angle_ctrl.align_to(0)
if self.y_ctrl.is_at_location():
self.next_state('strafe_tower')
@timed_state(duration=6, next_state='failed')
def strafe_tower(self, initial_call):
if initial_call:
self.tracker.reset()
self.x_ctrl.move_to(self.strafe_tower_distance)
self.moving_angle_ctrl.align_to(0)
self.shooter.force_spin()
if self.x_ctrl.is_at_location():
self.next_state('align_to_tower')
@timed_state(duration=5, next_state='failed')
def align_to_tower(self):
self.angle_ctrl.align_to(self.at_tower_angle)
self.shooter.force_spin()
if self.angle_ctrl.is_aligned():
self.drive.set_raw_rcw(0.0)
self.next_state('check_angle')
self.shooter.force_spin()
@timed_state(duration=0.5, next_state='sit_and_shoot')
def check_angle(self):
if not self.angle_ctrl.is_aligned_to(self.at_tower_angle):
self.next_state('align_to_tower')
@timed_state(duration=8, next_state='finish')
def sit_and_shoot(self):
self.drive.debug(debug_modules=True)
self.shooter.force_spin()
self.shooter.force_feed()
class TargetGoal(StateMachine):
# Aliases
intake = intake.Arm
drive = drive.Drive
shootBall = shootBall.ShootBall
# Fetch NetworkTables values
present = ntproperty('/components/autoaim/present', False)
targetHeight = ntproperty('/components/autoaim/target_height', 0)
idealHeight = tunable(-7)
heightThreshold = tunable(-7)
shoot = False
def target(self):
"""When called, seek target"""
self.engage()
self.shoot = False
def target_shoot(self):
"""When called, seek target then shoot"""
self.engage()
self.shoot = True
@state(first=True)
def align(self, initial_call):
"""First state: turn robot to be facing tower"""
# If it's the first time
if initial_call:
# Then turn on vision system
self.drive.enable_camera_tracking()
# If it's inline with the tower and it's been told to shoot when done aiming,
if self.drive.align_to_tower() and self.shoot:
# Then move on to shooting state.
self.next_state('camera_assisted_drive')
@state
def camera_assisted_drive(self):
"""Second state: go forward to shooting location"""
# If target is close
if self.targetHeight > self.heightThreshold:
# Move forward, guided by threshold
self.drive.move(
max(abs(self.idealHeight - self.targetHeight) / 55, .5), 0)
# Align to tower. Pretty self explanitory.
self.drive.align_to_tower()
else:
# Otherwise, move on to next step, shooting.
self.next_state('shoot')
@state
def shoot(self):
"""Align to tower and fire ball."""
self.drive.align_to_tower()
self.shootBall.shoot()
def done(self):
"""Finish up, disable everything."""
# Kill camera tracking
self.drive.disable_camera_tracking()
# Stop state machine
StateMachine.done(self)
class MyRobot(magicbot.MagicRobot):
drive = swervedrive.SwerveDrive
shooter = shooter.Shooter
gear_picker = gearpicker.GearPicker
climber = climber.Climber
gimbal = gimbal.Gimbal
field_centric = FieldCentric
tracker = PositionTracker
fc_tracker = FCPositionTracker
y_ctrl = YPosController
x_ctrl = XPosController
fc_y_ctrl = FCYPosController
fc_x_ctrl = FCXPosController
angle_ctrl = AngleController
moving_angle_ctrl = MovingAngleController
pos_history = PositionHistory
auto_align = AutoAlign
gamepad_mode = tunable(False)
def createObjects(self):
"""
Create basic components (motor controllers, joysticks, etc.).
"""
# NavX
self.navx = navx.AHRS.create_spi()
# Initialize SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
# Joysticks
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.secondary_joystick = wpilib.Joystick(2)
# Triggers and buttons
self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1)
# Drive motors
self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True)
self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True)
self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59)
self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True)
# Drive control
self.field_centric_button = ButtonDebouncer(self.left_joystick, 6)
self.predict_position = ButtonDebouncer(self.left_joystick, 7)
self.field_centric_drive = True
self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1)
self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4)
self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5)
self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10)
# Shooting motors
self.shooter_motor = ctre.CANTalon(15)
self.belt_motor = wpilib.spark.Spark(9)
self.light_controller = wpilib.VictorSP(8)
# Pistons for gear picker
self.picker = wpilib.DoubleSolenoid(6, 7)
self.pivot = wpilib.DoubleSolenoid(4, 5)
self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0))
# Toggling button on secondary joystick
self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2)
# Or, up and down buttons on right joystick
self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2)
self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3)
# Climb motors
self.climb_motor1 = wpilib.spark.Spark(4)
self.climb_motor2 = wpilib.spark.Spark(5)
# Camera gimble
self.gimbal_yaw = wpilib.Servo(6)
self.gimbal_pitch = wpilib.Servo(7)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
def robotInit(self):
super().robotInit()
wpilib.CameraServer.launch('camera/vision.py:main')
def autonomous(self):
"""
Prepare for autonomous mode.
"""
self.field_centric.set_raw_values = True
self.drive.allow_reverse = False
self.drive.wait_for_align = True
self.drive.threshold_input_vectors = True
super().autonomous()
def disabledPeriodic(self):
"""
Repeat periodically while robot is disabled.
Usually emptied.
Sometimes used to easily test sensors and other things.
"""
self.update_sd()
def disabledInit(self):
"""
Do once right away when robot is disabled.
"""
def teleopInit(self):
"""
Do when teleoperated mode is started.
"""
self.drive.flush() # This is a poor solution to the drive system maintain speed/direction
self.field_centric.set_raw_values = False
self.drive.allow_reverse = True
self.drive.wait_for_align = False
self.drive.squared_inputs = True
self.drive.threshold_input_vectors = True
def move(self, x, y, rcw):
if self.right_joystick.getRawButton(1):
rcw *= 0.75
if not self.field_centric_drive or self.left_joystick.getRawButton(1):
self.drive.move(x, y, rcw)
else:
self.field_centric.move(x, y)
self.drive.set_rcw(rcw)
def teleopPeriodic(self):
"""
Do periodically while robot is in teleoperated mode.
"""
# Drive system
if not self.gamepad_mode or self.ds.isFMSAttached():
self.move(self.left_joystick.getY()*-1, self.left_joystick.getX()*-1, self.right_joystick.getX())
else:
self.move(self.left_joystick.getRawAxis(1)*-1, self.left_joystick.getRawAxis(0), self.left_joystick.getRawAxis(2)*-1)
if self.field_centric_button.get():
if not self.field_centric_drive:
self.navx.reset()
self.field_centric_drive = not self.field_centric_drive
if self.left_joystick.getRawButton(2):
self.drive.request_wheel_lock = True
if self.right_joystick.getRawButton(4):
self.drive.set_raw_strafe(0.25)
elif self.right_joystick.getRawButton(5):
self.drive.set_raw_strafe(-0.25)
if self.right_joystick.getRawButton(3):
self.drive.set_raw_fwd(0.35)
elif self.right_joystick.getRawButton(2):
self.drive.set_raw_fwd(-0.35)
# Gear picker
if self.pivot_toggle_button.get():
if self.gear_picker._pivot_state == 1:
self.gear_picker.pivot_down()
else:
self.gear_picker.pivot_up()
if self.secondary_trigger.get():
self.gear_picker.actuate_picker()
# Climber
if self.left_joystick.getRawButton(3) or self.secondary_joystick.getRawButton(4):
self.climber.climb(-1)
if self.secondary_joystick.getRawButton(6):
self.climber.climb(-0.5)
if self.secondary_joystick.getRawButton(5):
self.light_controller.set(1)
else:
self.light_controller.set(0)
# Shooter
if self.secondary_joystick.getRawButton(3):
self.shooter.shoot()
else:
self.shooter.stop()
# Secondary driver gimble control
if self.secondary_joystick.getRawButton(12):
# scale.scale params: (input, input_min, input_max, output_min, output_max)
self.gimbal.yaw = scale.scale(self.secondary_joystick.getX()*-1, -1, 1, 0, 0.14)
self.gimbal.pitch = scale.scale(self.secondary_joystick.getY()*-1, -1, 1, 0.18, 0.72)
# Auto align test
if self.right_joystick.getRawButton(10):
self.auto_align.align()
if self.align_button.get_released():
self.auto_align.done()
self.update_sd()
def update_sd(self):
self.sd.putNumber('current/climb1_current_draw', self.pdp.getCurrent(1))
self.sd.putNumber('current/climb2_current_draw', self.pdp.getCurrent(2))
self.sd.putNumber('current/rr_rotate_current_draw', self.pdp.getCurrent(8))
self.sd.putNumber('current/fl_rotate_current_draw', self.pdp.getCurrent(7))
self.sd.putNumber('pneumatics/tank_pressure', self.pessure_sensor.getPressure())
self.drive.update_smartdash()
class MiddleGearPlace(VictisAuto):
"""
This state is meant the be extended and should never be run by itself
"""
MODE_NAME = 'Middle Gear Place'
DEFAULT = False
DISABLED = True # Important to prevent this class from being run by itself
# Injection
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
shooter = shooter.Shooter
x_ctrl = pos_controller.XPosController
y_ctrl = pos_controller.YPosController
fc_y_ctrl = pos_controller.FCYPosController
fc_x_ctrl = pos_controller.FCXPosController
angle_ctrl = angle_controller.AngleController
moving_angle_ctrl = angle_controller.MovingAngleController
tracker = position_tracker.PositionTracker
fc_tracker = position_tracker.FCPositionTracker
m_direction = tunable(
1, subtable='middle'
) # When direction is 1 the shooting tower is left of the robot
# Distances
# Warning: this is a field centric mode all positions are relative to starting position
m_out_y = tunable(6.5, subtable='middle')
m_out_x = tunable(0, subtable='middle')
m_back_y = tunable(
3, subtable='middle'
) # The cord that the robot backs off to before strafeing
# Shooting position
m_tower_y = tunable(3, subtable='middle')
m_tower_x = tunable(-4, subtable='middle')
m_tower_angle = tunable(65, subtable='middle')
def initialize(self):
pass
#################################################
# This portion of the code is for placing gears #
#################################################
@state
def middle_start(self):
self.drive.field_centric = True
self.gear_picker._picker_state = 2
self.angle_ctrl.reset_angle()
self.fc_tracker.enable()
self.next_state('middle_drive_to_gear')
@timed_state(duration=4, next_state='middle_try_place')
def middle_drive_to_gear(self):
self.fc_x_ctrl.move_to(self.m_out_x)
self.fc_y_ctrl.move_to(self.m_out_y)
self.moving_angle_ctrl.align_to(0)
if self.fc_y_ctrl.is_at_location():
self.gear_picker._picker_state = 1
self.next_state('middle_drive_back')
@timed_state(duration=1, next_state='middle_drive_back')
def middle_try_place(self):
self.fc_x_ctrl.move_to(self.m_out_x)
self.fc_y_ctrl.move_to(self.m_out_y)
if self.fc_y_ctrl.is_at_location():
self.next_state('middle_drive_back')
@timed_state(duration=3, next_state='failed')
def middle_drive_back(self, initial_call):
if initial_call:
self.gear_picker._picker_state = 1
self.fc_y_ctrl.move_to(self.m_back_y)
self.moving_angle_ctrl.align_to(0)
if self.fc_y_ctrl.is_at_location():
self.next_state('transition')
############################################
# This portion of the code is for shooting #
############################################
@state
def middle_start_shoot(self):
self.next_state('middle_to_tower')
@timed_state(duration=5, next_state='failed')
def middle_to_tower(self):
self.fc_y_ctrl.move_to(self.m_tower_y)
self.fc_x_ctrl.move_to(self.m_tower_x * self.m_direction)
self.moving_angle_ctrl.align_to(self.m_tower_angle * self.m_direction)
self.shooter.force_spin()
if self.fc_x_ctrl.is_at_location() and self.fc_y_ctrl.is_at_location():
self.next_state('middle_align_to_tower')
@timed_state(duration=5, next_state='failed')
def middle_align_to_tower(self):
self.angle_ctrl.align_to(self.m_tower_angle * self.m_direction)
self.shooter.force_spin()
if self.angle_ctrl.is_aligned():
self.next_state('middle_verify_tower_angle')
@timed_state(duration=0.5, next_state='middle_shoot')
def middle_verify_tower_angle(self):
"""
This state verifies that the robot is in position for 0.5s. Doing
this reduces inaccuracy
"""
self.shooter.force_spin()
if not self.angle_ctrl.is_aligned_to(
self.m_tower_angle * self.m_direction):
self.next_state('middle_align_to_tower')
@state
def middle_shoot(self):
self.shooter.force_spin()
self.shooter.force_feed()
######################################################
# This portion of the code is used when not shooting #
######################################################
@state
def middle_end(self):
self.next_state('finish')
class GearPlace(MiddleGearPlace, SideGearPlace):
"""
This is the main autonomous for 1418's 2017 robot. This class inherits from
the MiddleGearPlace and SideGearPlace classes. By changing the position
variable in NetworkTables the autonomous mode can be configured to place a
gear and shoot from any position. The configuration is usually done through
our dashboard (web ui).
"""
MODE_NAME = 'Gear Place'
DEFAULT = True
DISABLED = False
angle_ctrl = angle_controller.AngleController
moving_angle_ctrl = angle_controller.MovingAngleController
x_ctrl = pos_controller.XPosController
y_ctrl = pos_controller.YPosController
gear_picker = gearpicker.GearPicker
# Position tunable should be one of four things
# 'middle_left' - middle gear place with left boiler
# 'middle_right' - middle gear place with right boiler
# 'left' - left side gear place
# 'right' - right side gear place
position = tunable('left')
shoot = tunable(False)
def initialize(self):
MiddleGearPlace.initialize(self)
SideGearPlace.initialize(self)
@state(first=True)
def first_state(self):
self.gear_picker._picker_state = 2
if self.position == 'right':
self.s_direction = 1
self.position = 'side'
elif self.position == 'left':
self.s_direction = -1
self.position = 'side'
elif self.position == 'middle_left':
self.m_direction = 1
self.position = 'middle'
elif self.position == 'middle_right':
self.m_direction = -1
self.position = 'middle'
print('Starting ', self.position, ' gear place')
self.next_state(self.position + '_start')
@state
def transition(self):
if self.shoot:
self.next_state(self.position + '_start_shoot')
else:
self.next_state(self.position + '_end')
@state
def failed(self):
self.drive.debug(debug_modules=True)
self.next_state('finish')
@state
def finish(self):
self.drive.flush()
self.done()
class SideGearPlace(VictisAuto):
"""
This state is meant the be extended and should never be run by itself.
"""
MODE_NAME = 'Side Gear Place'
DEFAULT = False
DISABLED = True # Important to prevent this class from being run by itself
# Injection
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
shooter = shooter.Shooter
x_ctrl = pos_controller.XPosController
y_ctrl = pos_controller.YPosController
fc_y_ctrl = pos_controller.FCYPosController
fc_x_ctrl = pos_controller.FCXPosController
angle_ctrl = angle_controller.AngleController
moving_angle_ctrl = angle_controller.MovingAngleController
tracker = position_tracker.PositionTracker
fc_tracker = position_tracker.FCPositionTracker
s_direction = tunable(
1, subtable='side'
) # When set to 1 this will run gear place on the right side
# Distances
# Warning: this is a field centric mode all positions are relative to starting position
s_out_y = tunable(7.9, subtable='side')
s_out_x = tunable(0, subtable='side')
s_out_angle = tunable(-70, subtable='side')
s_to_peg_distance = tunable(2.5, subtable='side')
s_peg_y = tunable(
9.2, subtable='side'
) # TODO: Check this math (coverted from non field centric auto)
s_peg_x = tunable(-1, subtable='side')
s_back_y = tunable(7.5, subtable='side')
s_back_x = tunable(0, subtable='side')
s_tower_y = tunable(5.7, subtable='side')
s_tower_x = tunable(1, subtable='side')
s_tower_angle = tunable(-40, subtable='side')
s_drive_past_tower = tunable(False, subtable='side')
s_past_tower_y = tunable(12, subtable='side')
s_past_tower_x = tunable(0, subtable='side')
s_past_tower_angle = tunable(-180, subtable='side')
def initialize(self):
pass
#################################################
# This portion of the code is for placing gears #
#################################################
@state
def side_start(self):
self.drive.field_centric = True
self.gear_picker._picker_state = 2
self.angle_ctrl.reset_angle()
self.fc_tracker.enable()
self.next_state('side_drive_to_gear')
@timed_state(duration=7, next_state='failed')
def side_drive_to_gear(self):
self.fc_y_ctrl.move_to(self.s_out_y)
self.fc_x_ctrl.move_to(self.s_out_x)
if self.fc_tracker.get_y() > self.s_out_y / 2 and False:
self.moving_angle_ctrl.align_to(self.s_out_angle *
self.s_direction)
else:
self.moving_angle_ctrl.align_to(0)
if self.fc_y_ctrl.is_at_location() and self.fc_x_ctrl.is_at_location():
self.next_state('side_align_to_peg')
@timed_state(duration=7, next_state='failed')
def side_align_to_peg(self):
self.angle_ctrl.align_to(self.s_out_angle * self.s_direction)
if self.angle_ctrl.is_aligned():
self.next_state('side_verify_peg_alignment')
@timed_state(duration=0.5, next_state='side_drive_to_peg')
def side_verify_peg_alignment(self):
"""
This state verifies that the robot is in position for 0.5s. Doing
this reduces inaccuracy
"""
if not self.angle_ctrl.is_aligned_to(
self.s_out_angle * self.s_direction):
self.next_state('side_align_to_peg')
@timed_state(duration=3, next_state='side_try_place')
def side_drive_to_peg(self, initial_call):
if initial_call:
self.tracker.enable()
self.tracker.reset()
self.y_ctrl.move_to(self.s_to_peg_distance)
self.moving_angle_ctrl.align_to(self.s_out_angle * self.s_direction)
if self.y_ctrl.is_at_location():
self.next_state('side_drive_back')
@timed_state(duration=1, next_state='side_drive_back')
def side_try_place(self):
self.y_ctrl.move_to(self.s_to_peg_distance)
self.drive.set_raw_rcw(-0.4 * self.s_direction)
if self.y_ctrl.is_at_location():
self.next_state('side_drive_back')
@timed_state(duration=5, next_state='failed')
def side_drive_back(self, initial_call):
if initial_call:
self.tracker.reset()
self.gear_picker._picker_state = 1
self.gear_picker.pivot_down()
self.y_ctrl.move_to(-self.s_to_peg_distance)
self.moving_angle_ctrl.align_to(self.s_out_angle * self.s_direction)
if self.y_ctrl.is_at_location():
if not self.s_drive_past_tower:
self.next_state('transition')
else:
self.next_state('side_drive_past_tower')
@timed_state(duration=5, next_state='failed')
def side_drive_past_tower(self):
self.fc_y_ctrl.move_to(self.s_past_tower_y)
self.fc_x_ctrl.move_to(self.s_past_tower_x)
self.moving_angle_ctrl.align_to(self.s_out_angle * self.s_direction)
if self.fc_y_ctrl.is_at_location():
self.next_state('transition')
############################################
# This portion of the code is for shooting #
############################################
@state
def side_start_shoot(self):
self.next_state('side_to_tower')
@timed_state(duration=5, next_state='failed')
def side_to_tower(self):
self.fc_y_ctrl.move_to(self.s_tower_y)
self.fc_x_ctrl.move_to(self.s_tower_x * self.s_direction)
self.moving_angle_ctrl.align_to(self.s_tower_angle * self.s_direction)
self.shooter.force_spin()
if self.fc_y_ctrl.is_at_location() and self.fc_x_ctrl.is_at_location():
self.next_state('side_align_to_tower')
@timed_state(duration=7, next_state='failed')
def side_align_to_tower(self):
self.angle_ctrl.align_to(self.s_tower_angle * self.s_direction)
self.shooter.force_spin()
if self.angle_ctrl.is_aligned():
self.next_state('side_verify_tower_alignment')
@timed_state(duration=0.5, next_state='side_shoot')
def side_verify_tower_alignment(self):
"""
This state verifies that the robot is in position for 0.5s. Doing
this reduces inaccuracy
"""
self.shooter.force_spin()
if not self.angle_ctrl.is_aligned_to(
self.s_tower_angle * self.s_direction):
self.next_state('side_align_to_tower')
@state
def side_shoot(self):
self.shooter.force_spin()
self.shooter.force_feed()
######################################################
# This portion of the code is used when not shooting #
######################################################
@state
def side_end(self):
self.next_state('finish')
class ModularAutonomous(LowBar, ChevalDeFrise, Portcullis, Charge, Default):
MODE_NAME = 'Modular_Autonomous'
DEFAULT = False
sd = NetworkTable
intake = Intake.Arm
drive = Drive.Drive
targetGoal = targetGoal.TargetGoal
present = ntproperty('/components/autoaim/present', False)
opposite = tunable(120)
Ramp_Distance = tunable(6)
def initialize(self):
LowBar.initialize(self)
Portcullis.initialize(self)
@state(first=True)
def startModularAutonomous(self):
print(self.sd.getValue('robotDefense', 'Default') + 'Start')
self.intake.manualZero()
self.drive.reset_gyro_angle()
self.next_state(self.sd.getValue('robotDefense', 'LowBar') + 'Start')
self.position = int(self.sd.getValue('robotPosition', '1'))
@state
def transition(self):
# tangent^-1(opposite/adj)
#opposite = 10
#adj = 50*(position - 1)
if self.position == 3:
self.angleConst = 1
elif self.position == 2:
self.angleConst = -1
self.rotateAngle = math.degrees(math.atan(
self.opposite / 50)) * self.angleConst
self.drive_distance = math.sqrt(2500 + self.opposite**2)
if self.position == 1 or self.position == 4:
self.next_state('drive_to_wall')
else:
self.next_state('rotate')
@state
def rotate(self):
if self.drive.angle_rotation(self.rotateAngle):
self.drive.reset_drive_encoders()
self.next_state('drive_to_position')
@state
def drive_to_position(self):
if self.drive.drive_distance(self.drive_distance):
self.next_state('rotate_back')
@state
def rotate_back(self):
if self.drive.angle_rotation(-45 * self.angleConst):
self.drive.reset_gyro_angle()
self.drive.enable_camera_tracking()
self.next_state('rotate_to_align')
@timed_state(duration=1, next_state='target')
def rotate_to_align(self, initial_call):
if initial_call:
self.drive.reset_gyro_angle()
if self.drive.align_to_tower():
self.next_state('target')
@state
def target(self):
self.targetGoal.target()
class MiddleGearPlace(VictisAuto):
MODE_NAME = 'Middle Gear Place'
DEFAULT = False
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
x_ctrl = XPosController
y_ctrl = YPosController
angle_ctrl = AngleController
moving_angle_ctrl = MovingAngleController
tracker = PositionTracker
out_distance = tunable(6)
#drive_back_distance = tunable(-3)
strafe_distance = tunable(8)
drive_past_line_distance = tunable(8)
@timed_state(duration=4, next_state='rcw_with_gear', first=True)
def drive_out(self, initial_call):
# Go forward
if initial_call:
self.drive.field_centric = False
self.gear_picker._picker_state = 2
self.angle_ctrl.reset_angle()
self.tracker.enable()
self.y_ctrl.move_to(self.out_distance)
self.moving_angle_ctrl.align_to(0)
if self.y_ctrl.is_at_location():
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@timed_state(duration=1, next_state='try_release')
def rcw_with_gear(self):
self.y_ctrl.move_to(self.out_distance)
#self.drive.set_raw_rcw(0.4)
if self.y_ctrl.is_at_location():
self.drive.set_raw_rcw(0.0)
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@state
def try_release(self):
self.drive.debug()
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@timed_state(duration=5, next_state='failed')
def drive_back(self, initial_call):
if initial_call:
self.tracker.reset()
self.y_ctrl.move_to(self.drive_back_distance)
self.moving_angle_ctrl.align_to(0)
if self.y_ctrl.is_at_location():
self.next_state('finish')
"""
class RightSideGearPlace(VictisAuto):
"""
Place robot 15in from string 90deg to string."""
MODE_NAME = 'Right side gear place'
DEFAULT = False
DIRECTION = 1
drive = swervedrive.SwerveDrive
gear_picker = gearpicker.GearPicker
x_ctrl = XPosController
y_ctrl = YPosController
angle_ctrl = AngleController
moving_angle_ctrl = MovingAngleController
tracker = PositionTracker
out_distance = tunable(7.5)
rotate_to_angle = tunable(-60)
wiggle_value = tunable(-5)
to_gear_distance = tunable(2)
drive_back_distance = tunable(-2.7)
drive_past_line_distance = tunable(5)
@timed_state(duration=7, next_state='failed', first=True)
def drive_out(self, initial_call):
# Go forward
if initial_call:
self.drive.field_centric = False
self.gear_picker._picker_state = 2
self.angle_ctrl.reset_angle()
self.tracker.enable()
self.y_ctrl.move_to(self.out_distance)
self.moving_angle_ctrl.align_to(0)
if self.y_ctrl.is_at_location():
self.next_state('rotate')
@timed_state(duration=5, next_state='failed')
def rotate(self):
self.angle_ctrl.align_to(self.rotate_to_angle * self.DIRECTION)
if self.angle_ctrl.is_aligned():
self.next_state('drive_to_gear')
@timed_state(duration=0.5, next_state='drive_to_gear')
def check_rotate(self):
if not self.angle_ctrl.is_aligned_to(
self.rotate_to_angle * self.DIRECTION):
self.next_state('rotate')
@timed_state(duration=3, next_state='rcw_with_gear')
def drive_to_gear(self, initial_call):
if initial_call:
self.tracker.reset()
self.y_ctrl.move_to(self.to_gear_distance)
self.moving_angle_ctrl.align_to(self.rotate_to_angle * self.DIRECTION)
if self.y_ctrl.is_at_location():
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@timed_state(duration=1, next_state='try_release')
def rcw_with_gear(self):
self.y_ctrl.move_to(self.to_gear_distance)
self.drive.set_raw_rcw(0.4 * self.DIRECTION)
if self.y_ctrl.is_at_location():
self.drive.set_raw_rcw(0.0)
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@state
def try_release(self):
self.drive.debug()
self.gear_picker._picker_state = 1
self.next_state('drive_back')
@timed_state(duration=5, next_state='failed')
def drive_back(self, initial_call):
if initial_call:
self.tracker.reset()
self.y_ctrl.move_to(self.drive_back_distance)
self.moving_angle_ctrl.align_to(self.rotate_to_angle * self.DIRECTION)
if self.y_ctrl.is_at_location():
self.next_state('rotate_back')
@state
def rotate_back(self):
"""
IF MODIFIED: Function in ShootLeftGearPlace must be modified too"""
self.angle_ctrl.align_to(0)
if self.angle_ctrl.is_aligned():
self.next_state('drive_past_line')
@state
def drive_past_line(self, initial_call):
if initial_call:
self.tracker.reset()
self.moving_angle_ctrl.align_to(0)
self.y_ctrl.move_to(self.drive_past_line_distance)
if self.y_ctrl.is_at_location():
self.next_state('finish')
class PositionTracker:
fl_module = swervemodule.SwerveModule
fr_module = swervemodule.SwerveModule
rl_module = swervemodule.SwerveModule
rr_module = swervemodule.SwerveModule
x_pos = tunable(0)
y_pos = tunable(0)
def setup(self):
self.enabled = False
self._position = {
'y': 0,
'x': 0,
'rcw': 0
}
self._zeros = {
'front_left': 0,
'rear_right': 0,
'rear_left': 0,
'front_right': 0
}
self.modules = {
'front_right': self.fr_module,
'front_left': self.fl_module,
'rear_left': self.rl_module,
'rear_right': self.rr_module
}
self.module_torque_angle = {
'front_right': (math.pi / 4),
'front_left': -(math.pi / 4),
'rear_left': (math.pi / 4),
'rear_right': -(math.pi / 4)
}
self.width = (22/12)/2
self.length = (18.5/12)/2
def get_diff(self, value, old_zero):
diff = value - old_zero
new_zero = value
return diff, new_zero
def separate_y(self, dist, theta):
"""
This function separates the movment in the y direction with wheels that 0 deg is forward.
"""
return math.cos(theta) * dist
def separate_x(self, dist, theta):
"""
This function separates the movment in the x direction with wheels that 0 deg is forward.
"""
return math.sin(theta) * dist
def enable(self, zero_position = True):
if not self.fl_module.has_drive_encoder or not self.rr_module.has_drive_encoder:
if not self.rl_module.has_drive_encoder or not self.fr_module.has_drive_encoder:
raise 'Not enough drive encoders to predict position'
self.enabled = True
if zero_position:
self.reset()
def disable(self, zero_position=False):
self.enabled = False
if zero_position:
self._position['y'] = 0.0
self._position['x'] = 0.0
self._position['rcw'] = 0.0
def reset(self):
self._position['y'] = 0.0
self._position['x'] = 0.0
self._position['rcw'] = 0.0
self._zeros['front_left'] = self.fl_module.get_drive_encoder_distance()
self._zeros['rear_right'] = self.rr_module.get_drive_encoder_distance()
self._zeros['rear_left'] = self.rl_module.get_drive_encoder_distance()
self._zeros['front_right'] = self.fr_module.get_drive_encoder_distance()
def get_x(self):
return self._position['x']
def get_y(self):
return self._position['y']
def _calculate(self):
encoders = 0
radius = math.hypot(self.length, self.width)
x = 0
y = 0
rcw = 0
for key in self.modules:
if self.modules[key].has_drive_encoder:
dist = self.modules[key].get_drive_encoder_distance()
theta = swervemodule.SwerveModule.voltage_to_rad(self.modules[key].get_voltage())
dist, self._zeros[key] = self.get_diff(dist, self._zeros[key])
x += -self.separate_x(dist, theta)
y += self.separate_y(dist, theta)
# Rotation calculations
theta += self.module_torque_angle[key]
if key == 'rear_right' or key == 'front_right':
dist *= -1
rcw += radius * (math.cos(theta) * dist)
encoders += 1
self._position['x'] += x * (1/encoders)
self._position['y'] += y * (1/encoders)
self._position['rcw'] += rcw * (1/encoders)
def execute(self):
if self.enabled:
self._calculate()
self.x_pos = self._position['x']
self.y_pos = self._position['y']
class TrajectoryFollower:
"""
Move along generated paths for autonomous
"""
WHEEL_DIAMETER = 0.5
KV = tunable(1.0269)
KA = tunable(0.0031)
ANGLE_CONSTANT = tunable(0.8)
drive: drive.Drive
navx: navx.AHRS
l_encoder: wpilib.Encoder
r_encoder: wpilib.Encoder
generated_trajectories: dict
def on_enable(self):
"""
Setup encoder follower objects.
"""
self._current_trajectory = None
self.last_difference = 0
self.left_follower = pf.followers.EncoderFollower(None)
self.right_follower = pf.followers.EncoderFollower(None)
self.left_follower.configurePIDVA(1.0, 0, 0, self.KV, self.KA)
self.right_follower.configurePIDVA(1.0, 0, 0, self.KV, self.KA)
self._cofigure_encoders()
def follow_trajectory(self, trajectory_name: str):
"""
Follow a specified trajectory.
:param trajectory_name: The name of the trajectory to follow.
"""
print('Following Trajectory:', trajectory_name)
self._current_trajectory = trajectory_name
self.left_follower.setTrajectory(self.generated_trajectories[trajectory_name][0])
self.right_follower.setTrajectory(self.generated_trajectories[trajectory_name][1])
self._cofigure_encoders()
def _cofigure_encoders(self):
"""
Configure the encoders for following a trajectory.
"""
self.l_encoder.reset()
self.r_encoder.reset()
self.left_follower.configureEncoder(self.l_encoder.get(), 1024, self.WHEEL_DIAMETER)
self.right_follower.configureEncoder(self.r_encoder.get(), 1024, self.WHEEL_DIAMETER)
def is_following(self, trajectory_name):
"""
Check whether a trajectory is being followed.
:param trajectory_name: The name of the trajectory to check.
"""
return self._current_trajectory is not None and self._current_trajectory == trajectory_name
def execute(self):
"""
Calculate the movement values and move the robot.
"""
"""
