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. """ """