def __init__(self, distance, speed): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.odemetry = odemetry.Odemetry() self.speed = speed self.distance = distance
def __init__(self): super().__init__() self.drive = drive.Drive() self.distance = distance.DistanceSensor() self.distance.init() self.requires(self.drive) self.requires(self.distance)
class Robot(CommandBasedRobot): drive = drive.Drive() def robotInit(self): pass def disabledInit(self): pass def disabledPeriodic(self): pass def autonomousInit(self): autoCommand = driveTime.DriveTime(-1, 5) autoCommand.start() def autonomousPeriodic(self): Scheduler.getInstance().run() def teleopInit(self): pass def teleopPeriodic(self): Scheduler.getInstance().run() def testInit(self): pass def testPeriodic(self): pass
def __init__(self, allocentric=False): super().__init__() self.allocentric = allocentric self.drive = drive.Drive() self.requires(self.drive) self.drive.zeroSensors() self.odemetry = odemetry.Odemetry()
def robotInit(self): """Run when the robot turns on.""" field_styles = coloredlogs.DEFAULT_FIELD_STYLES field_styles['filename'] = {'color': 'cyan'} coloredlogs.install( fmt= "%(asctime)s[%(msecs)d] %(filename)s:%(lineno)d %(name)s %(levelname)s %(message)s", datefmt="%m-%d %H:%M:%S", field_styles=field_styles) # install to created handler Command.getRobot = lambda x=0: self # Update constants from json file on robot # if self.isReal(): # Constants.updateConstants() # Update constants for dashboard editing Constants.initSmartDashboard() # Initialize drive objects drive.Drive().init() # The PDP # self.pdp = PDP(7) # Set command group member variables self.autonomous = autogroup.AutonomousCommandGroup() self.disabled = disabledgroup.DisabledCommandGroup() self.disabled.setRunWhenDisabled(True) self.teleop = teleopgroup.TeleopCommandGroup() self.test = testgroup.TestCommandGroup() self.global_ = globalgroup.GlobalCommandGroup() self.global_.setRunWhenDisabled(True) # Start the camera sever CameraServer.launch() self.watchdog = watchdog.Watchdog(0.05, self._watchdogTimeout) self.globalInit()
def __init__(self, filename): super().__init__() self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.spline = hs.HermiteSpline(filename=filename) self.follower = purepursuit.PurePursuit(self.spline) self.follower.computeVelocities()
def execute(self): power = -math.pow(oi.OI().getJoystick().getY(), Constants.TANK_DRIVE_EXPONENT) rotation = -oi.OI().getJoystick().getZ() rotation = (rotation, 0)[abs(rotation) < 0.05] left = power - rotation right = power + rotation drive.Drive().setPercentOutput(left, right)
def __init__(self): super().__init__() self.drive = drive.Drive() self.longarm = longarm.LongArm() self.shortarm = shortarm.ShortArm() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.requires(self.longarm) self.requires(self.shortarm)
def __init__(self, left_velocity, right_velocity, timeout=1000): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.timer = wpilib.Timer() self.timeout = timeout self.left_velocity = left_velocity self.right_velocity = right_velocity
def __init__(self, radius, velocity, timeout): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.radius = radius self.velocity = velocity self.timeout = timeout self.timer = wpilib.Timer()
def __init__(self, controller): self.controller = controller self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.drivetrain = drivetrains.MecanumDrivetrain(x_wheelbase=units.inchesToFeet(Constants.X_WHEEL_BASE), y_wheelbase=units.inchesToFeet( Constants.Y_WHEEL_BASE), speed=units.inchesToFeet(Constants.THEORETICAL_MAX_VELOCITY)) self.starting_x = units.feetToInches( config.config_obj['pyfrc']['robot']['starting_x']) self.starting_y = units.feetToInches( config.config_obj['pyfrc']['robot']['starting_y'])
def __init__(self, allocentric=False): super().__init__() self.allocentric = allocentric self.drive = drive.Drive() self.requires(self.drive) self.drive.zeroSensors() self.odemetry = odemetry.Odemetry() self.multiplier = 1000 self.last_snap = 0 self.setInterruptible(True)
def __init__(self, setpoint, relative=False): super().__init__() self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.setpoint = units.degreesToRadians(setpoint) self.relative = relative self.timer = wpilib.Timer() self.timestamp = 0 self.last_timestamp = 0 self.cur_error = 0
def __init__(self, x_speed, y_speed, rotation, timeout=1): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.timer = wpilib.Timer() self.timeout = timeout self.x_speed = x_speed self.y_speed = y_speed self.rotation = rotation
def __init__(self, fl_signal, fr_signal, bl_signal, br_signal, timeout=1000): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.timer = wpilib.Timer() self.timeout = timeout self.fl_signal = fl_signal self.fr_signal = fr_signal self.bl_signal = bl_signal self.br_signal = br_signal
def __init__(self, controller): self.controller = controller self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.drivetrain = drivetrains.TwoMotorDrivetrain( x_wheelbase=units.inchesToFeet(Constants.WHEEL_BASE), speed=units.inchesToFeet(Constants.THEORETICAL_MAX_VELOCITY)) self.starting_x = units.feetToInches( config.config_obj['pyfrc']['robot']['starting_x']) self.starting_y = units.feetToInches( config.config_obj['pyfrc']['robot']['starting_y']) self.kl_encoder = Constants.DRIVE_ENCODER_TICKS_PER_REVOLUTION_LEFT / \ Constants.WHEEL_CIRCUMFERENCE self.kr_encoder = Constants.DRIVE_ENCODER_TICKS_PER_REVOLUTION_RIGHT / \ Constants.WHEEL_CIRCUMFERENCE
def __init__(self, setpoint): """Turn to setpoint (degrees).""" super().__init__() self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.setpoint = setpoint src = self.odemetry.pidgyro self.PID = PIDController(Constants.TURN_TO_ANGLE_KP, Constants.TURN_TO_ANGLE_KI, Constants.TURN_TO_ANGLE_KD, src, self._setMotors) self.PID.setInputRange(-180.0, 180.0) self.PID.setOutputRange(-0.7, 0.7) self.PID.setContinuous(True) self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE) self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def __init__(self): """Initilize the Odemetry class""" super().__init__() self.drive = drive.Drive() self.timestamp = 0 self.last_timestamp = 0 # Gyroscope self.gyro = adxrs450_gyro.ADXRS450_Gyro() self.gyro.calibrate() self.pose = pose.Pose() self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0 self.last_angle = 0
def __init__(self, setpoint, relative=False): super().__init__() self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.setpoint = units.degreesToRadians(setpoint) self.kp = Constants.TURN_TO_ANGLE_KP self.ki = Constants.TURN_TO_ANGLE_KI self.kd = Constants.TURN_TO_ANGLE_KD self.error_tolerance = Constants.TURN_TO_ANGLE_TOLERANCE self.timeout = Constants.TURN_TO_ANGLE_TIMEOUT self.controller = pid.PID( self.setpoint, self.kp, self.ki, self.kd, True, -180, 180) self.timestamp = 0 self.last_timestamp = 0 self.cur_error = 0 self.relative = relative self.timer = wpilib.Timer() self.timeout = 1000
def __init__(self, setpoint): """Turn to setpoint (degrees).""" super().__init__() self.drive = drive.Drive() self.odemetry = odemetry.Odemetry() self.requires(self.drive) self.setpoint = setpoint src = self.odemetry.pidgyro self.PID = PIDController(Constants.TURN_TO_ANGLE_KP, Constants.TURN_TO_ANGLE_KI, Constants.TURN_TO_ANGLE_KD, src, self._setMotors) logging.debug( "Turn to angle constructed with angle {}".format(setpoint)) self.PID.setInputRange(-180.0, 180.0) self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT, Constants.TURN_TO_ANGLE_MAX_OUTPUT) self.PID.setContinuous(True) self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE) self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def robotInit(self): Command.getRobot = lambda x=0: self """Run when the robot turns on.""" # Update constants from json file on robot if self.isReal(): Constants.updateConstants() # Initialize drive objects drive.Drive().init() # The PDP self.pdp = PDP(Constants.PDP_ID) # Robot odemetry command self.updateodemetry = updateodemetry.UpdateOdemetry() # Set command group member variables self.autonomous = autogroup.AutonomousCommandGroup() self.disabled = disabledgroup.DisabledCommandGroup() self.disabled.setRunWhenDisabled(True) self.teleop = teleopgroup.TeleopCommandGroup() self.test = testgroup.TestCommandGroup() # Start the camera sever CameraServer.launch()
def __init__(self): """Initilize the Odemetry class.""" super().__init__() self.drive = drive.Drive() self.timestamp = 0 self.last_timestamp = 0 self.dt = 0 # Gyroscope if hal.isSimulation(): self.gyro = analoggyro.AnalogGyro(0) self.pidgyro = pidanaloggyro.PIDAnalogGyro(self.gyro) else: self.gyro = PigeonIMU(intake.Intake().l_motor) self.pidgyro = pidpigeon.PIDPigeon(self.gyro) self.pose = pose.Pose() self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0 self.last_angle = 0
def __init__(self): """Initilize the Odemetry class.""" super().__init__() self.drive = drive.Drive() self.timestamp = 0 self.last_timestamp = 0 self.dt = 0 self.ir_motor = ctre.WPI_TalonSRX(Constants.IR_MOTOR_ID) # Gyroscope if hal.isSimulation(): self.gyro = analoggyro.AnalogGyro(0) else: self.gyro = PigeonIMU(self.ir_motor) self.calibrate() self.pose = pose.Pose() self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0 self.last_angle = 0
def robotInit(self): """Run when the robot turns on.""" field_styles = coloredlogs.DEFAULT_FIELD_STYLES field_styles['filename'] = {'color': 'cyan'} coloredlogs.install( level='WARN', fmt= "%(asctime)s[%(msecs)d] %(filename)s:%(lineno)d %(name)s %(levelname)s %(message)s", datefmt="%m-%d %H:%M:%S", field_styles=field_styles) # install to created handler Command.getRobot = lambda x=0: self # Update constants from json file on robot # if self.isReal(): # Constants.updateConstants() # Update constants for dashboard editing Constants.initSmartDashboard() # Initialize motor objects drive.Drive().init() longarm.LongArm().init() shortarm.ShortArm().init() intake.Intake().init() climbroller.ClimbRoller().init() # The PDP # self.pdp = PDP(7) # Set command group member variables self.autonomous = autogroup.AutonomousCommandGroup() self.disabled = disabledgroup.DisabledCommandGroup() self.disabled.setRunWhenDisabled(True) self.teleop = teleopgroup.TeleopCommandGroup() self.test = testgroup.TestCommandGroup() self.global_ = globalgroup.GlobalCommandGroup() self.global_.setRunWhenDisabled(True) self.watchdog = watchdog.Watchdog(Constants.WATCHDOG_TIMEOUT, self._watchdogTimeout) self.globalInit() LiveWindow.disableAllTelemetry()
def __init__(self): super().__init__() self.drive = drive.Drive() self.requires(self.drive)
def __init__(self): super().__init__() self.drive = drive.Drive() self.backarm = backarm.BackArm() self.requires(self.drive) self.requires(self.backarm)
def __init__(self, pov_port): super().__init__() self.drive = drive.Drive() self.pov_port = 0
def __init__(self): super().__init__() self.requires(drive.Drive()) drive.Drive().zeroSensors()
def __init__(self): super().__init__() self.drive = drive.Drive() self.requires(self.drive) self.vision = vision.Vision()