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