class AcquireCube(Command): 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 initialize(self): self.state = 0 self.intake.set_arm_state(ArmState.DOWN) self.intake.set_grab_state(GrabState.OUT) self.timer.start() def execute(self): if self.state == 0: self.drive.arcade_drive(0, 0) if self.timer.get() > 0.2: self.state = 1 elif self.state == 1: self.intake.intake() self.drive.arcade_drive(self.drive_speed, 0) def isFinished(self): return self.intake.has_acquired_cube() or self.isTimedOut() def end(self): self.intake.set_grab_state(GrabState.IN) self.intake.set_arm_state(ArmState.UP) self.intake.run_intake(0) self.drive.arcade_drive(0, 0)
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, 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 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()
class MoveElevatorToInitialPosition(Command): 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 initialize(self): self._elevator.stop() self._smartDashboard.putString("ElevatorPosition", str(self._elevator.currentPosition())) self._timer.reset() self._timer.start() def execute(self): self._elevator.move(self._speed) self._smartDashboard.putString("ElevatorPosition", str(self._elevator.currentPosition())) def isFinished(self): if self._timer.get() > 1.0: return True else: return False def interrupted(self): self.end() def end(self): self._elevator.stop()
class MoveLiftOneBoxHeight(Command): def __init__(self, robot, d): super().__init__() self.robot = robot self.requires(robot.lift) self.total_timer = Timer() self.direction = d # Called just before this Command runs the first time def initialize(self): self.total_timer.reset() self.total_timer.start() # Called repeatedly when this Command is scheduled to run def execute(self): setpoint = self.robot.lift.getCurrentSetpoint() if self.direction == Direction.UP: setpoint = setpoint + robotmap.BOX_HEIGHT_INCHES elif self.direction == Direction.DOWN: setpoint = setpoint - robotmap.BOX_HEIGHT_INCHES self.robot.lift.setMotionMagicSetpoint(setpoint) # Make this return true when this Command no longer needs to run execute() def isFinished(self): return True
def __init__(self, robot, angle): super().__init__() self.requires(robot.drivetrain) self.robot = robot self.angle = angle self.timer = Timer()
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, d): super().__init__() self.robot = robot self.requires(robot.lift) self.m_setpoint = d #inches self.total_timer = Timer()
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
class AutoSetLiftPosition(Command): def __init__(self, robot, d): super().__init__() self.robot = robot self.requires(robot.lift) self.m_setpoint = d #inches self.total_timer = Timer() # Called just before this Command runs the first time def initialize(self): self.total_timer.reset() self.total_timer.start() self.robot.lift.setMotionMagicSetpoint(self.m_setpoint) # Make this return true when this Command no longer needs to run execute() def isFinished(self): returnval = False if self.total_timer.get() > 1.0: if self.robot.lift.isMotionMagicNearTarget(): returnval = True elif self.total_timer > 9.0: returnval = True return returnval # Called when another command which requires one or more of the same # subsystems is scheduled to run def interrupted(self): self.robot.lift.hold() self.robot.debug.Stop()
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
class PreciseDelay(object): """ Used to synchronize a timing loop. Usage: delay = PreciseDelay(time_to_delay) while something: # do things here delay.wait() """ 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 wait(self): """ Waits until the delay period has passed """ # we must *always* yield here, so other things can run Timer.delay(0.001) while not self.timer.hasPeriodPassed(self.delay_period): Timer.delay(0.001)
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
class Auto3(): CLAW_DEPLOY = 1 DRIVE_OFF_PLATFORM = 2 LIFT_DEPLOY = 3 def __init__(self, robot, logger): self.logger = logger self.robot = robot self.timer = Timer() def init(self): self.logger.info("Initializing auto 3") self.state = 0 self.timer.start() def iterate(self): if self.state == 0: self.state = 1 elif self.state == 1: self.state = 2 elif self.state == 2: self.state = 3 else: pass
def read(self, first_address, count): data = [first_address, count] data.append(crc7(data)) with self.mutex: retcount = self.port.write(data) if retcount != len(data): raise IOError("Write error (%s != %s)" % (retcount, len(data))) # FIXME if not hal.isSimulation(): Timer.delay(0.001) data = self.port.read(True, count + 1) if len(data) != count + 1: raise IOError("Read error (%s != %s)" % (len(data), count + 1)) crc = data[-1] data = data[:-1] if crc7(data) != crc: raise IOError("CRC error") return data
def __init__(self, robot): super().__init__() self.requires(robot.drivetrain) self.robot = robot self.pMotionProfiler = PFLJustDriveForward(robot) self.pTimer = Timer()
def read(self, first_address, count): data = [first_address, count] data.append(crc7(data)) with self.mutex: retcount = self.port.write(data) if retcount != len(data): raise IOError("Write error (%s != %s)" % (retcount, len(data))) # FIXME if not hal.isSimulation(): Timer.delay(0.001) data = self.port.read(True, count + 1) if len(data) != count + 1: raise IOError("Read error (%s != %s)" % (len(data), count+1)) crc = data[-1] data = data[:-1] if crc7(data) != crc: raise IOError("CRC error") return data
def move_arm_to_30(self): '''while(self.arm.getPOT() <= 30): self.arm.armAuto(1,0,30,rate=0.7) self.arm.armAuto(0,0,30) ''' Timer.delay(3) self.next_state('drive_forward_step_3')
def move_arm_to_0(self): '''while(self.arm.getPOT() >= 0.5): self.arm.armAuto(0,1,0,rate=0.5) self.arm.armAuto(0,0,0.5) ''' Timer.delay(3) self.next_state('drive_forward_step_2')
def timePassed(self, seconds): if self._timer <= 0: self._timer = Timer.getFPGATimestamp() if Timer.getFPGATimestamp() - self._timer >= seconds: self._timer = -1 return True else: return False
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 wait(self): """ Waits until the delay period has passed """ # we must *always* yield here, so other things can run Timer.delay(0.001) while not self.timer.hasPeriodPassed(self.delay_period): Timer.delay(0.001)
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 get(self, input): if input == self.last_input: time = Timer.getFPGATimestamp() if time - self.last_timestamp <= self.debounce_period: return -1 else: self.last_timestamp = time return input else: self.last_input = input self.last_timestamp = Timer.getFPGATimestamp() return input
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 initialize(self): """Called just before this Command runs the first time.""" self.start_time = round(Timer.getFPGATimestamp(), 1) print("\n" + f"** Started {self.getName()} at {self.start_time} s **", flush=True) SmartDashboard.putString( "alert", f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **" ) # note I made this a bit more complicated to demonstrate the flexibility of the button approach # two button approach if self.command == 'stop': self.robot.shooter.stop_flywheel() self.shooter_enabled = False elif self.command == 'spin': self.robot.shooter.set_flywheel(velocity=self.velocity) self.shooter_enabled = True else: # toggle button approach if self.shooter_enabled: self.robot.shooter.stop_flywheel() self.shooter_enabled = False else: self.robot.shooter.set_flywheel(velocity=self.velocity) self.shooter_enabled = True
def initialize(self): """Called just before this Command runs the first time.""" self.start_time = round( Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print( "\n" + f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **" ) SmartDashboard.putString( "alert", f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **" ) if self.source == 'dashboard': self.setpoint = SmartDashboard.getNumber('z_angle', 1) # may want to break if no valid setpoint is passed self.start_angle = self.robot.drivetrain.navx.getAngle() if self.absolute: # trust the navx to be correctly oriented to the field self.setpoint = self.setpoint - self.start_angle if self.setpoint > 180: self.setpoint = -(360 - self.setpoint) if self.setpoint < -180: self.setpoint = (360 + self.setpoint) self.error = 0 self.prev_error = 0
def get_output(self, current_input: float, setpoint: float) -> float: '''Get PID output for process current_input: The current PID input setpoint: Desired output of process/input to PID ''' # Current time in seconds current_time = Timer.getFPGATimestamp() # Time elapsed since last update time_change = current_time - self._previous_time # The current error current_error = self._get_continuous_error(setpoint - current_input) self._integral_term += self._coefs.i * (current_error * time_change) self._integral_term = clamp(self._integral_term, self._output_max, self._output_min) # Protect againsts ZeroDivisionError caused # by time resolution in simulator if time_change <= 0.0: time_change = 0.005 derivative = (current_input - self._previous_input) / time_change self._previous_input = current_input self._previous_time = current_time output = ((self._coefs.p * current_error) + self._integral_term + (self._coefs.d * derivative)) return clamp(output, self._output_max, self._output_min)
def searching(self) -> None: """ The vision system does not have a target, we try to find one using odometry """ if self.is_manual_aiming: self.next_state_now("manual_aiming") 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 = Timer.getFPGATimestamp() # 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
def stop( self ): current = Timer.getMsClock( ) self.offset = current - self.offset self.time = self.offset self.running = False
def start( self ): if self.running: return self.offset = Timer.getMsClock( ) self.running = True
def restart( self ): self.offset = Timer.getMsClock( ) self.time = 0.0 self.running = True
def __init__(self): self.debounce_period = 0.5 self.last_input = -1 self.last_timestamp = Timer.getFPGATimestamp()
def time(self): return Timer.getFPGATimestamp()
def getTimeMS( self ): current = Timer.getMsClock( ) current -= self.offset current += self.time return float( current )
def __init__(self, robot): super().__init__() self.robot = robot # Constants WHEEL_DIAMETER = 8 PI = 3.1415 ENCODER_TICK_COUNT_250 = 250 ENCODER_TICK_COUNT_360 = 360 ENCODER_GOAL = 0 # default ENCODER_TOLERANCE = 1 # inch0 self.RPM = 4320/10.7 self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415 self.CONTROL_TYPE = False # False = disable PID components self.LEFTFRONTCUMULATIVE = 0 self.LEFTBACKCUMULATIVE = 0 self.RIGHTFRONTCUMULATIVE = 0 self.RIGHTBACKCUMULATIVE = 0 self.rfmotor = CANTalon(0) self.rbmotor = CANTalon(1) self.lfmotor = CANTalon(2) self.lbmotor = CANTalon(3) self.lfmotor.reverseOutput(True) self.lbmotor.reverseOutput(True) #self.rfmotor.reverseOutput(True) #self.rbmotor.reverseOutput(True)#practice bot only self.rfmotor.enableBrakeMode(True) self.rbmotor.enableBrakeMode(True) self.lfmotor.enableBrakeMode(True) self.lbmotor.enableBrakeMode(True) absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lbmotor.setEncPosition(absolutePosition) absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lfmotor.setEncPosition(absolutePosition) absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rbmotor.setEncPosition(absolutePosition) absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rfmotor.setEncPosition(absolutePosition) self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) #setting up the distances per rotation self.lfmotor.configEncoderCodesPerRev(4096) self.rfmotor.configEncoderCodesPerRev(4096) self.lbmotor.configEncoderCodesPerRev(4096) self.rbmotor.configEncoderCodesPerRev(4096) self.lfmotor.setPID(0.0005, 0, 0.0, profile=0) self.rfmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.setPID(0.0005, 0, 0.0, profile=0) self.rbmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.configNominalOutputVoltage(+0.0, -0.0) self.lbmotor.configPeakOutputVoltage(+12.0, -12.0) self.lbmotor.setControlMode(CANTalon.ControlMode.Speed) self.lfmotor.configNominalOutputVoltage(+0.0, -0.0) self.lfmotor.configPeakOutputVoltage(+12.0, -12.0) self.lfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rbmotor.configNominalOutputVoltage(+0.0, -0.0) self.rbmotor.configPeakOutputVoltage(+12.0, -12.0) self.rbmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.configNominalOutputVoltage(+0.0, -0.0) self.rfmotor.configPeakOutputVoltage(+12.0, -12.0) self.rfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.setPosition(0) self.rbmotor.setPosition(0) self.lfmotor.setPosition(0) self.lbmotor.setPosition(0) self.lfmotor.reverseSensor(True) self.lbmotor.reverseSensor(True) ''' # changing the encoder output from DISTANCE to RATE (we're dumb) self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) # LiveWindow settings (Encoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder) ''' ''' # Checking the state of the encoders on the Smart Dashboard wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent) ''' if self.CONTROL_TYPE: # Initializing PID Controls self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02) self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02) self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02) self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02) # PID Absolute Tolerance Settings self.pidRightFront.setAbsoluteTolerance(0.05) self.pidLeftFront.setAbsoluteTolerance(0.05) self.pidRightBack.setAbsoluteTolerance(0.05) self.pidLeftBack.setAbsoluteTolerance(0.05) # PID Output Range Settings self.pidRightFront.setOutputRange(-1, 1) self.pidLeftFront.setOutputRange(-1, 1) self.pidRightBack.setOutputRange(-1, 1) self.pidLeftBack.setOutputRange(-1, 1) # Enable PID #self.enablePIDs() ''' # LiveWindow settings (PID) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack) ''' self.dashTimer = Timer() # Timer for SmartDashboard updating self.dashTimer.start() '''
class driveTrain(Component) : def __init__(self, robot): super().__init__() self.robot = robot # Constants WHEEL_DIAMETER = 8 PI = 3.1415 ENCODER_TICK_COUNT_250 = 250 ENCODER_TICK_COUNT_360 = 360 ENCODER_GOAL = 0 # default ENCODER_TOLERANCE = 1 # inch0 self.RPM = 4320/10.7 self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415 self.CONTROL_TYPE = False # False = disable PID components self.LEFTFRONTCUMULATIVE = 0 self.LEFTBACKCUMULATIVE = 0 self.RIGHTFRONTCUMULATIVE = 0 self.RIGHTBACKCUMULATIVE = 0 self.rfmotor = CANTalon(0) self.rbmotor = CANTalon(1) self.lfmotor = CANTalon(2) self.lbmotor = CANTalon(3) self.lfmotor.reverseOutput(True) self.lbmotor.reverseOutput(True) #self.rfmotor.reverseOutput(True) #self.rbmotor.reverseOutput(True)#practice bot only self.rfmotor.enableBrakeMode(True) self.rbmotor.enableBrakeMode(True) self.lfmotor.enableBrakeMode(True) self.lbmotor.enableBrakeMode(True) absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lbmotor.setEncPosition(absolutePosition) absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lfmotor.setEncPosition(absolutePosition) absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rbmotor.setEncPosition(absolutePosition) absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rfmotor.setEncPosition(absolutePosition) self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) #setting up the distances per rotation self.lfmotor.configEncoderCodesPerRev(4096) self.rfmotor.configEncoderCodesPerRev(4096) self.lbmotor.configEncoderCodesPerRev(4096) self.rbmotor.configEncoderCodesPerRev(4096) self.lfmotor.setPID(0.0005, 0, 0.0, profile=0) self.rfmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.setPID(0.0005, 0, 0.0, profile=0) self.rbmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.configNominalOutputVoltage(+0.0, -0.0) self.lbmotor.configPeakOutputVoltage(+12.0, -12.0) self.lbmotor.setControlMode(CANTalon.ControlMode.Speed) self.lfmotor.configNominalOutputVoltage(+0.0, -0.0) self.lfmotor.configPeakOutputVoltage(+12.0, -12.0) self.lfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rbmotor.configNominalOutputVoltage(+0.0, -0.0) self.rbmotor.configPeakOutputVoltage(+12.0, -12.0) self.rbmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.configNominalOutputVoltage(+0.0, -0.0) self.rfmotor.configPeakOutputVoltage(+12.0, -12.0) self.rfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.setPosition(0) self.rbmotor.setPosition(0) self.lfmotor.setPosition(0) self.lbmotor.setPosition(0) self.lfmotor.reverseSensor(True) self.lbmotor.reverseSensor(True) ''' # changing the encoder output from DISTANCE to RATE (we're dumb) self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) # LiveWindow settings (Encoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder) ''' ''' # Checking the state of the encoders on the Smart Dashboard wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent) ''' if self.CONTROL_TYPE: # Initializing PID Controls self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02) self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02) self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02) self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02) # PID Absolute Tolerance Settings self.pidRightFront.setAbsoluteTolerance(0.05) self.pidLeftFront.setAbsoluteTolerance(0.05) self.pidRightBack.setAbsoluteTolerance(0.05) self.pidLeftBack.setAbsoluteTolerance(0.05) # PID Output Range Settings self.pidRightFront.setOutputRange(-1, 1) self.pidLeftFront.setOutputRange(-1, 1) self.pidRightBack.setOutputRange(-1, 1) self.pidLeftBack.setOutputRange(-1, 1) # Enable PID #self.enablePIDs() ''' # LiveWindow settings (PID) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack) ''' self.dashTimer = Timer() # Timer for SmartDashboard updating self.dashTimer.start() ''' # Adding components to the LiveWindow (testing) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor) ''' def log(self): #The log method puts interesting information to the SmartDashboard. (like velocity information) ''' #no longer implemented because of change of hardware wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity()) ''' wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition()) wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition()) wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition()) wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition()) ''' wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57)) ''' # drive forward function def drive_forward(self, speed) : self.drive.tankDrive(speed, speed, True) # manual drive function for Tank Drive def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT): #self.lfmotor.setCloseLoopRampRate(1) #self.lbmotor.setCloseLoopRampRate(1) #self.rfmotor.setCloseLoopRampRate(1) #self.rbmotor.setCloseLoopRampRate(1) if (leftB == True): #Straight Button rightSpeed = leftSpeed if (rightB == True): #Slow Button #leftSpeed = leftSpeed/1.75 #rightSpeed = rightSpeed/1.75 if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning leftSpeed = leftSpeed/1.75 rightSpeed = rightSpeed/1.75 # Fast button if(rightT == True): #self.lfmotor.setCloseLoopRampRate(24) #self.lbmotor.setCloseLoopRampRate(24) #self.rfmotor.setCloseLoopRampRate(24) #self.rbmotor.setCloseLoopRampRate(24) leftSpeed = leftSpeed*(1.75) rightSpeed = rightSpeed*(1.75) if(leftT == True): leftSpeed = 0.1 rightSpeed = 0.1 # Creating margin for error when using the joysticks, as they're quite sensitive if abs(rightSpeed) < 0.04 : rightSpeed = 0 if abs(leftSpeed) < 0.04 : leftSpeed = 0 if self.CONTROL_TYPE: self.pidRightFront.setSetpoint(rightSpeed) self.pidRightBack.setSetpoint(rightSpeed) self.pidLeftFront.setSetpoint(leftSpeed) self.pidLeftBack.setSetpoint(leftSpeed) else: self.lfmotor.set(leftSpeed*512) self.rfmotor.set(rightSpeed*512) self.lbmotor.set(leftSpeed*512) self.rbmotor.set(rightSpeed*512) #autononmous tank drive (to remove a need for a slow, striaght, or fast button) def autonTankDrive(self, leftSpeed, rightSpeed): self.log() #self.drive.tankDrive(leftSpeed, rightSpeed, True) self.rfmotor.set(rightSpeed) self.rbmotor.set(rightSpeed*(-1)) self.lfmotor.set(leftSpeed) self.lbmotor.set(leftSpeed*(-1)) # stop function def drive_stop(self) : self.drive.tankDrive(0,0) # fucntion to reset the PID's and encoder values def reset(self): self.rfmotor.setPosition(0) self.rbmotor.setPosition(0) self.lfmotor.setPosition(0) self.lbmotor.setPosition(0) if self.CONTROL_TYPE: self.LEFTFRONTCUMULATIVE = 0 self.RIGHTFRONTCUMULATIVE = 0 self.LEFTBACKCUMULATIVE= 0 self.RIGHTBACKCUMULATIVE = 0 self.pidLeftBack.setSetpoint(0) self.pidLeftFront.setSetpoint(0) self.pidRightBack.setSetpoint(0) self.pidRightFront.setSetpoint(0) # def getDistance(self) # return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE))) def turn_angle(self, degrees): desired_inches = self.INCHES_PER_DEGREE * degrees if degrees < 0: while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches : self.autonTankDrive(0.4, -0.4) elif degrees > 0: while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches : self.autonTankDrive(-0.4, 0.4) # Enable PID Controllers def enablePIDs(self): ''' #No longer required because we swapped from analog encoders to magnetic encoders self.pidLeftFront.enable() self.pidLeftBack.enable() self.pidRightFront.enable() self.pidRightBack.enable() ''' # Disable PID Controllers def disablePIDs(self): ''' #see explaination above self.pidLeftFront.disable() self.pidLeftBack.disable() self.pidRightFront.disable() self.pidRightBack.disable() ''' def getAutonDistance(self): return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4 #detirmines how many ticks the encoder has processed def getMotorDistance(self, motor, cumulativeDistance): currentRollovers = 0 #number of times the encoder has gone from 1023 to 0 previousValue = cumulativeDistance #variable for comparison currentValue = motor.getEncPosition() #variable for comparison if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0 currentRollovers += 1 #notes the rollover return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks #converts ticks from getMotorDistance into inches def convertEncoderRaw(self, selectedEncoderValue): return selectedEncoderValue * self.INCHES_PER_REV