def init_vision(heading=0.0): t = time.monotonic() v = Vision() v.chassis = FakeChassis() # Inject some fake odometry v.odometry.appendleft(Odometry(0, 0, heading, t - 0.3)) v.odometry.appendleft(Odometry(1, 1, heading, t - 0.2)) v.odometry.appendleft(Odometry(2, 2, heading, t - 0.1)) v.odometry.appendleft(Odometry(3, 3, heading, t)) return v, t
def test_odom_deque_order(): v = Vision() v.chassis = FakeChassis() for i in range(5): time.sleep(0.05) v.execute() assert len(v.odometry) > 0 newer_odom = v.odometry.popleft() for odom in v.odometry: assert newer_odom.t > odom.t newer_odom = odom
def test_sample_images(): variables = ['x', 'y', 'w', 'h'] vision = Vision() with open('sample_img/tests.csv', 'r') as csvfile: # filename, x, y, w, h testreader = csv.reader(csvfile, delimiter=',') for sample in testreader: image = cv2.imread('sample_img/' + sample[0]) # Rescale if necessary scaled = cv2.resize(image, (Vision.video_width, Vision.video_height)) results = vision._vision_process.findTarget(scaled) yield find_target, sample[0], results[:-1], sample[1:], [0.05, 0.05] # Don't send the return image vision.free()
def test_nt_update(): v = Vision() nt = NetworkTable.getTable('vision') nt.putNumber('x', 0.5) assert v._values['x'] == 0.5 # Only updates to time should update the averaged values assert v.pidGet() == 0.0 nt.putNumber('time', 1234) # No change if width is zero assert v.pidGet() == 0.0 assert v.no_vision_counter == 1 nt.putNumber('w', 0.7) nt.putNumber('time', 1235) assert v.pidGet() != 0.0 assert v.no_vision_counter == 0
def createObjects(self): self.logger = logging.getLogger("robot") self.sd = NetworkTable.getTable('SmartDashboard') self.intake_motor = wpilib.CANTalon(14) self.shooter_motor = wpilib.CANTalon(12) self.defeater_motor = wpilib.CANTalon(1) self.joystick = wpilib.Joystick(0) self.gamepad = wpilib.Joystick(1) self.pressed_buttons_js = set() self.pressed_buttons_gp = set() # needs to be created here so we can pass it in to the PIDController self.bno055 = BNO055() self.vision = Vision() self.range_finder = RangeFinder(0) self.heading_hold_pid_output = BlankPIDOutput() Tu = 1.6 Ku = 0.6 Kp = Ku * 0.3 self.heading_hold_pid = wpilib.PIDController(0.6, 2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output) self.heading_hold_pid.setTolerance(3.0*math.pi/180.0) self.heading_hold_pid.setContinuous(True) self.heading_hold_pid.setInputRange(-math.pi, math.pi) self.heading_hold_pid.setOutputRange(-1.0, 1.0) self.intake_motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder) self.intake_motor.reverseSensor(False)
def createObjects(self): """Robot initialization function""" self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.chassis_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.chassis_right_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.indexer_motors = [ctre.WPI_TalonSRX(3)] self.indexer_switches = [wpilib.DigitalInput(9)] self.injector_slave_motor = ctre.WPI_TalonSRX(43) self.injector_slave_motor.follow(self.indexer_motors[0]) self.injector_slave_motor.setInverted(True) self.loading_piston = wpilib.Solenoid(0) self.shooter_centre_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.shooter_outer_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.colour_sensor = rev.color.ColorSensorV3(wpilib.I2C.Port.kOnboard) self.spinner_motor = wpilib.Spark(2) self.spinner_solenoid = wpilib.DoubleSolenoid(2, 3) self.turret_centre_index = wpilib.DigitalInput(1) self.turret_motor = ctre.WPI_TalonSRX(10) self.vision = Vision() # operator interface self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.spinner_joystick = wpilib.Joystick(2) self.turret_joystick = wpilib.Joystick(3)
def predict(self, timestep=1): """Predict what the measurement should be in the next timestep. :param timestep: the number of timesteps in the past that we are predicting forward *from*""" F = np.identity(self.state_vector_size) F[0][1] = self.loop_dt B = np.identity(self.state_vector_size) self.imu_deque.append(self.get_heading_state()) u = Vision.rad_to_vision_units(self.imu_deque[-timestep] - self.imu_deque[-timestep - 1]) self.filter.predict(F, u, B)
def createObjects(self): self.logger = logging.getLogger("robot") self.sd = NetworkTable.getTable('SmartDashboard') self.intake_motor = wpilib.CANTalon(14) self.shooter_motor = wpilib.CANTalon(12) self.defeater_motor = wpilib.CANTalon(1) self.joystick = wpilib.Joystick(0) self.gamepad = wpilib.Joystick(1) self.pressed_buttons_js = set() self.pressed_buttons_gp = set() # needs to be created here so we can pass it in to the PIDController self.bno055 = BNO055() self.vision = Vision() self.range_finder = RangeFinder(0) self.heading_hold_pid_output = BlankPIDOutput() Tu = 1.6 Ku = 0.6 Kp = Ku * 0.3 self.heading_hold_pid = wpilib.PIDController( 0.8, 0.0, 1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output) """self.heading_hold_pid = wpilib.PIDController(0.6, 2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output)""" self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0) self.heading_hold_pid.setContinuous(True) self.heading_hold_pid.setInputRange(-math.pi, math.pi) self.heading_hold_pid.setOutputRange(-0.2, 0.2) #self.heading_hold_pid.setOutputRange(-1.0, 1.0) self.intake_motor.setFeedbackDevice( wpilib.CANTalon.FeedbackDevice.QuadEncoder) self.intake_motor.reverseSensor(False) self.joystick_rate = 0.3
def get_players(constants, entities): players = [] for i in range(constants['player_count']): # Building the players fighter_component = Fighter(hp=30, defense=1, power=2) inventory_component = Inventory(26) level_component = Level() equipment_component = Equipment() vision_component = Vision(None, constants['fov_radius']) players.append( Entity(0, 0, '@', libtcod.blue, 'Player{0}'.format(i + 1), blocks=True, render_order=RenderOrder.ACTOR, fighter=fighter_component, inventory=inventory_component, level=level_component, equipment=equipment_component, vision=vision_component)) # Give the player a dagger to start with equippable_component = Equippable(EquipmentSlots.MAIN_HAND, power_bonus=2) dagger = Entity(0, 0, '-', libtcod.sky, 'Dagger', equippable=equippable_component) players[-1].inventory.add_item(dagger) players[-1].equipment.toggle_equip(dagger) entities.append(players[-1]) return players
class StrongholdRobot(magicbot.MagicRobot): chassis = Chassis intake = Intake shooter = Shooter defeater = Defeater boulder_automation = BoulderAutomation def createObjects(self): self.logger = logging.getLogger("robot") self.sd = NetworkTable.getTable('SmartDashboard') self.intake_motor = wpilib.CANTalon(14) self.shooter_motor = wpilib.CANTalon(12) self.defeater_motor = wpilib.CANTalon(1) self.joystick = wpilib.Joystick(0) self.gamepad = wpilib.Joystick(1) self.pressed_buttons_js = set() self.pressed_buttons_gp = set() # needs to be created here so we can pass it in to the PIDController self.bno055 = BNO055() self.vision = Vision() self.range_finder = RangeFinder(0) self.heading_hold_pid_output = BlankPIDOutput() Tu = 1.6 Ku = 0.6 Kp = Ku * 0.3 self.heading_hold_pid = wpilib.PIDController( 0.8, 0.0, 1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output) """self.heading_hold_pid = wpilib.PIDController(0.6, 2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output)""" self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0) self.heading_hold_pid.setContinuous(True) self.heading_hold_pid.setInputRange(-math.pi, math.pi) self.heading_hold_pid.setOutputRange(-0.2, 0.2) #self.heading_hold_pid.setOutputRange(-1.0, 1.0) self.intake_motor.setFeedbackDevice( wpilib.CANTalon.FeedbackDevice.QuadEncoder) self.intake_motor.reverseSensor(False) self.joystick_rate = 0.3 def putData(self): self.sd.putDouble("range_finder", self.range_finder.getDistance()) self.sd.putDouble("gyro", self.bno055.getHeading()) self.sd.putDouble("vision_pid_get", self.vision.pidGet()) self.sd.putDouble("vision_x", self.vision._values['x']) self.sd.putDouble("vision_w", self.vision._values['w']) self.sd.putDouble("vision_h", self.vision._values['h']) self.sd.putDouble("vx", self.chassis.vx) self.sd.putDouble("vy", self.chassis.vy) self.sd.putDouble("vz", self.chassis.vz) self.sd.putDouble("input_twist", self.chassis.inputs[2]) self.sd.putDouble("field_oriented", self.chassis.field_oriented) self.sd.putDouble("raw_yaw", self.bno055.getRawHeading()) self.sd.putDouble("raw_pitch", self.bno055.getPitch()) self.sd.putDouble("raw_roll", self.bno055.getRoll()) self.sd.putDouble("shooter_speed", -self.shooter.shooter_motor.getSetpoint() ) # minus sign here so +ve is shooting self.sd.putDouble("heading_pid_output", self.heading_hold_pid_output.output) self.sd.putDouble("heading_hold_pid_setpoint", self.heading_hold_pid.getSetpoint()) self.sd.putString("boulder_state", self.boulder_automation.current_state) self.sd.putDouble("intake_speed", self.intake.intake_motor.getSetpoint()) self.sd.putDouble("distance_pid_output", self.chassis.distance_pid_output.output) self.sd.putBoolean("track_vision", self.chassis.track_vision) self.sd.putDouble("pov", self.joystick.getPOV()) self.sd.putDouble("gyro_z_rate", self.bno055.getHeadingRate()) self.sd.putDouble( "heading_hold_error", self.heading_hold_pid.getSetpoint() - self.bno055.getAngle()) self.sd.putDouble("defeater_current", self.defeater_motor.getOutputCurrent()) self.sd.putDouble("defeater_speed", self.defeater_motor.get()) self.sd.putDouble("joystick_throttle", self.joystick.getThrottle()) self.sd.putDouble("range_pid_get", self.range_finder.pidGet()) self.sd.putDouble("encoder_distance", self.chassis.distance) distances = [] for module in self.chassis._modules.values(): distances.append( abs(module.distance) / module.drive_counts_per_metre) for key, distance in zip(self.chassis._modules.keys(), distances): self.sd.putDouble("encoder_motor_" + key, distance) def disabledInit(self): self.boulder_automation.done() def disabledPeriodic(self): """This function is called periodically when disabled.""" self.putData() def teleopInit(self): self.boulder_automation.done() def autoInit(self): self.boulder_automation.done() def teleopPeriodic(self): """This function is called periodically during operator control.""" try: if self.debounce(6, gamepad=True): self.boulder_automation.toggle_shoot_boulder() except: self.onException() try: if self.debounce(2) or self.debounce(1, gamepad=True): self.boulder_automation.toggle_intake_boulder() except: self.onException() try: if self.debounce(7): self.chassis.toggle_field_oriented() except: self.onException() try: if self.debounce(8): enabled = self.heading_hold_pid.isEnable() self.heading_hold_pid.disable() self.bno055.resetHeading() self.heading_hold_pid.setSetpoint( constrain_angle(self.bno055.getAngle())) self.heading_hold_pid.reset() if enabled: self.heading_hold_pid.enable() except: self.onException() """try: if self.debounce(10): self.chassis.toggle_vision_tracking() except: self.onException()""" try: if self.debounce(10): self.chassis.toggle_range_holding(self.chassis.correct_range) except: self.onException() try: if self.debounce(1) or self.debounce(8, gamepad=True): self.boulder_automation.toggle_shoot_boulder() except: self.onException() try: if self.debounce(9): self.chassis.toggle_heading_hold() except: self.onException() try: if self.debounce(4): self.defeater.up() except: self.onException() try: if self.debounce(5): self.shooter.stop() self.intake.stop() except: self.onException() try: if self.debounce(3): self.chassis.track_vision = True self.chassis.range_setpoint = self.chassis.correct_range self.chassis.distance_pid.enable() # self.shooter.start_shoot() except: self.onException() try: if self.debounce(6): self.defeater.down() except: self.onException() """try: if self.debounce(10): self.shooter.backdrive() self.intake.backdrive() except: self.onException()""" try: if self.joystick.getPOV() != -1: self.chassis.heading_hold = True direction = 0.0 if self.joystick.getPOV() == 0: # shooter centre goal direction = math.pi elif self.joystick.getPOV() == 90: # shooter right goal direction = math.pi / 3.0 + math.pi elif self.joystick.getPOV() == 270: # shooter left goal direction = -math.pi / 3.0 + math.pi elif self.joystick.getPOV() == 180: direction = 0.0 self.chassis.set_heading_setpoint(direction) except: self.onException() try: if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2): self.chassis.field_oriented = False else: self.chassis.field_oriented = True except: self.onException() try: if self.gamepad.getRawButton(3): self.boulder_automation.engage("backdrive_manual") elif self.boulder_automation.current_state == "backdrive_manual": self.boulder_automation.done() except: self.onException() """try: if self.debounce(1, gamepad=True): self.chassis.zero_encoders() self.chassis.distance_pid.setSetpoint(1.2) self.chassis.distance_pid.enable() except: self.onException()""" try: if self.debounce(10, gamepad=True): self.vision.write_image() except: self.onException() try: if self.joystick.getRawButton(12): self.joystick_rate = 0.6 else: self.joystick_rate = 0.4 except: self.onException() self.chassis.inputs = [ -rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2), -rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2), -rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=self.joystick_rate), (self.joystick.getThrottle() - 1.0) / -2.0 ] for input in self.chassis.inputs[0:3]: if input != 0.0: # Break out of auto if we move the stick self.chassis.distance_pid.disable() self.chassis.range_setpoint = None self.chassis.track_vision = False #self.chassis.field_oriented = True self.putData() def testPeriodic(self): """This function is called periodically during test mode.""" wpilib.LiveWindow.run() def debounce(self, button, gamepad=False): device = None if gamepad: pressed_buttons = self.pressed_buttons_gp device = self.gamepad else: pressed_buttons = self.pressed_buttons_js device = self.joystick if device.getRawButton(button): if button in pressed_buttons: return False else: pressed_buttons.add(button) return True else: pressed_buttons.discard(button) return False
class StrongholdRobot(magicbot.MagicRobot): chassis = Chassis intake = Intake shooter = Shooter defeater = Defeater def createObjects(self): self.logger = logging.getLogger("robot") self.sd = NetworkTable.getTable('SmartDashboard') self.intake_motor = wpilib.CANTalon(14) self.shooter_motor = wpilib.CANTalon(12) self.defeater_motor = wpilib.CANTalon(1) self.joystick = wpilib.Joystick(0) self.gamepad = wpilib.Joystick(1) self.pressed_buttons_js = set() self.pressed_buttons_gp = set() # needs to be created here so we can pass it in to the PIDController self.bno055 = BNO055() self.vision = Vision() self.range_finder = RangeFinder(0) self.heading_hold_pid_output = BlankPIDOutput() Tu = 1.6 Ku = 0.6 Kp = Ku * 0.3 self.heading_hold_pid = wpilib.PIDController(0.6, 2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0, self.bno055, self.heading_hold_pid_output) self.heading_hold_pid.setTolerance(3.0*math.pi/180.0) self.heading_hold_pid.setContinuous(True) self.heading_hold_pid.setInputRange(-math.pi, math.pi) self.heading_hold_pid.setOutputRange(-1.0, 1.0) self.intake_motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder) self.intake_motor.reverseSensor(False) def putData(self): self.sd.putDouble("range_finder", self.range_finder.getDistance()) self.sd.putDouble("gyro", self.bno055.getHeading()) vision_array = self.vision.get() vision_x = None vision_w = None vision_h = None if not vision_array: vision_x = -2 vision_w = -2 vision_h = -2 else: vision_x = vision_array[0] vision_w = vision_array[2] vision_h = vision_array[3] self.sd.putDouble("vision_pid_get", self.vision.pidGet()) self.sd.putDouble("vision_x", vision_x) self.sd.putDouble("vision_w", vision_w) self.sd.putDouble("vision_h", vision_h) self.sd.putDouble("vx", self.chassis.vx) self.sd.putDouble("vy", self.chassis.vy) self.sd.putDouble("vz", self.chassis.vz) self.sd.putDouble("input_twist", self.chassis.inputs[2]) self.sd.putDouble("field_oriented", self.chassis.field_oriented) self.sd.putDouble("raw_yaw", self.bno055.getRawHeading()) self.sd.putDouble("raw_pitch", self.bno055.getPitch()) self.sd.putDouble("raw_roll", self.bno055.getRoll()) self.sd.putDouble("shooter_speed", -self.shooter._speed) # minus sign here so +ve is shooting self.sd.putDouble("heading_pid_output", self.heading_hold_pid_output.output) self.sd.putDouble("heading_hold_pid_setpoint", self.heading_hold_pid.getSetpoint()) self.sd.putDouble("intake_state", self.intake.state) self.sd.putDouble("intake_speed", self.intake._speed) self.sd.putDouble("distance_pid_output", self.chassis.distance_pid_output.output) self.sd.putBoolean("track_vision", self.chassis.track_vision) self.sd.putDouble("pov", self.joystick.getPOV()) self.sd.putDouble("gyro_z_rate", self.bno055.getHeadingRate()) self.sd.putDouble("heading_hold_error", self.heading_hold_pid.getSetpoint()-self.bno055.getAngle()) self.sd.putDouble("defeater_current", self.defeater_motor.getOutputCurrent()) self.sd.putDouble("defeater_speed", self.defeater_motor.get()) self.sd.putDouble("joystick_throttle", self.joystick.getThrottle()) self.sd.putDouble("range_pid_get", self.range_finder.pidGet()) self.sd.putDouble("encoder_distance", self.chassis.distance) def disabledInit(self): pass def disabledPeriodic(self): """This function is called periodically when disabled.""" self.putData() def teleopInit(self): pass def teleopPeriodic(self): """This function is called periodically during operator control.""" try: if self.debounce(2) or self.debounce(1, gamepad=True): self.intake.toggle() except: self.onException() try: if self.debounce(7): self.chassis.toggle_field_oriented() except: self.onException() try: if self.debounce(8): enabled = self.heading_hold_pid.isEnable() self.heading_hold_pid.disable() self.bno055.resetHeading() self.heading_hold_pid.setSetpoint(constrain_angle(self.bno055.getAngle())) self.heading_hold_pid.reset() if enabled: self.heading_hold_pid.enable() except: self.onException() try: if self.debounce(10): self.chassis.toggle_vision_tracking() except: self.onException() try: if self.debounce(12): self.chassis.toggle_range_holding(self.chassis.correct_range) except: self.onException() try: if self.debounce(1): if self.shooter.state == shooter.States.off: self.shooter.start_shoot() else: self.shooter.toggle() self.intake.fire() except: self.onException() try: if self.debounce(9): self.chassis.toggle_heading_hold() except: self.onException() try: if self.debounce(4): self.defeater.up() except: self.onException() try: if self.debounce(5): self.shooter.stop() self.intake.stop() except: self.onException() try: if self.debounce(3): self.chassis.track_vision = True self.chassis.range_setpoint = self.chassis.correct_range self.chassis.distance_pid.enable() # self.shooter.start_shoot() except: self.onException() try: if self.debounce(6): self.defeater.down() except: self.onException() """try: if self.debounce(10): self.shooter.backdrive() self.intake.backdrive() except: self.onException()""" try: if self.joystick.getPOV() != -1: self.chassis.heading_hold = True direction = 0.0 if self.joystick.getPOV() == 0: # shooter centre goal direction = math.pi elif self.joystick.getPOV() == 90: # shooter right goal direction = math.pi/3.0+math.pi elif self.joystick.getPOV() == 270: # shooter left goal direction = -math.pi/3.0+math.pi elif self.joystick.getPOV() == 180: direction = 0.0 self.chassis.set_heading_setpoint(direction) except: self.onException() try: if self.joystick.getRawButton(11) or self.gamepad.getRawButton(2): self.chassis.field_oriented = False else: self.chassis.field_oriented = True except: self.onException() try: if self.gamepad.getRawButton(3): self.intake.backdrive_slow() except: self.onException() try: if self.debounce(1, gamepad=True): self.chassis.zero_encoders() self.chassis.distance_pid.setSetpoint(1.2) self.chassis.distance_pid.enable() except: self.onException() try: if self.gamepad.getRawButton(4): self.shooter.backdrive_recovery() except: self.onException() self.chassis.inputs = [-rescale_js(self.joystick.getY(), deadzone=0.05, exponential=1.2), - rescale_js(self.joystick.getX(), deadzone=0.05, exponential=1.2), - rescale_js(self.joystick.getZ(), deadzone=0.2, exponential=15.0, rate=0.3), (self.joystick.getThrottle() - 1.0) / -2.0 ] for input in self.chassis.inputs[0:3]: if input != 0.0: # Break out of auto if we move the stick self.chassis.distance_pid.disable() self.chassis.range_setpoint = None self.chassis.track_vision = False self.chassis.field_oriented = True self.putData() def testPeriodic(self): """This function is called periodically during test mode.""" wpilib.LiveWindow.run() def debounce(self, button, gamepad = False): device = None if gamepad: pressed_buttons = self.pressed_buttons_gp device = self.gamepad else: pressed_buttons = self.pressed_buttons_js device = self.joystick if device.getRawButton(button): if button in pressed_buttons: return False else: pressed_buttons.add(button) return True else: pressed_buttons.discard(button) return False