def iterate(self, robot_mode, pilot_stick, copilot_stick): if robot_mode == RobotMode.TEST: self.test_mode() return if pilot_stick.LeftBumper().get(): self.claw_close.set(False) self.claw_open.set(True) elif pilot_stick.RightBumper().get(): self.claw_close.set(True) self.claw_open.set(False) if pilot_stick.A().get(): self.shoot_ball() #Do we need a boolean to look for pressed edge? if pilot_stick.DpadUp().get(): self.stow_claw() #Do we need a boolean to look for pressed edge? if pilot_stick.DpadDown().get(): self.deploy_claw() self.iterate_state_machine() SmartDashboard.putString("Claw State", self.current_state.name)
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 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 iterate(self, robot_mode, pilot_stick, copilot_stick): pilot_x = pilot_stick.LeftStickX() pilot_y = pilot_stick.LeftStickY() if abs(pilot_x) > 0 or abs(pilot_y) > 0: self.current_state = DriveState.OPERATOR_CONTROL self.drive.arcadeDrive(pilot_x, pilot_y, False) else: if self.current_state == DriveState.FOLLOW_PATH: self.process_auto_path() if pilot_stick.X().get(): self.drive_front_extend.set(True) self.drive_front_retract.set(False) self.drive_back_extend.set(False) self.drive_back_retract.set(True) elif pilot_stick.Y().get(): self.drive_front_extend.set(False) self.drive_front_retract.set(True) self.drive_back_extend.set(True) self.drive_back_retract.set(False) elif pilot_stick.B().get(): self.drive_front_extend.set(False) self.drive_front_retract.set(True) self.drive_back_extend.set(False) self.drive_back_retract.set(True) SmartDashboard.putString("Drive State", self.current_state.name)
def change_PIDs(self, factor=1, dict_0=None, dict_1=None): """Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set slot 0 (position) or slot 1 (velocity) """ keys = ['kP', 'kI', 'kD', 'kIz', 'kFF'] for key in keys: if dict_0 == None: self.PID_dict_pos[key] *= factor else: self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier SmartDashboard.putString( "alert", f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}" ) if dict_1 == None: self.PID_dict_vel[key] *= factor else: self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier SmartDashboard.putString( "alert", f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}" ) self.configure_controllers(pid_only=True) self.display_PIDs()
def periodic(self) -> None: """Perform odometry and update dash with telemetry""" self.counter += 1 self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()), self.l_encoder.getPosition(), -self.r_encoder.getPosition()) if self.counter % 10 == 0: # start keeping track of where the robot is with an x and y position (only good for WCD)' pose = self.get_pose() SmartDashboard.putString( 'drive_pose', f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]' ) SmartDashboard.putString( 'drive_encoders LR', f'[{self.get_position(self.l_encoder):2.3f}, {self.get_position(self.r_encoder):2.2f}]' ) SmartDashboard.putString('drive_heading', f'[{-self.navx.getAngle():2.3f}]') if self.counter % 100 == 0: pass # self.display_PIDs() msg = f"Positions: ({self.l_encoder.getPosition():2.2f}, {self.r_encoder.getPosition():2.2}, {self.navx.getAngle():2.2})" msg = msg + f" Rates: ({self.l_encoder.getVelocity():2.2f}, {self.r_encoder.getVelocity():2.2f}) Time: {Timer.getFPGATimestamp() - self.robot.enabled_time:2.1f}" SmartDashboard.putString("alert", msg) SmartDashboard.putString("sparks", str(self.error_dict))
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()} 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 **") self.robot.shooter.set_flywheel(self.velocity) self.robot.shooter.set_hood_setpoint(self.angle)
def end(self, message='Ended') -> None: """Called once after isFinished returns true""" self.robot.drivetrain.stop() print( f"** {message} {self.getName()} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **" ) SmartDashboard.putString('/Vision/TestKey', 'ended')
def end(self): """Called once after isFinished returns true""" end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print("\n" + f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **") SmartDashboard.putString("alert", f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **") for key in self.telemetry: SmartDashboard.putNumberArray("telemetry_" + str(key), self.telemetry[key]) self.robot.drivetrain.stop()
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 **" )
def end(self, message='Ended'): """Called once after isFinished returns true""" end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print( f"** {message} {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **" ) SmartDashboard.putString( "alert", f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **" ) self.robot.drivetrain.stop()
def autonomousInit(self): #self.m_autoSelected = self.m_autoChooser.GetSelected() self.m_autoSelected = KAUTONAME_CUSTOM SmartDashboard.putString("Auto Selected", self.m_autoSelected) print("Auto selected: " + self.m_autoSelected) if (self.m_autoSelected == KAUTONAME_CUSTOM): # Custom Auto goes here print("Auto selected: " + "Custom Autonomous") else: # Default Auto goes here print("Auto selected: " + "Default Autonomous")
def periodic(self) -> None: """Perform odometry and update dash with telemetry""" self.counter += 1 self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()), self.l_encoder.getDistance(), -self.r_encoder.getDistance()) if self.counter % 10 == 0: # start keeping track of where the robot is with an x and y position (only good for WCD)' pose = self.get_pose() SmartDashboard.putString('drive_pose', f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]' ) if self.counter % 100 == 0: pass
def autonomousPeriodic(self): self.m_autoSelected = KAUTONAME_CUSTOM if (self.m_autoSelected == KAUTONAME_CUSTOM): # Custom Auto goes here print("Auto selected: " + "Custom Autonomous") else: # Default Auto goes here print("Auto selected: " + "Default Autonomous") SmartDashboard.putString("Auto Selected", self.m_autoSelected) wpilib.SmartDashboard.putNumber("YawAngle", self.m_imu.getAngle())
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.name} with input {self.state} at {self.start_time} s **", flush=True) SmartDashboard.putString( "alert", f"** Started {self.name} with input {self.state} at {self.start_time} s **" ) self.heading = self.robot.navigation.get_angle()
def iterate(self, robot_mode, pilot_stick, copilot_stick): if robot_mode == RobotMode.TEST: self.test_mode() return if pilot_stick.Back().get(): self.stow_harpoon() if pilot_stick.Start().get(): self.deploy_harpoon() self.iterate_state_machine() SmartDashboard.putString("Harpoon State", self.current_state.name) SmartDashboard.putNumber("IRVolts", self.ir_harpoon.getVoltage())
def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick): self.robot_mode = robot_mode lift_position = self.lift_talon.getAnalogInRaw() if lift_position > self.lift_stow_position + 5: SmartDashboard.putBoolean("Creep", True) else: SmartDashboard.putBoolean("Creep", False) if robot_mode == RobotMode.TEST: if self.test_lift_pneumatic.getSelected() == 1: self.lift_pneumatic_extend.set(False) self.lift_pneumatic_retract.set(True) else: self.lift_pneumatic_extend.set(True) self.lift_pneumatic_retract.set(False) #need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range if lift_position > self.min_lift_position or lift_position < self.max_lift_position: self.lift_talon.set( ControlMode.PercentOutput, -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS)) elif lift_position < self.min_lift_position: #allow upward motion self.lift_talon.set( ControlMode.PercentOutput, max( -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0)) elif lift_position > self.max_lift_position: #allow downward motion self.lift_talon.set( ControlMode.PercentOutput, min( -1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0)) else: self.lift_talon.set(ControlMode.PercentOutput, 0.0) else: self.iterate_state_machine(pilot_stick, copilot_stick) SmartDashboard.putString("Lift State", self.current_state.name) SmartDashboard.putString("Lift Preset", self.current_lift_preset.name) SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint) SmartDashboard.putNumber("Lift Position", lift_position)
def initialize(self): """Called just before this Command runs the first time.""" if self.from_dashboard: self.setpoint = SmartDashboard.getNumber('Auto Rotation', 0) self.start_time = round( Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print( "\n" + f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **" ) SmartDashboard.putString( "alert", f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **" ) self.start_angle = self.robot.navigation.get_angle() self.error = 0 self.prev_error = 0
def initialize(self): """Called just before this Command runs the first time.""" if self.from_dashboard: self.setpoint = SmartDashboard.getNumber('Auto Distance',0) self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print("\n" + f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **") SmartDashboard.putString("alert", f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **") if self.control_type == 'position': self.robot.drivetrain.goToSetPoint(self.setpoint) elif self.control_type == 'velocity': self.robot.drivetrain.set_velocity(self.setpoint) else: pass self.has_arrived = False self.telemetry = {'time':[], 'position':[], 'velocity':[], 'current':[], 'output':[]} self.counter = 0 self.extra_time = 0.5 # amount of time to give it to slow down after we reach setpoint self.setTimeout(self.initial_timeout) # needs to be in initialize because we modify it in execute - dashboard instance keeps getting reused
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()} with enabling as {not self.shooter_enabled} 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 **" ) # toggle button approach if self.shooter_enabled: self.robot.shooter.stop_flywheel() self.shooter_enabled = False else: self.robot.shooter.set_flywheel(velocity=self.omega) self.shooter_enabled = True
def dashboardPeriodic(self): #self.wristUp = self.getNumber("WristUpSpeed" , 0.5) #self.wristDown = self.getNumber("WristDownSpeed" , 0.2) #self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) #self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) #self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain) #self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putData("PID Controller", self.cargoController) SmartDashboard.putNumber("Wrist Angle", self.getAngle()) SmartDashboard.putNumber("Wrist Power", self.input) SmartDashboard.putNumber("Wrist Power2", self.input2) SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand) SmartDashboard.putBoolean("Wrist PinState Quad A", self.wrist.getPinStateQuadA()) SmartDashboard.putBoolean("Wrist PinState Quad B", self.wrist.getPinStateQuadB()) self.F = SmartDashboard.getNumber("F Gain", 0)
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()} at {self.start_time} s **") SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time} s **") if self.use_dash: self.kp = SmartDashboard.getNumber('kp_vel', self.kp) self.kd = SmartDashboard.getNumber('kd_vel', self.kd) self.left_speed_setpoint = SmartDashboard.getNumber('l_vel', self.left_speed_setpoint) self.right_speed_setpoint = SmartDashboard.getNumber('r_vel', self.right_speed_setpoint) self.left_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02) self.right_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02) self.previous_time = -1 self.left_controller.reset() self.right_controller.reset()
def end(self, message='Ended'): """Called once after isFinished returns true""" end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print(f"** {message} {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **") SmartDashboard.putString("alert", f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **") self.robot.drivetrain.stop() self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry) if self.write_telemetry: location = Path.cwd() if self.robot.isSimulation() else Path('/home/lvuser/py/') # it's not called robot on the robot timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") file_name = timestamp + '_'+ self.course + f'_kpv_{self.kp_vel:2.1f}'.replace('.','p') + f'_vel_{str(self.velocity).replace(".","p")}' +'.pkl' pickle_file = location / 'sim' / 'data' / file_name with open(pickle_file.absolute(), 'wb') as fp: out_dict = {'TIMESTAMP':timestamp,'DATA':self.telemetry, 'COURSE':self.course, 'VELOCITY':self.velocity, 'KP_VEL':self.kp_vel, 'KD_VEL':self.kd_vel, 'BETA':self.beta, 'ZETA':self.zeta} pickle.dump(out_dict, fp) print(f'*** Wrote ramsete command data to {file_name} ***') else: print(f'*** Skipping saving of telemetry to disk ***')
def log(self): self.counter += 1 if self.counter % 10 == 0: # start keeping track of where the robot is with an x and y position (only good for WCD) try: distance = 0.5 * (self.sparkneo_encoder_1.getPosition() - self.sparkneo_encoder_3.getPosition()) except: print( f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}" ) distance = 0 self.x = self.x + (distance - self.previous_distance) * math.sin( math.radians(self.robot.navigation.get_angle())) self.y = self.y + (distance - self.previous_distance) * math.cos( math.radians(self.robot.navigation.get_angle())) self.previous_distance = distance # send values to the dash to make sure encoders are working well #SmartDashboard.putNumber("Robot X", round(self.x, 2)) #SmartDashboard.putNumber("Robot Y", round(self.y, 2)) for ix, encoder in enumerate(self.encoders): SmartDashboard.putNumber( f"Position Enc{str(int(1+ix))}", round(encoder.getPosition() - self.encoder_offsets[ix], 2)) SmartDashboard.putNumber(f"Velocity Enc{str(int(1+ix))}", round(encoder.getVelocity(), 2)) SmartDashboard.putNumber( "Current M1", round(self.spark_neo_left_front.getOutputCurrent(), 2)) SmartDashboard.putNumber( "Current M3", round(self.spark_neo_right_front.getOutputCurrent(), 2)) SmartDashboard.putBoolean('AccLimit', self.is_limited) if self.counter % 1000 == 0: # self.display_PIDs() SmartDashboard.putString( "alert", f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}" )
def initialize(self): """Called just before this Command runs the first time.""" # allow setpoint to be controlled by the dashboard if self.source == 'dashboard': setpoint = SmartDashboard.getNumber('z_distance', 1) self.setpoint_sign = math.copysign(1, setpoint) self.setpoint = (math.fabs(setpoint)) # correction for overshoot # self.robot.drivetrain.reset_encoders() # does not work in sim mode self.start_pos = self.robot.drivetrain.get_average_encoder_distance() if self.k_dash: self.kp, self.kd = SmartDashboard.getNumber( 'zdrive_kp', self.kp), SmartDashboard.getNumber('zdrive_kd', self.kd) self.ki, self.period = SmartDashboard.getNumber( 'zdrive_ki', self.ki), SmartDashboard.getNumber('zdrive_per', self.kperiod) self.start_time = round( Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print( "\n" + f"** Started {self.getName()} with setpoint {self.setpoint} (kp={self.kp}, ki={self.ki}, kd={self.kd}) at {self.start_time} s **" ) SmartDashboard.putString( "alert", f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **" ) self.has_finished = False self.controller = wpilib.controller.PIDController(self.kp, self.ki, self.kd, period=self.kperiod) self.controller.setSetpoint(self.setpoint) self.controller.setTolerance(self.tolerance) self.controller.reset()
def end(self, message='Ended'): """Called once after isFinished returns true""" end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1) elapsed_time = end_time - self.start_time + self.time_offset print( f"** {message} {self.getName()} at {end_time} s after {round(elapsed_time,1)} s **" ) SmartDashboard.putString( "alert", f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **" ) self.robot.drivetrain.stop() AutonomousRamseteSimple.time_offset += end_time - self.start_time # maintain running time in string of commands BUT don't reassign or it makes an instance # ToDo: update this so it only happens at the end of a list of commands. May need a new variable. self.write_telemetry = SmartDashboard.getBoolean( "ramsete_write", self.write_telemetry) if self.write_telemetry: timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") file_name = timestamp + '_' + self.course + f'_kpv_{self.kp_vel:2.1f}'.replace( '.', 'p') + f'_vel_{str(self.velocity).replace(".","p")}' + '.pkl' pickle_file = Path.cwd() / 'sim' / 'data' / file_name with open(pickle_file.absolute(), 'wb') as fp: out_dict = { 'TIMESTAMP': timestamp, 'DATA': self.telemetry, 'COURSE': self.course, 'VELOCITY': self.velocity, 'KP_VEL': self.kp_vel, 'KD_VEL': self.kd_vel, 'BETA': self.beta, 'ZETA': self.zeta } pickle.dump(out_dict, fp) print(f'*** Wrote ramsete command data to {file_name} ***') else: print( f'*** Skipping saving of {len(self.telemetry)} telemetry points to disk ***' )
def robotInit(self): self.timer = wpilib.Timer() self.timer.start() self.stick = XboxController(0) SmartDashboard.putString("LEFT_Y_VALUES", "LEFT_Y_VALUES") SmartDashboard.putNumber("left_y_rate", 0.6) SmartDashboard.putNumber("left_y_expo", 1.0) SmartDashboard.putNumber("left_y_deadband", 0.1) SmartDashboard.putNumber("left_y_power", 1.5) SmartDashboard.putNumber("left_y_min", -0.5) SmartDashboard.putNumber("left_y_max", 0.5) SmartDashboard.putString("LEFT_X_VALUES", "LEFT_X_VALUES") SmartDashboard.putNumber("left_x_rate", 0.5) SmartDashboard.putNumber("left_x_expo", 1.0) SmartDashboard.putNumber("left_x_deadband", 0.1) SmartDashboard.putNumber("left_x_power", 1.5) SmartDashboard.putNumber("left_x_min", -0.5) SmartDashboard.putNumber("left_x_max", 0.5) SmartDashboard.putString("RIGHT_Y_VALUES", "RIGHT_Y_VALUES") SmartDashboard.putNumber("right_y_rate", 1.0) SmartDashboard.putNumber("right_y_expo", 0.0) SmartDashboard.putNumber("right_y_deadband", 0.1) SmartDashboard.putNumber("right_y_power", 1.0) SmartDashboard.putNumber("right_y_min", -1.0) SmartDashboard.putNumber("right_y_max", 1.0) SmartDashboard.putString("RIGHT_X_VALUES", "RIGHT_X_VALUES") SmartDashboard.putNumber("right_x_rate", 0.5) SmartDashboard.putNumber("right_x_expo", 1.0) SmartDashboard.putNumber("right_x_deadband", 0.1) SmartDashboard.putNumber("right_x_power", 1.5) SmartDashboard.putNumber("right_x_min", -0.5) SmartDashboard.putNumber("right_x_max", 0.5)
def initialize(self): self.robot.info("SpinIntakeWheelsCommand %s init" % self.direction) self.setTimeout(10) # TODO: finialize timing SmartDashboard.putString("Intakewheels spinning", self.direction)
def teleopPeriodic(self): # push all tunable values if self.driver.get_update_telemetry(): for t in self.tunables: t.push() self.drivetrain_component.push_pid_dash() # update telemetry values dash.putNumber("NavX Angle", self.navx_component.get_angle()) dash.putNumber("Drivetrain Position", self.drivetrain_component.get_position()) dash.putNumber("Current Left Velocity", self.encoders.get_velocity(EncoderSide.LEFT)) dash.putNumber("Current Right Velocity", self.encoders.get_velocity(EncoderSide.RIGHT)) dash.putNumber("Battery Voltage", self.robot_controller.getBatteryVoltage()) dash.putString("Current PID Mode", { DriveMode.PID_DRIVE: "PID Drive", DriveMode.PID_TURN: "PID Turn", DriveMode.VELOCITY_CONTROL: "Velocity Control" }[self.pid_mode]) # pull all pid values off the dashboard if self.driver.get_update_pid_dash(): self.drivetrain_component.update_pid_dash() # get the current arc for arc driving current_arc = ArcDrive( self.dash_arc_radius.get(), self.dash_arc_distance.get(), self.dash_v_max.get(), RobotMap.Drivetrain.wheelbase ) # handle whatever the current pid controller is if starting pid if self.driver.get_enable_pid(): if self.pid_mode == DriveMode.PID_TURN: self.drivetrain_component.turn_to_angle(self.dash_target_angle.get()) if self.pid_mode == DriveMode.PID_DRIVE: self.drivetrain_component.drive_to_position(self.dash_target_position.get()) if self.pid_mode == DriveMode.VELOCITY_CONTROL: self.drivetrain_component.velocity_control(self.dash_target_velocity_left.get(), self.dash_target_velocity_right.get()) # drive the robot via commands if self.drivetrain_component.drive_mode == DriveMode.MANUAL_DRIVE: driver_x, driver_y = self.driver.get_driver_input_curvature() self.drivetrain_component.curvature_drive(driver_y, driver_x) elif self.drivetrain_component.drive_mode == DriveMode.PID_TURN: self.drivetrain_component.turn_to_angle(self.dash_target_angle.get()) elif self.drivetrain_component.drive_mode == DriveMode.PID_DRIVE: self.drivetrain_component.drive_to_position(self.dash_target_position.get()) elif self.drivetrain_component == DriveMode.VELOCITY_CONTROL: self.drivetrain_component.velocity_control(self.dash_target_velocity_left.get(), self.dash_target_velocity_right.get()) # switch to manual control if enabled if self.driver.get_manual_control_override(): self.drivetrain_component.drive_mode = DriveMode.MANUAL_DRIVE # rotate through pid control modes if self.driver.get_toggle_pid_type_pressed(): self.pid_mode = { DriveMode.PID_TURN: DriveMode.PID_DRIVE, DriveMode.PID_DRIVE: DriveMode.VELOCITY_CONTROL, DriveMode.VELOCITY_CONTROL: DriveMode.PID_TURN }[self.pid_mode]
def initialize(self): """Called just before this Command runs the first time.""" self.previous_time = -1 self.init_time = round( Timer.getFPGATimestamp() - self.robot.enabled_time, 1) self.robot.drivetrain.drive.feed( ) # this initialization is taking some time now # update gains from dash if desired if self.dash is True: self.kp_vel = SmartDashboard.getNumber("ramsete_kpvel", self.kp_vel) self.beta = SmartDashboard.getNumber("ramsete_B", self.beta) self.zeta = SmartDashboard.getNumber("ramsete_Z", self.zeta) self.write_telemetry = SmartDashboard.getBoolean( "ramsete_write", self.write_telemetry) # create controllers self.follower = wpilib.controller.RamseteController( self.beta, self.zeta) self.left_controller = wpilib.controller.PIDController( self.kp_vel, 0, self.kd_vel) self.right_controller = wpilib.controller.PIDController( self.kp_vel, 0, self.kd_vel) self.left_controller.reset() self.right_controller.reset() #ToDo - make this selectable, probably from the dash, add the other trajectories (done) trajectory_choice = self.path # path is passed in on constructor this time self.velocity = float(self.robot.oi.velocity_chooser.getSelected() ) # get the velocity from the GUI self.trajectory = drive_constants.generate_trajectory( trajectory_choice, self.velocity, simulation=self.robot.isSimulation(), save=False) self.course = trajectory_choice # Note - we are setting to pose to have the robot physically in the start position - usually absolute matters if self.relative: # figure out if we want to start where we are or where the trajectory says we should be self.start_pose = geo.Pose2d( self.trajectory.sample(0).pose.X(), self.trajectory.sample(0).pose.Y(), self.robot.drivetrain.get_rotation2d()) else: #self.start_pose = self.robot.drivetrain.get_pose() field_x = SmartDashboard.getNumber( 'field_x', self.trajectory.sample(0).pose.X()) field_y = SmartDashboard.getNumber( 'field_y', self.trajectory.sample(0).pose.Y()) self.start_pose = geo.Pose2d( field_x, field_y, self.robot.drivetrain.get_rotation2d()) self.robot.drivetrain.reset_odometry(self.start_pose) self.robot.drivetrain.drive.feed( ) # this initialization is taking some time now initial_state = self.trajectory.sample(0) # these are all meters in 2021 self.previous_speeds = self.kinematics.toWheelSpeeds( wpimath.kinematics.ChassisSpeeds( initial_state.velocity, 0, initial_state.curvature * initial_state.velocity)) self.start_time = round( Timer.getFPGATimestamp() - self.robot.enabled_time, 1) print( "\n" + f"** Started {self.getName()} on {self.course} with load time {self.start_time-self.init_time:2.2f}s" f" (b={self.beta}, z={self.zeta}, kp_vel={self.kp_vel}) at {self.start_time} s **" ) SmartDashboard.putString( "alert", f"** Started {self.getName()} at {self.start_time} s **") if self.reset_telemetry: # don't reset telemetry if we want to chain trajectories together print( 'Time\tTrVel\tTrRot\tlspd\trspd\tram an\tram vx\tram vy\tlffw\trffw\tlpid\trpid' ) self.telemetry.clear( ) # Aha. important not to set it to a new value (=[]) , because then it makes an instance AutonomousRamseteSimple.time_offset = 0 # seems like an easier way to do it, but man. Globals are always ugly. else: pass
def teleopPeriodic(self): #print(self.limelight.get) #print(self.color_wheel_motor.__dir__()); #print("execute method") try: # drive the drivetrain as needed driver_x, driver_y = self.driver.get_curvature_output() # manually handled driving if self.drivetrain.state == DrivetrainState.MANUAL_DRIVE: self.drivetrain.curvature_drive(driver_y, driver_x) # set turbo mode self.drivetrain.set_power_scaling(self.driver.process_turbo_mode()) # # check and see if we need to activate driver assists # if self.drivetrain.ready_straight_assist() and self.driver.ready_straight_assist(): # self.drivetrain.drive_straight(driver_y) # elif self.drivetrain.state == DrivetrainState.AIDED_DRIVE_STRAIGHT: # self.drivetrain.state = DrivetrainState.MANUAL_DRIVE # revert to manual control if enabled if self.driver.get_manual_control_override(): self.drivetrain.state = DrivetrainState.MANUAL_DRIVE except: print("DRIVETRAIN ERROR") try: # move intake lift if self.sysop.get_intake_lower(): self.intake_lift.send_down() elif self.sysop.get_intake_raise(): self.intake_lift.send_up() except: print("INTAKE LIFT ERROR") try: #print(self.intake_roller_motor.getSelectedSensorPosition()); # intake rollers if self.sysop.get_intake_intake(): self.intake_roller.intake() elif self.sysop.get_intake_outtake(): self.intake_roller.outtake() else: self.intake_roller.stop() except: print("INTAKE ROLLER ERROR") try: if self.sysop.get_target_aim(): self.shooter.aim_and_fire() elif self.sysop.get_shooter_stop(): self.shooter.stop_fire() self.drivetrain.reset_state() elif self.sysop.get_change_shooter_power() != 0: self.shooter.adjust_power( self.sysop.get_change_shooter_power()) except: traceback.print_exc() print("AUTO AIM ERROR") try: self.climber.set_power(self.sysop.get_climb_axis()) except: print("CLIMBER ERROR") try: manual_fortune_input = self.sysop.get_manual_fortune_axis() fms_color_position = self.ds.getGameSpecificMessage() dash.putString("Color sensor color", self.color_sensor.get_color()) dash.putString("FMS Color", fms_color_position) if manual_fortune_input != 0: self.fortune_controller.manual_power(manual_fortune_input) # elif self.sysop.get_position_control() and fms_color_position != "": # self.fortune_controller.position_control({ # "B": WheelOfFortuneColor.BLUE, # "R": WheelOfFortuneColor.GREEN, # "G": WheelOfFortuneColor.RED, # "Y": WheelOfFortuneColor.YELLOW, # }[fms_color_position]) # TODO this is commented out because it'll be more effective to use manual control # elif self.sysop.get_rotation_control(): # self.fortune_controller.rotation_control() elif self.fortune_controller.state == ColorWheelState.MANUAL: self.fortune_controller.manual_power(0) except: print("COLOR WHEEL ERROR")