class Drive: """ Handle robot drivetrain. All drive interaction must go through this class. """ train: wpilib.drive.DifferentialDrive y = will_reset_to(0) rot = will_reset_to(0) SPEED_CONSTANT = 0.5 ROT_CONSTANT = 0.75 def move(self, y: float, rot: float): """ Move robot. :param y: Speed of motion in the y direction. [-1..1] :param rot: Speed of rotation. [-1..1] """ self.y = y self.rot = rot def execute(self): """ Handle driving. """ self.train.arcadeDrive(self.y * self.SPEED_CONSTANT, self.rot * self.ROT_CONSTANT)
class Drivetrain: drive_l1: ctre.WPI_TalonSRX drive_l2: ctre.VictorSPX drive_l3: ctre.VictorSPX drive_r1: ctre.WPI_TalonSRX drive_r2: ctre.VictorSPX drive_r3: ctre.VictorSPX speed = magicbot.will_reset_to(0) rotation = magicbot.will_reset_to(0) def setup(self): self.drive_l2.follow(self.drive_l1) self.drive_l3.follow(self.drive_l1) self.drive_r2.follow(self.drive_r1) self.drive_r3.follow(self.drive_r1) self.drive_l1.setNeutralMode(self.drive_l1.NeutralMode.Brake) self.drive_r1.setNeutralMode(self.drive_r1.NeutralMode.Brake) self.ddrive = wpilib.drive.DifferentialDrive(self.drive_l1, self.drive_r1) # def on_enable(self): # self.x = 0 # self.y = 0 def drive(self, speed, rotation): self.speed = -speed self.rotation = -rotation def autoAlign(self, rotation): self.rotation = rotation def execute(self): self.ddrive.arcadeDrive(self.speed, self.rotation, False)
class Drive: """ Handle robot drivetrain. All drive interaction must go through this class. """ train: wpilib.drive.DifferentialDrive y = will_reset_to(0) rot = will_reset_to(0) speed_constant = tunable(1.05) rotational_constant = tunable(0.5) squared_inputs = tunable(False) def setup(self): """ Run setup code on the injected variables (train) """ def move(self, y: float, rot: float): """ Move robot. :param y: Speed of motion in the y direction. [-1..1] :param rot: Speed of rotation. [-1..1] """ self.y = y self.rot = rot def execute(self): """ Handle driving. """ self.train.arcadeDrive(self.speed_constant * self.y, self.rotational_constant * self.rot, squareInputs=self.squared_inputs)
class Drive: drive_train: wpilib.drive.DifferentialDrive navx: navx.AHRS right_encoder: ExternalEncoder left_encoder: ExternalEncoder y = will_reset_to(0) rotation = will_reset_to(0) squared = True def drive(self, y, rot, squared=True): self.y = y self.rotation = rot self.squared = squared def execute(self): self.drive_train.arcadeDrive(-self.y, -self.rotation, self.squared) self.update_sd() def reset_encoders(self): self.left_encoder.zero() self.right_encoder.zero() def update_sd(self): wpilib.SmartDashboard.putData("Drive/Left Drive Encoder", self.left_encoder.encoder) wpilib.SmartDashboard.putData("Drive/Right Drive Encoder", self.right_encoder.encoder) wpilib.SmartDashboard.putNumber("Drive/Navx", self.navx.getYaw())
class DriveTrain: ''' Simple magicbot drive object ''' #wall_p = tunable(-1.8) #distance = tunable(0) #analog = tunable(0) #tx = tunable(0) #ty = tunable(0) #offset = tunable(1.0) #MaxY = tunable(0.8) #ultrasonic = wpilib.AnalogInput drive: wpilib.drive.DifferentialDrive gyro: wpilib.ADXRS450_Gyro x = will_reset_to(0) y = will_reset_to(0) def __init__(self): self.moveDeg = None self.saveAng = None # This stores the information of moveDeg for the second time # distance is .98 def move(self, y, x): self.y = y self.x = x -0.135 def move2(self, deg): self.moveDeg = deg def rotate(self, x): self.x = x def execute(self): self.tx = self.x self.ty = self.y if self.moveDeg != None: if self.saveAng == None: self.saveAng = self.gyro.getAngle() wantAngle = self.saveAng + self.moveDeg angle = self.gyro.getAngle() error = angle - wantAngle print(error) output = (error * 0.01) output = max(min(0.5, output), -0.5) self.x = output self.moveDeg = None self.drive.arcadeDrive(self.y, self.x, True)
class Arm: """ Single Jointed arm for that holds the hatch and cargo mechs """ arm_motor = WPI_TalonSRX arm_limit = wpilib.DigitalInput reset_angle = will_reset_to(180) kp = tunable(1) ki = tunable(0) kd = tunable(0) kf = tunable(0) target_angle = will_reset_to(180) zeroed = False # Constants armRatio = 24 / 84 def setup(self): self.arm_controller = ArmPID() self.blahhhh = ElevatorPID(self.kp, self.ki, self.kd, self.kf) self.arm_motor.clearStickyFaults() self.zeroed = False def set(self, angle): self.target_angle = angle self.arm_controller.resetIntegrator() def angle(self, ticks): return ticks * (360 * self.armRatio) / 4096 def ticks(self, angle): return int(angle * (4096 / (360 * self.armRatio))) def execute(self): if not self.zeroed: self.arm_motor.set(0.2) if self.arm_limit.get(): self.zeroed = True self.arm_motor.setSelectedSensorPosition( self.ticks(self.reset_angle) ) return # exits out of the function if we aren't zeroed self.arm_motor.set( self.arm_controller.update( self.kp, self.ki, self.kd, self.kf, self.target_angle, self.angle(self.arm_motor.getSelectedSensorPosition()), ) )
class Align: limelight: Limelight drive: Drive aligning = will_reset_to(False) angle = will_reset_to(0) def execute(self): if self.aligning: self.drive.align(self.angle, True) def align(self, angle): self.aligning = True self.angle = angle
class Drive: """ Handle robot drivetrain. All drive interaction must go through this class. """ train: wpilib.drive.DifferentialDrive y = will_reset_to(0) rot = will_reset_to(0) speed_constant = tunable(1.05) rotational_constant = tunable(0.5) squared_inputs = tunable(False) fine_movement = will_reset_to(False) fine_speed_multiplier = tunable(0.5) fine_rotation_multiplier = tunable(0.5) def __init__(self): self.enabled = False def setup(self): """ Set input threshold. """ self.train.setDeadband(0.1) def move(self, y: float, rot: float, fine_movement: bool = False): """ Move robot. :param y: Speed of motion in the y direction. [-1..1] :param rot: Speed of rotation. [-1..1] :param fine_movement: Decrease speeds for precise motion. """ self.y = y self.rot = rot self.fine_movement = fine_movement def execute(self): """ Handle driving. """ self.train.arcadeDrive( self.speed_constant * self.y * (self.fine_speed_multiplier if self.fine_movement else 1), self.rotational_constant * self.rot * (self.fine_rotation_multiplier if self.fine_movement else 1), squaredInputs=self.squared_inputs)
class Hang: winch_motor: ctre.WPI_TalonFX kracken_hook_latch: wpilib.DoubleSolenoid WINCH_SPEED = 0.5 winch_desired_output = will_reset_to(0.0) def __init__(self) -> None: self.fire_hook = False def on_disable(self) -> None: # stop motors self.winch_motor.stopMotor() def execute(self) -> None: # solenoid if self.fire_hook: self.kracken_hook_latch.set(wpilib.DoubleSolenoid.Value.kForward) # drive winch self.winch_motor.set(self.winch_desired_output) def raise_hook(self) -> None: # fire solenoid self.fire_hook = True def winch(self) -> None: # drive motor self.winch_desired_output = self.WINCH_SPEED def pay_out(self) -> None: # drive motor backwards to release cable in pits self.winch_desired_output = -self.WINCH_SPEED
class Cargo: lift_piston = wpilib.Solenoid motor = ctre.TalonSRX motor_speed = will_reset_to(0) is_in_position = tunable(False) motor_input_speed = tunable(-0.6) motor_output_speed = tunable(1) def lift(self): self.is_in_position = False def lower(self): self.is_in_position = True def intake(self): self.motor_speed = self.motor_input_speed def outtake(self): self.motor_speed = self.motor_output_speed def execute(self): self.motor.set(ctre.ControlMode.PercentOutput, self.motor_speed)
class Intake: """ Operate robot intake. """ intake_wheels: wpilib.SpeedControllerGroup _intake_wheel_speed = will_reset_to(0) def spin(self, speed: float = SPEED_MULTIPLIER): """ Set the speed of intake wheels. :param speed: The requested speed, between -1 and 1. """ self._intake_wheel_speed = speed def pull(self): """ Pull cube into intake. """ self.spin(-SPEED_MULTIPLIER) def push(self): """ Push cube out of intake. """ self.spin(SPEED_MULTIPLIER) def execute(self): """ Run intake motors. """ self.intake_wheels.set(self._intake_wheel_speed)
class Component2: component1 = Component1 some_motor = wpilib.Talon # This is changed to the value in robot.py SOME_CONSTANT = int # This gets reset after each invocation of execute() did_something = will_reset_to(False) def on_enable(self): """Called when the robot enters teleop or autonomous mode""" self.logger.info( "Robot is enabled: I have SOME_CONSTANT=%s", self.SOME_CONSTANT ) def do_something(self): self.did_something = True def execute(self): if self.did_something: self.some_motor.set(1) else: self.some_motor.set(0)
class Drive: lMotor = wpilib.Talon rMotor = wpilib.Talon # This is changed to the value in robot.py SOME_CONSTANT = int # This gets reset after each invocation of execute() started_driving = will_reset_to(False) def on_enable(self): '''Called when the robot enters teleop or autonomous mode''' self.logger.info("Robot is enabled: I have SOME_CONSTANT=%s", self.SOME_CONSTANT) def start_driving(self, speed, lTurn, rTurn): self.started_driving = True self.driving_speed = speed self.lTurning_speed = lTurn self.rTurning_speed = rTurn def execute(self): if self.started_driving: self.lMotor.setSpeed(-self.driving_speed + self.lTurning_speed * 0.2) self.rMotor.setSpeed(self.driving_speed + self.rTurning_speed * 0.2) else: self.lMotor.setSpeed(0) self.rMotor.setSpeed(0)
class Fork: fork_switch: wpilib.DigitalInput arm_motor: ctre.WPI_TalonSRX position = will_reset_to(0) last_position = 0 fork_switch_on = tunable(False) def execute(self): # TODO: the limit switch seems to be broken on the robot :( self.fork_switch_on = self.fork_switch.get() # if the limit switch is on and you last opened it, # and you are trying to open it #if self.fork_switch_on and self.position and self.last_position == self.position: # self.position = 0 #elif self.position != 0: # if nothing is being pressed - do this # self.last_position = self.position self.arm_motor.set(self.position) #self.arm_motor.set(0) # @timed_state(state_tm) def open(self): self.position = -1 def close(self): self.position = 1
class Drivetrain: shifter: wpilib.Solenoid train: wpilib.drive.DifferentialDrive forward_speed = will_reset_to(0) turn_speed = will_reset_to(0) kP = tunable(1) kI = tunable(0) kD = tunable(0) drive_multiplier = tunable(.7) turn_multiplier = tunable(.5) high_gear = tunable(False) currentSpeed = tunable(0) integral = tunable(0) previous_error = tunable(0) def execute(self): self.shifter.set(self.high_gear) self.train.arcadeDrive(self.forward_speed, self.turn_speed) def PID(self, targetSpeed): ''' Uses the current speed alongside the target speed to create a smooth ramping ''' error = self.currentSpeed - targetSpeed self.integral = self.integral + (error * .02) derivative = (error - self.previous_error) / .02 return self.kP * error + self.kI * self.integral + self.kD * derivative def shift_toggle(self): self.high_gear = not self.high_gear def move(self, speed, turn): self.forward_speed = -speed * self.drive_multiplier self.turn_speed = turn * self.turn_multiplier def movePID(self, speed, turn): self.forward_speed = self.PID(speed) self.turn_speed = turn
class Intake: intake_motor: VictorSP speed = will_reset_to(0) def spin(self, speed: float): self.speed = speed def execute(self): self.intake_motor.set(self.speed)
class Intake: intake_motor: WPI_VictorSPX intake_motor_lower: CANSparkMax intake_switch: wpilib.DigitalInput intake_solenoid: wpilib.DoubleSolenoid balls_collected = ntproperty("/components/intake/ballsCollected", 0) speed = will_reset_to(0.0) speed_lower = will_reset_to(0.0) solenoid_state = will_reset_to(wpilib.DoubleSolenoid.Value.kForward) previous_limit = False def spin(self, speed, speed_lower: float): self.spin_inside(speed) self.spin_lower(speed_lower) def spin_lower(self, speed_lower: float): self.speed_lower = speed_lower def spin_inside(self, speed: float): self.speed = speed def extend(self): self.set_solenoid(True) def retract(self): self.set_solenoid(False) def set_solenoid(self, extended: bool): self.solenoid_state = wpilib.DoubleSolenoid.Value.kReverse if extended else wpilib.DoubleSolenoid.Value.kForward def execute(self): self.intake_motor.set(self.speed) self.intake_motor_lower.set(self.speed_lower) if self.intake_switch.get(): if self.intake_switch.get() is not self.previous_limit: self.balls_collected += 1 self.previous_limit = self.intake_switch.get() self.intake_solenoid.set(self.solenoid_state)
class Intake: left_intake_motor: wpilib.Spark right_intake_motor: wpilib.Spark speed = will_reset_to(0) def set_speed(self, new_speed: float): self.speed = new_speed def execute(self): self.left_intake_motor.set(self.speed) self.right_intake_motor.set(self.speed)
class Intake: left_motor: ctre.WPI_TalonSRX right_motor: ctre.WPI_TalonSRX intake_speed = will_reset_to(0) kIntakeSpeed = tunable(.7) def execute(self): self.left_motor.set(self.intake_speed) self.right_motor.set(-self.intake_speed) def suck(self): self.intake_speed = -self.kIntakeSpeed def push(self): self.intake_speed = self.kIntakeSpeed
class Intake: intake_motor: ctre.WPI_TalonSRX speed = magicbot.will_reset_to(0) magnitude = magicbot.tunable(0.5) #TO DO: Find the correct directions for the intake and outake. def ballIn(self): self.speed = self.magnitude def ballOut(self): self.speed = -self.magnitude def execute(self): self.intake_motor.set(self.speed)
class Winch: """ Operate robot winch. """ winch_motors: wpilib.SpeedControllerGroup _climb_speed = will_reset_to(0) def run(self, speed: float = 1): """ Set the motor speed of each climbing motor. :param speed: The requested speed, between -1 and 1. """ self._climb_speed = speed def execute(self): """ Run climbing motors. """ self.winch_motors.set(self._climb_speed)
def __init__(self): # Current drive mode, this changes when a control calls its drive function self.drive_mode = will_reset_to(DriveMode.TANK) # Rotation, negative turns to the left, also known as z # Used for both self.rotation = will_reset_to(0) # Speed, positive is forward (joystick must be inverted) # Used for both self.y = will_reset_to(0) # Horizontal speed # Mecanum only self.x = will_reset_to(0) self.fod = will_reset_to(False) self.adjusted = will_reset_to(True)
class CargoManipulator: """ Operate cargo manipulation system. """ # cargo_intake_motors: wpilib.SpeedControllerGroup left_cargo_intake_motor: WPI_TalonSRX pull_speed = (0.5) light_pull_speed = (0.2) push_speed = (1.0) intake_speed = will_reset_to(0) def pull(self): """ """ self.intake_speed = -self.pull_speed def pull_lightly(self): """ Pull at a lower speed, to hold in ball. """ self.intake_speed = -self.light_pull_speed def push(self): """ Move lift downward. Used when controlling arm through buttons. """ self.intake_speed = self.push_speed def execute(self): """ Run elevator motors. """ # self.cargo_intake_motors.set(-self.intake_speed) self.left_cargo_intake_motor.set(self.intake_speed)
class Drive: """ Handle robot drivetrain. All drive interaction must go through this class. """ limelight: Limelight train: wpilib.drive.DifferentialDrive navx: navx.AHRS left_motors: wpilib.SpeedControllerGroup right_motors: wpilib.SpeedControllerGroup y = will_reset_to(0.0) rot = will_reset_to(0.0) aligning = will_reset_to(False) deadband = will_reset_to(0.1) auto = will_reset_to(False) left_voltage = will_reset_to(0) right_voltage = will_reset_to(0) speed_constant = will_reset_to(1.05) rotational_constant = will_reset_to(0.8) squared_inputs = False angle_p = ntproperty('/align/kp', 0.04) angle_i = ntproperty('/align/ki', 0) angle_d = ntproperty('/align/kd', 0.0) angle_reported = ntproperty('/align/angle', 0) angle_to = ntproperty('/align/angle_to', 0) def setup(self): """ Run setup code on the injected variables (train) """ self.angle_controller = PIDController(self.angle_p, self.angle_i, self.angle_d) self.angle_controller.setTolerance(2, 1) self.angle_controller.enableContinuousInput(0, 360) self.angle_setpoint = None self.calculated_pid = False def set_target(self, angle: float, relative=True): if relative and angle is not None: self.angle_setpoint = (self.angle + angle) % 360 self.angle_to = self.angle_setpoint else: self.angle_setpoint = angle if angle is not None: self.angle_controller.setSetpoint(self.angle_setpoint) else: self.calculated_pid = False self.angle_controller.reset() def align(self): self.aligning = True self.deadband = 0 def voltageDrive(self, left_voltage, right_voltage): self.left_voltage = left_voltage self.right_voltage = right_voltage self.auto = True self.deadband = 0 def move(self, y: float, rot: float): """ Move robot. :param y: Speed of motion in the y direction. [-1..1] :param rot: Speed of rotation. [-1..1] """ self.y = y self.rot = rot @property def target_locked(self): # self.logger.info(f'SET {self.angle_controller.getSetpoint()} ANG {self.navx.getAngle()}') return self.angle_controller.atSetpoint() and self.calculated_pid @property def angle(self): raw = self.navx.getAngle() while raw < 0: raw += 360 return raw % 360 def execute(self): """ Handle driving. """ self.train.setDeadband(self.deadband) self.angle_reported = self.angle if self.aligning and self.angle_setpoint is not None: self.right_motors.setInverted(False) if self.angle_controller.atSetpoint() and self.calculated_pid: self.train.arcadeDrive(0, 0, squareInputs=False) return # Use new network tables variables for testing self.angle_controller.setP(self.angle_p) self.angle_controller.setD(self.angle_d) self.angle_controller.setI(self.angle_i) output = self.angle_controller.calculate(self.angle) self.logger.info( f'Output: {output}, Error: {self.angle_controller.getPositionError()}' ) # Manual I-term zone (15 degrees) if abs(self.angle_controller.getPositionError()) <= 3: self.angle_controller.setI(self.angle_i) # Minumum and Maximum effect of integrator on output self.angle_controller.setIntegratorRange(-0.05, 0.05) else: self.angle_controller.setI(0) self.angle_controller.setIntegratorRange(0, 0) if output < 0: output = max(output, -0.35) else: output = min(output, 0.35) self.train.arcadeDrive(0, output, squareInputs=False) self.calculated_pid = True elif self.auto: self.right_motors.setInverted(True) self.right_motors.setVoltage(self.right_voltage) self.left_motors.setVoltage(self.left_voltage) else: self.right_motors.setInverted(False) self.train.arcadeDrive( self.speed_constant * self.y, self.rotational_constant * self.rot, squareInputs=self.squared_inputs, ) if not self.limelight.targetExists(): self.limelight.target_state = 0 elif self.target_locked: self.limelight.target_state = 2 else: self.limelight.target_state = 1
class ShooterController(StateMachine): """Statemachine for high level control of the shooter and injector""" VERBOSE_LOGGING = True chassis: Chassis indexer: Indexer shooter: Shooter turret: Turret target_estimator: TargetEstimator led_screen: LEDScreen range_finder: RangeFinder fire_command = will_reset_to(False) MAX_MISALIGNMENT = 0.2 # m from centre of target MIN_SCAN_PERIOD = 3.0 TARGET_LOST_TO_SCAN = 0.5 CAMERA_TO_LIDAR = 0.15 def __init__(self) -> None: super().__init__() self.spin_command = False self.distance = None self.time_of_last_scan: Optional[float] = None self.time_target_lost: Optional[float] = None self.disabled_flash: int = 0 self.fired_count: int = 0 def execute(self) -> None: super().execute() self.update_LED() def update_LED(self) -> None: # Flash if turret and shooter are disabled if self.shooter.disabled: if self.disabled_flash < 25: self.led_screen.set_bottom_row(255, 0, 0) self.led_screen.set_middle_row(255, 0, 0) self.led_screen.set_top_row(255, 0, 0) else: self.led_screen.set_bottom_row(0, 0, 0) self.led_screen.set_middle_row(0, 0, 0) self.led_screen.set_top_row(0, 0, 0) self.disabled_flash = (self.disabled_flash + 1) % 50 return if self.shooter.is_ready(): self.led_screen.set_bottom_row(0, 255, 0) else: self.led_screen.set_bottom_row(255, 0, 0) if self.indexer.is_ready(): self.led_screen.set_middle_row(0, 255, 0) else: self.led_screen.set_middle_row(255, 0, 0) if self.target_estimator.is_ready(): if self.aimed_at_target(): self.led_screen.set_top_row(0, 255, 0) else: self.led_screen.set_top_row(255, 128, 0) else: self.led_screen.set_top_row(255, 0, 0) @state(first=True) def startup(self, initial_call) -> None: """ Wait for the turret to complete it's inital slew before doing anything """ if initial_call: # target_angle = self.chassis.find_power_port_angle() self.turret.slew(0) if self.turret.is_ready(): self.next_state("searching") @state def searching(self) -> None: """ The vision system does not have a target, we try to find one using odometry """ if self.target_estimator.is_ready(): # print(f"searching -> tracking {self.vision.get_vision_data()}") self.time_target_lost = None self.next_state("tracking") else: # Scan starting straight downrange. time_now = time.monotonic() # Start a scan only if it's been a minimum time since we lost the target if (self.time_target_lost is None or (time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN): # Start a scan only if it's been a minimum time since we started # a scan. This allows the given scan to grow enough to find the # target so that we don't just start the scan over every cycle. if (self.time_of_last_scan is None or (time_now - self.time_of_last_scan) > self.MIN_SCAN_PERIOD): self.turret.scan(-self.chassis.get_heading()) self.time_of_last_scan = time_now # TODO test this # target_angle = self.chassis.angle_to_target() # self.turret.scan(target_angle) @state def tracking(self) -> None: """ Aiming towards a vision target and spinning up flywheels """ # collect data only once per loop if not self.target_estimator.is_ready(): self.time_target_lost = time.monotonic() self.next_state("searching") # print(f"tracking -> searching {self.vision.get_vision_data()}") else: target_data = self.target_estimator.get_data() if abs(target_data.angle) > self.find_allowable_angle(): # print(f"Telling turret to slew by {target_data.angle}") self.turret.slew(target_data.angle) if self.turret.is_ready(): self.shooter.set_range(target_data.distance) if self.ready_to_fire() and self.fire_command: self.next_state("firing") @state(must_finish=True) def firing(self, initial_call) -> None: """ Positioned to fire, inject and expel a single ball """ if initial_call: self.shooter.fire() self.fired_count += 1 elif not self.shooter.is_firing(): self.next_state("tracking") def fire_input(self) -> None: """ Called by robot.py to indicate the fire button has been pressed """ self.fire_command = True @feedback def ready_to_fire(self) -> bool: return (self.shooter.is_ready() and self.aimed_at_target() and self.indexer.is_ready() and self.target_estimator.is_ready() and self.turret.is_ready()) @feedback def aimed_at_target(self) -> bool: target_data = self.target_estimator.get_data() return abs(target_data.angle) < self.find_allowable_angle() def find_allowable_angle(self) -> float: """ Find the maximum angle by which the turret can be misaligned to still score a hit Currently does not consider angle from target, assumes we are at max distance """ # dist: the maximum value of our lookup table dist = self.shooter.ranges[-1] angle = math.atan(self.MAX_MISALIGNMENT / dist) return angle
class Drivetrain: motorr1: rev.CANSparkMax motorr2: rev.CANSparkMax motorl1: rev.CANSparkMax motorl2: rev.CANSparkMax using_vision = will_reset_to(False) vision_dist_kP = tunable(0.15) vision_dist_kI = tunable(0) vision_dist_kD = tunable(0) vision_turn_kP = tunable(0.055) vision_turn_kI = tunable(0.001) vision_turn_kD = tunable(0) vision_turn_integral_range = tunable(0.1) forward = will_reset_to(0) turn = will_reset_to(0) twist_power = will_reset_to(0) tank_drive: bool twist: bool vision_integral = 0 intake_is_front = True turn_multiplier = tunable(0.7) def vision_aim(self, x: float, y: float, aim_x=True, aim_y=True): self.using_vision = True # out = self.vision_pid.calculate(x) # print(out) self.vision_integral += x p = self.vision_turn_kP * x i = self.vision_turn_kI * self.vision_integral out = min(.25, max(p + i, -.25)) self.turn = out self.twist_power = self.turn if abs(x) < self.vision_turn_integral_range: self.vision_integral = 0 # self.forward = min(.5, max(self.vision_dist_kP * y, -.5)) def reset_integral(self): self.vision_integral = 0 def setup(self): self.motorr1.restoreFactoryDefaults() self.motorr2.restoreFactoryDefaults() self.motorl1.restoreFactoryDefaults() self.motorl2.restoreFactoryDefaults() self.motorr2.follow(self.motorr1) self.motorl2.follow(self.motorl1) self.drive = wpilib.drive.DifferentialDrive(self.motorl1, self.motorr1) # self.vision_pid = wpilib.controller.PIDController(0.006, 0.001, 0.0009) # self.vision_pid.setSetpoint(0) # self.vision_pid.calculate(0) # self.vision_pid.setTolerance(0.1) def arcadeDrive(self, forward, turn, twist): self.forward = forward * (-1 if self.intake_is_front else 1) self.turn = turn self.twist_power = twist def tankDrive(self, left, right): if self.intake_is_front: self.forward = left self.turn = right else: self.forward = right self.turn = left def execute(self): if not self.using_vision: self.reset_integral() if self.tank_drive: # Reusing forward and turn as left and right to reduce memory usage self.drive.tankDrive(self.forward, self.turn) else: if self.twist: self.drive.arcadeDrive(self.forward, self.twist_power, True) wpilib.SmartDashboard.putNumber("turnSpeed", self.twist_power) else: self.drive.arcadeDrive(self.forward, self.turn, True) wpilib.SmartDashboard.putNumber("turnSpeed", self.turn) wpilib.SmartDashboard.putNumber("forwardSpeed", self.forward)
class Chassis: left_rear: rev.CANSparkMax left_front: rev.CANSparkMax right_rear: rev.CANSparkMax right_front: rev.CANSparkMax imu: NavX open_loop = magicbot.tunable(False) vx = magicbot.will_reset_to(0.0) vz = magicbot.will_reset_to(0.0) def setup(self) -> None: self.left_front.setInverted(False) self.right_front.setInverted(True) self.disable_brake_mode() self.left_rear.follow(self.left_front) self.right_rear.follow(self.right_front) self.left_encoder = rev.CANEncoder(self.left_front) self.right_encoder = rev.CANEncoder(self.right_front) rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO for enc in (self.left_encoder, self.right_encoder): enc.setPositionConversionFactor(rev_to_m) enc.setVelocityConversionFactor(rev_to_m / 60) self.left_pid: rev.CANPIDController = self.left_front.getPIDController( ) self.right_pid: rev.CANPIDController = self.right_front.getPIDController( ) self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194, kV=2.79, kA=0.457) for pid in (self.left_pid, self.right_pid): # TODO: needs tuning pid.setP(5.19 / 10) pid.setI(0) pid.setD(0) pid.setIZone(0) pid.setFF(0) pid.setOutputRange(-1, 1) self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH) self.odometry = DifferentialDriveOdometry(self._get_heading()) # default_position on the field self.reset_odometry(Pose2d(-3, 0, Rotation2d(math.pi))) def execute(self) -> None: # XXX: https://github.com/robotpy/robotpy-wpilib/issues/635 chassis_speeds = ChassisSpeeds() chassis_speeds.vx = self.vx chassis_speeds.omega = self.vz speeds = self.kinematics.toWheelSpeeds(chassis_speeds) left_ff = self.ff_calculator.calculate(speeds.left) right_ff = self.ff_calculator.calculate(speeds.right) if self.open_loop: self.left_front.setVoltage(left_ff) self.right_front.setVoltage(right_ff) else: self.left_pid.setReference(speeds.left, rev.ControlType.kVelocity, arbFeedforward=left_ff) self.right_pid.setReference(speeds.right, rev.ControlType.kVelocity, arbFeedforward=right_ff) self.odometry.update( self._get_heading(), self.left_encoder.getPosition(), self.right_encoder.getPosition(), ) def drive(self, vx: float, vz: float) -> None: """Sets the desired robot velocity. Arguments: vx: Forwards linear velocity in m/s. vz: Angular velocity in rad/s, anticlockwise positive. """ self.vx = vx self.vz = vz def disable_closed_loop(self) -> None: """Run the drivetrain in open loop mode (feedforward only).""" self.open_loop = True def enable_closed_loop(self) -> None: """Run the drivetrain in velocity closed loop mode.""" self.open_loop = False def enable_brake_mode(self) -> None: for motor in ( self.left_front, self.left_rear, self.right_front, self.right_rear, ): motor.setIdleMode(rev.IdleMode.kBrake) def disable_brake_mode(self) -> None: for motor in ( self.left_front, self.left_rear, self.right_front, self.right_rear, ): motor.setIdleMode(rev.IdleMode.kCoast) def _get_heading(self) -> Rotation2d: """Get the current heading of the robot from the IMU, anticlockwise positive.""" return Rotation2d(self.imu.getYaw()) def get_pose(self) -> Pose2d: """Get the current position of the robot on the field.""" return self.odometry.getPose() @magicbot.feedback def get_heading(self) -> float: """Get the current heading of the robot.""" return self.get_pose().rotation().radians() @magicbot.feedback def get_left_velocity(self) -> float: return self.left_encoder.getVelocity() @magicbot.feedback def get_right_velocity(self) -> float: return self.right_encoder.getVelocity() def reset_odometry(self, pose: Pose2d) -> None: """Resets the odometry to start with the given pose. This is intended to be called at the start of autonomous. """ self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) self.odometry.resetPosition(pose, self._get_heading()) def find_field_angle(self, field_point: Translation2d) -> float: """Return the theoretical angle (in radians) to the target based on odometry""" pose = self.get_pose() rel = field_point - pose.translation() rel_heading = Rotation2d(rel.y, rel.x) + pose.rotation() return rel_heading.radians() def find_power_port_angle(self) -> float: return self.find_field_angle(POWER_PORT_POSITION)
class Shooter: limelight_state = will_reset_to(1) motor_rpm = will_reset_to(0) feeder_motor_speed = will_reset_to(0) # target_rpm = tunable(-3500) feed_speed_setpoint = tunable(-1) rpm_error = tunable(300) x_aim_error = tunable(1.2) y_aim_error = tunable(2) motor: PIDSparkMax feeder_motor: TalonSRX camera_state: int @property def target_rpm(self): _, y = self.aim() return -1 * (5.232 * pow(y, 2) - 70.76 * y + 3300) def setup(self): """ Runs right after the createObjects method is run. Sets up all the networktables values and configures the shooter motor PID values """ self.limelight = limelight.Limelight() self.log() # Shooter motor configuration self.motor.fromKu(0.0008, 0.6) # P = 0.03, I = 0.05, D = 0.125 self.motor.setFF(1 / 5880) # self.motor.setFF(0) # self.motor.setPID(.0008, 0, 0) # self.motor.setPID(0.0015, 0.008, 0.005) self.motor._motor_pid.setIZone(0.5) self.motor.motor.setSmartCurrentLimit(100) @feedback def aim(self) -> Tuple[float, float]: """ Will return the distances from the crosshair """ # self.limelight_state = 3 # self.camera_state = 0 x = self.limelight.horizontal_offset y = self.limelight.vertical_offset return (x, y) @property def is_aimed(self): """ Test whether the target is within a tolerance """ x, y = self.aim() if abs(x) > self.x_aim_error: return False # if abs(y) > self.y_aim_error: # return False return True @property def is_ready(self): """ Returns whether the current rpm of the motor is within a certain range, specified by the `rpm_error` property """ return (self.rpm_error + self.motor_rpm > self.motor.rpm > -self.rpm_error + self.motor_rpm) def shoot(self): """ Sets the shooter to start moving toward the setpoint """ self.motor_rpm = self.target_rpm def feed(self): """ Start the feeder to move the power cells towards the flywheel """ self.feeder_motor_speed = self.feed_speed_setpoint def backdrive(self): """ Start the feeder to move the power cells towards the flywheel """ self.feeder_motor_speed = 1 def execute(self): self.limelight.light(self.limelight_state) # self.limelight.pipeline(self.limelight_state) if abs(self.motor_rpm) < 200: self.motor.stop() else: self.motor.set(self.motor_rpm) if (abs(self.motor_rpm) > 500 and abs(self.motor.rpm) > abs(self.motor_rpm) - self.rpm_error ) or self.feeder_motor_speed: if not self.feeder_motor_speed: self.feed() self.feeder_motor.set(ControlMode.PercentOutput, self.feeder_motor_speed) else: self.feeder_motor.set(ControlMode.PercentOutput, 0) # self.feeder_motor.set(ControlMode.PercentOutput, self.feeder_motor_speed) self.log() def log(self): """ Get values relating to the shooter and post them to the dashboard for logging reasons. """ wpilib.SmartDashboard.putBoolean( "limelightLightState", True if self.limelight_state == 3 else False) wpilib.SmartDashboard.putBoolean("shooterReady", self.is_ready) # wpilib.SmartDashboard.putBoolean("isAimed", self.is_aimed) wpilib.SmartDashboard.putBoolean("targetsFound", self.limelight.valid_targets) wpilib.SmartDashboard.putNumber("shooterSpeedTarget", abs(self.motor_rpm)) wpilib.SmartDashboard.putNumber("shooterMotorSpeed", abs(self.motor.rpm)) wpilib.SmartDashboard.putNumber("shooterOutput", self.motor.motor.getAppliedOutput())
class ShooterController(StateMachine): """Statemachine for high level control of the shooter and injector""" VERBOSE_LOGGING = True chassis: Chassis indexer: Indexer shooter: Shooter turret: Turret target_estimator: TargetEstimator led_screen: LEDScreen range_finder: RangeFinder fire_command = will_reset_to(False) TARGET_RADIUS = (3 * 12 + 3.25) / 2 * 0.0254 # Circumscribing radius of target BALL_RADIUS = 7 / 2 * 0.0254 # convert from freedom units CENTRE_ACCURACY = ( 0.1 # maximum distance the centre of the ball will be from our target point ) TOTAL_RADIUS = BALL_RADIUS + CENTRE_ACCURACY OFFSET = TOTAL_RADIUS / math.sin(math.pi / 3) TRUE_TARGET_RADIUS = TARGET_RADIUS - OFFSET TARGET_POSITION = wpilib.geometry.Pose2d( 0, -2.404, wpilib.geometry.Rotation2d(math.pi)) # in field co ordinates MIN_SCAN_PERIOD = 3.0 TARGET_LOST_TO_SCAN = 0.5 def __init__(self) -> None: super().__init__() self.spin_command = False self.distance = None self.time_of_last_scan: Optional[float] = None self.time_target_lost: Optional[float] = None def execute(self) -> None: super().execute() self.update_LED() def update_LED(self) -> None: if self.shooter.is_ready(): self.led_screen.set_bottom_row(0, 255, 0) else: self.led_screen.set_bottom_row(255, 0, 0) if self.indexer.is_ready(): self.led_screen.set_middle_row(0, 255, 0) else: self.led_screen.set_middle_row(255, 0, 0) if self.target_estimator.is_ready(): if self.turret.is_ready(): self.led_screen.set_top_row(0, 255, 0) else: self.led_screen.set_top_row(255, 128, 0) else: self.led_screen.set_top_row(255, 0, 0) @state(first=True) def searching(self) -> None: """ The vision system does not have a target, we try to find one using odometry """ if self.target_estimator.is_ready(): # print(f"searching -> tracking {self.vision.get_vision_data()}") self.time_target_lost = None self.next_state("tracking") else: # Scan starting straight downrange. TODO: remove this if the above # seems worthwhile time_now = time.monotonic() # Start a scan only if it's been a minimum time since we lost the target if (self.time_target_lost is None or (time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN): # Start a scan only if it's been a minimum time since we started # a scan. This allows the given scan to grow enough to find the # target so that we don't just start the scan over every cycle. if (self.time_of_last_scan is None or (time_now - self.time_of_last_scan) > self.MIN_SCAN_PERIOD): self.turret.scan(-self.chassis.get_heading()) self.time_of_last_scan = time_now @state def tracking(self) -> None: """ Aiming towards a vision target and spinning up flywheels """ # collect data only once per loop if not self.target_estimator.is_ready(): self.time_target_lost = time.monotonic() self.next_state("searching") # print(f"tracking -> searching {self.vision.get_vision_data()}") else: target_data = self.target_estimator.get_data() # if abs(target_data.angle) > self.find_allowable_angle(target_data.distance): # print(f"Telling turret to slew by {target_data.angle}") self.turret.slew(target_data.angle) if self.turret.is_ready(): self.shooter.set_range(target_data.distance) if self.ready_to_fire() and self.fire_command: self.next_state("firing") @state(must_finish=True) def firing(self, initial_call) -> None: """ Positioned to fire, inject and expel a single ball """ if initial_call: self.shooter.fire() elif not self.shooter.is_firing(): self.next_state("tracking") def fire_input(self) -> None: """ Called by robot.py to indicate the fire button has been pressed """ self.fire_command = True @feedback def ready_to_fire(self) -> bool: return (self.shooter.is_ready() and self.turret.is_ready() and self.indexer.is_ready()) def find_allowable_angle(self, dist: float) -> float: """ Find the maximum angle by which the turret can be misaligned to still score a hit Currently does not consider angle from target dist: planar distance from the target """ dist = min(dist, 14.0) angle = math.atan(self.TRUE_TARGET_RADIUS / dist) # print(f"angle tolerance +- {angle} true target radius{self.TRUE_TARGET_RADIUS}") return angle
def setup(self): self.extend = will_reset_to(False) self.knee_speed = will_reset_to(0) self.drive_speed = will_reset_to(0)