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 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 Cargo: current = tunable(0) smooth = tunable(0) alpha = tunable(0.92) inThreshold = tunable(11) cargo_intake_motor: ctre.WPI_TalonSRX def setup(self): self.speed = 0 self.kMin = 1 self.kMax = 4 # def shoot(self): self.speed = 1 def intake(self): self.speed = -1 def off(self): self.speed = 0 def isBallIn(self): return self.smooth > self.inThreshold def execute(self): self.cargo_intake_motor.set(self.speed) self.current = self.cargo_intake_motor.getOutputCurrent() self.smooth = (self.alpha * self.smooth) + (1 - self.alpha) * self.current
class FeederMap: """Simple map that holds the logic for running elements of the feeder.""" compatString = ["doof"] shooterMotors: ShooterMotorCreation xboxMap: XboxMap logger: logging loaderMotorSpeed = tunable(.4) intakeMotorSpeed = tunable(.7) def on_enable(self): pass # self.logger.setLevel(logging.DEBUG) def run(self, loaderFunc): """Called when execution of a feeder element is desired.""" if loaderFunc == Type.kIntake: if self.xboxMap.getMechRightTrig( ) > 0 and self.xboxMap.getMechLeftTrig() == 0: self.shooterMotors.runIntake(self.intakeMotorSpeed, Direction.kForwards) self.logger.debug("right trig intake", self.xboxMap.getMechRightTrig()) elif self.xboxMap.getMechLeftTrig( ) > 0 and self.xboxMap.getMechRightTrig() == 0: self.shooterMotors.runIntake(self.intakeMotorSpeed, Direction.kBackwards) self.logger.debug("left trig intake", self.xboxMap.getMechLeftTrig()) else: self.shooterMotors.stopIntake() if loaderFunc == Type.kLoader: if self.xboxMap.getMechRightTrig( ) > 0 and self.xboxMap.getMechLeftTrig() == 0: self.shooterMotors.runLoader(self.loaderMotorSpeed, Direction.kForwards) self.logger.debug("right trig manual", self.xboxMap.getMechRightTrig()) elif self.xboxMap.getMechLeftTrig( ) > 0 and self.xboxMap.getMechRightTrig() == 0: self.shooterMotors.runLoader(self.loaderMotorSpeed, Direction.kBackwards) self.logger.debug("left trig manual", self.xboxMap.getMechLeftTrig()) else: self.shooterMotors.stopLoader() def execute(self): pass
class CounterFSM(StateMachine): """ This state machine will continuously watch the current of the intake motor, watching to see if any power cells are being loaded, based off the spikes in current draw. If multiple cells are grabbed, it will likely only count it as one. If the intake is not running, the current will not be watched. """ intake: Intake ball_count: int balanced_zone = tunable(1) spike_zone = tunable(2) @state(first=True) def watch_intake_state(self): """ Watch the intake to wait for it to become enabled. Once enabled, wait for the motor controller to read a current deadzone that can be detected as the motor being at the correct speed. """ if abs(self.intake.intake_speed) > 0.2: self.next_state("intake_running_ignore_current") @state() def intake_running_ignore_current(self): """ Wait for the motor controller to read a current deadzone that can be detected as the motor being at the correct speed. If the intake is stopped partway through this, move back to the main state. """ if abs(self.intake.intake_speed) < 0.2: self.next_state("watch_intake_state") if self.intake.motor.getOutputCurrent() < self.balanced_zone: self.next_state("intake_running") @state() def intake_running(self): """ Increment the ball count if a spike in current is detected. It is very likely that when the motor spikes in current, a ball has touched the motors, causing the motor to slightly torque itself, drawing more current. If the intake is stopped partway through this, move back to the main state. """ if abs(self.intake.intake_speed) < 0.2: self.next_state("watch_intake_state") if self.intake.motor.getOutputCurrent() > self.spike_zone: self.ball_count += 1 self.next_state("intake_running_ingore_current")
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 Recorder: """ Record control input for playback as an autonomous mode. """ directory = tunable('/tmp') title = tunable('') frames = [] def start(self, voltage): """ Start a recording. :param voltage: Battery output voltage. Necessary for scaling speeds later on. """ self.voltage = voltage def capture(self, joysticks): """ Make snapshot of joystick inputs during this cycle. :param joysticks: Tuple of joysticks to read. """ self.frames.append({ 'joysticks': [{ 'axes': [joystick.getRawAxis(axs) for axs in range(joystick.getAxisCount())], # TODO: Buttons are one-indexed. Trying to interact with the 0th button will throw. 'buttons': [joystick.getRawButton(btn) for btn in range(joystick.getButtonCount())], 'pov': [joystick.getPOV(pov) for pov in range(joystick.getPOVCount())], } for joystick in joysticks] }) def stop(self): """ End recording and save recorded data to file. """ with open('{directory}/{filename}.json'.format( directory=self.directory, filename=self.title if self.title else int(time.time())), 'w+') as f: json.dump({ 'voltage': self.voltage, 'frames': self.frames, }, f) self.title = '' self.voltage = None self.frames = [] def execute(self): """ Run periodically when injected through MagicBot. """ pass
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 Arm: solenoid: wpilib.Solenoid arm_up = tunable(False) def execute(self): self.solenoid.set(self.arm_up) def toggle(self): ''' Toggles whether the arm is up or down, depending on its current position. ''' self.arm_up = not self.arm_up def toTop(self): ''' Makes the arm go up, no matter what. ''' self.arm_up = True def toBottom(self): ''' Makes the arm go down, no matter what. ''' self.arm_up = False
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 Forklift: winch_motor: ctre.WPI_TalonSRX position = 0 mode = 'pct' pct = 0 encoder = tunable(0) pid_p = tunable(0.9) set_p = None top_position = tunable(400) mid_position = tunable(45) low_position = tunable(0) def execute(self): if self.set_p is None or abs(self.set_p - self.pid_p) < 0.0001: self.winch_motor.config_kP(0, self.pid_p, 0) self.set_p = self.pid_p if self.mode == 'pct': self.winch_motor.set(self.pct) self.pct = 0 else: # Don't uncomment this until we test it! #self.winch_motor.set(WPI_TalonSRX.ControlMode.Position, self.position) pass self.encoder = self.winch_motor.getSensorCollection().getQuadraturePosition() def normal(self, v): self.mode = 'pct' self.pct = v def top(self): self.mode = 'pos' self.position = self.top_position def mid(self): self.mode = 'pos' self.position = self.mid_position def bot(self): self.mode = 'pos' self.position = self.low_position
class Ledstrip: blinkin: wpilib.Spark powerOut = tunable(0) def setMode(self, powerOut): self.powerOut = powerOut def execute(self): self.blinkin.set(self.powerOut)
class Elevator: motors_loader: dict downSpeed = tunable(-.4) upSpeed = tunable(.4) def on_enable(self): self.speed = 0 self.elevatorMotor = self.motors_loader['elevatorMotor'] print("Elevator Enabled") def setRaise(self): self.speed = self.upSpeed def setLower(self): self.speed = self.downSpeed def stop(self): self.speed = 0 def execute(self): self.elevatorMotor.set(self.speed)
class Ramp: is_practice_bot = DigitalInput solenoid = DoubleSolenoid motor = WPI_TalonSRX hold_position_controller = HoldPositionController speed = tunable(1.0) def setup(self): self.state = RampState.LOCKED self.pending_winch = WinchState.DISABLED self.motor.setInverted(True) def release(self): self.state = RampState.RELEASED def is_released(self): return self.state == RampState.RELEASED def lock(self): self.state = RampState.LOCKED def raise_ramp(self): self.pending_winch = WinchState.RAISING def lower_ramp(self): self.pending_winch = WinchState.LOWERING def execute(self): if self.is_practice_bot.get(): # No ramps on practice bot return # Winch if self.pending_winch == WinchState.RAISING: self.motor.set(self.speed) self.pending_winch = WinchState.DISABLED elif self.pending_winch == WinchState.LOWERING: self.motor.set(-self.speed) self.pending_winch = WinchState.DISABLED else: self.motor.set(0) # Solenoid lock if self.state == RampState.LOCKED: self.solenoid.set(DoubleSolenoid.Value.kReverse) elif self.state == RampState.RELEASED: self.solenoid.set(DoubleSolenoid.Value.kForward) def on_disabled(self): self.motor.set(0)
class YPosController(BasePIDComponent): drive = swervedrive.SwerveDrive tracker = position_tracker.PositionTracker kP = tunable(0.09) kI = tunable(0.0) kD = tunable(0.0) kF = tunable(0.0) kToleranceFeet = tunable(0.25) kIzone = tunable(0.25) def __init__(self): super().__init__(self.get_position, 'y_ctrl') self.set_abs_output_range(0.16, 0.8) def get_position(self): return self.tracker.get_y() / 1.0 def move_to(self, position): self.setpoint = position def is_at_location(self): return self.enabled and abs(self.get_position() - self.setpoint) < self.kToleranceFeet def execute(self): super().execute() if self.rate is not None: if self.is_at_location(): self.drive.set_raw_fwd(0) else: self.drive.set_raw_fwd(self.rate)
class AngleController: drivetrain: Drivetrain kP = tunable(default=0.2) kI = tunable(default=0) kD = tunable(default=0) def __init__(self): self.pid_output = 0 def setup(self): self.pid = wpilib.PIDController(self.kP, self.kI, self.kD, source=self.drivetrain.gyro, output=self) self.pid.setOutputRange(-0.4, 0.4) self.pid.setInputRange(-180, 180) self.pid.setContinuous() self.pid.setAbsoluteTolerance(2) def align_to(self, angle): self.pid.setSetpoint(angle) self.pid.enable() def is_enabled(self): return self.pid.enabled def reset_angle(self): self.drivetrain.gyro.reset() def pidWrite(self, output): self.pid_output = output def execute(self): if self.pid.enabled: if self.pid.onTarget(): self.pid.disable() else: self.drivetrain.move_x(self.pid_output)
class IRSensor: serial = SerialPort i2c = I2C threshold = tunable(300) def setup(self): self.activations = None self.angle = None self.displacement = None self.oriented = False self.saved_threshold = 300 def set_threshold(self): self.serial.writeString("t"+str(self.threshold)) return self.serial.readString(3)==str(self.threshold) def get_array_one(self): a = str(bin(0))[2:] b = str(bin(0))[2:] return a+'0'*(10-len(a))+b+'0'*(10-len(b)) def get_array_two(self): a = str(bin(0))[2:] b = str(bin(0))[2:] return a+'0'*(10-len(a))+b+'0'*(10-len(b)) def compute_orientation(self): c1 = [i for i,j in enumerate(self.activations[:16]) if int(j)] c1 = sum(c1)/(len(c1)+0.01) c2 = [i for i,j in enumerate(self.activations[16:]) if int(j)] c2 = sum(c2)/(len(c2)+0.01) self.displacement = 62.5 - (8*c1+8*c2)/2 self.angle = degrees(atan2(c1-62.5,IR_SEPARATION/2)) def isOriented(self): return self.oriented def execute(self): if self.saved_threshold != self.threshold: success = self.set_threshold() if success: self.saved_threshold = self.threshold else: self.threshold = self.saved_threshold self.activations = self.get_array_one()+self.get_array_two() self.compute_orientation() table.putValue('activations', self.activations) table.putValue('angle', self.activations) table.putValue('displacement', self.activations)
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 Hatch: lift_piston = wpilib.Solenoid hold_piston = wpilib.Solenoid is_holding = tunable(False) is_in_position = tunable(False) def lift(self): self.is_in_position = False def lower(self): self.is_in_position = True def hold(self): self.is_holding = True def release(self): self.is_holding = False def execute(self): self.lift_piston.set(self.is_in_position) self.hold_piston.set(self.is_holding)
class AngleController(PIDOutput): drive: Drive navx: navx.AHRS kP = tunable(0.002) kI = tunable(0.0) kD = tunable(0.0) rate = 0.0 def setup(self): self.pid_controller = wpilib.PIDController( 0.01, 0.0001, 0.0001, self.navx, self ) self.pid_controller.setInputRange(-180.0, 180.0) self.pid_controller.setOutputRange(-.75, .75) self.pid_controller.setContinuous() self.pid_controller.setAbsoluteTolerance(3) self.pid_controller.enable() def enable(self): self.pid_controller.enable() def disable(self): self.pid_controller.disable() def align_to(self, value: float): self.pid_controller.setSetpoint(value) def on_target(self): return self.pid_controller.onTarget() # PIDOutput def pidWrite(self, output: float): self.rate = output def execute(self): self.drive.drive(0, self.rate)
class Robot(magicbot.MagicRobot): drive: Drive time = tunable(0) voltage = tunable(0) rotation = tunable(0) def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( CANSparkMax(1, MotorType.kBrushless), CANSparkMax(2, MotorType.kBrushless), CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup( CANSparkMax(4, MotorType.kBrushless), CANSparkMax(5, MotorType.kBrushless), CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() def teleopInit(self): self.drive.squared_inputs = True self.drive.rotational_constant = 0.5 self.train.setDeadband(0.1) def teleopPeriodic(self): self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX())
class Elevator: USE_MOTIONMAGIC = True shooter_motor = WPI_TalonFX shooter_motor_slave = WPI_TalonFX kFreeSpeed = tunable(0.1) kZeroingSpeed = tunable(0.1) kP = tunable(0.3) kI = tunable(0.0) kD = tunable(0.0) kF = tunable(0.0) kCruiseVelocity = 30000 kAcceleration = 12000 setpoint = tunable(0) value = tunable(0) error = tunable(0)
class Lift: lift_master: CANTalon lift_encoder: common.encoder.BaseEncoder setpoint = tunable(0) def setup(self): self.manual_override = False self.manual_override_value = 0 self.pid_controller = wpilib.PIDController(0.022, 0.0, 0.0, self.lift_encoder, self.lift_master) self.pid_controller.setAbsoluteTolerance(0.5) self.pid_controller.setContinuous(False) self.pid_controller.setOutputRange(-.165, .6) self.pid_controller.enable() def set_setpoint(self, new_pos): self.setpoint = new_pos self.pid_controller.setSetpoint(new_pos) def get_setpoint(self) -> float: return self.setpoint def set_manual_override_value(self, new_value): self.manual_override_value = new_value def set_manual_override(self, override: bool): # Disable pid if manual override is being enabled if not self.manual_override and override: self.pid_controller.disable() # Renable pid and zero the encoders if manual override is being disabled if self.manual_override and not override: self.lift_encoder.zero() self.pid_controller.enable() self.manual_override = override def execute(self): if self.manual_override: self.lift_master.set(CANTalon.ControlMode.PercentOutput, self.manual_override_value) wpilib.SmartDashboard.putData("Lift Encoder", self.lift_encoder.encoder) wpilib.SmartDashboard.putBoolean("Lift Target", self.pid_controller.onTarget())
class TwoSteps(AutonomousStateMachine): MODE_NAME = 'Two Steps' DEFAULT = True drive = Drive drive_speed = tunable(-1) # @timed_state(duration=2, next_state='start_driving', first=True) # def dont_do_something(self): @timed_state(duration=5, first=True) def drive_forward(self): # '''This happens second''' self.drive.start_driving(1, 0, 0)
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 ScorpionLoader: compatString = ["scorpion"] motors_shooter: dict motors_loader: dict xboxMap: XboxMap shooterSpeed = tunable(-0.4) def setup(self): self.intakeSpeed = 0 self.loaderSpeed = 0 self.shooterSpeed = 0 self.loaderMotor = self.motors_loader["loaderMotor"] self.intakeMotor = self.motors_loader["intakeMotor"] self.logger.info("Shooter Motor Component Created") def runLoader(self, lSpeed): self.loaderSpeed = lSpeed self.loader = True def runIntake(self, iSpeed): self.intakeSpeed = iSpeed self.intake = True def stopIntake(self): self.intake = False def stopLoader(self): self.loader = False def isLoaderActive(self): return self.loader def checkController(self): self.runIntake(self.shooterSpeed if self.xboxMap.mechLeft < -.2 else 0) self.runLoader(self.xboxMap.mechRight) def execute(self): if self.intake: self.intakeMotor.set(self.intakeSpeed) elif self.intake == False: self.intakeMotor.set(0) if self.loader: self.loaderMotor.set(self.loaderSpeed) elif self.loader == False: self.loaderMotor.set(0)
class SideGear(AutonomousStateMachine): drivetrain = DriveTrain rotator = Rotator turn = tunable(.73) def on_enable(self): super().on_enable() self.p = wpilib.Timer() self.p.start() def execute(self): super().execute() if self.p.hasPeriodPassed(0.3): self.logger.info("Distance: %0.3f", self.drivetrain.getDistance()) @timed_state(duration=1.5, first=True, next_state='initialRotate') def drive_forward(self): self.drivetrain.move(-0.7, 0) @timed_state(duration=4, next_state='nextRotate') def initialRotate(self): self.drivetrain.move(0, self.sign * self.turn) self.rotator.rotateTotarget() if self.rotator.found: self.next_state('detect') @timed_state(duration=4, next_state='initialRotate') def nextRotate(self): self.drivetrain.move(0, self.sign * -self.turn) self.rotator.rotateTotarget() if self.rotator.found: self.next_state('detect') @timed_state(duration=3, next_state='drive_to_the_wall') def detect(self): self.drivetrain.move(0, self.sign * self.turn) self.rotator.rotateTotarget() if not self.rotator.found: self.next_state('initialRotate') @state def drive_to_the_wall(self): self.rotator.rotateTotarget() self.drivetrain.driveToWall()
class Straight(AutonomousStateMachine): MODE_NAME = 'Drive Straight' DEFAULT = True drivetrain = Drivetrain drive_speed = tunable(0.5) @timed_state(duration=2, first=True) def dont_do_something(self): '''This happens first''' pass @timed_state(duration=2) def do_something(self): '''This happens second''' self.drivetrain.driveStraight()
class TwoSteps(AutonomousStateMachine): MODE_NAME = "Two Steps" DEFAULT = True component2 = Component2 drive_speed = tunable(-1) @timed_state(duration=2, next_state="do_something", first=True) def dont_do_something(self): """This happens first""" pass @timed_state(duration=5) def do_something(self): """This happens second""" self.component2.do_something()