class Arcade(Subsystem): """ Subsystem to control the motors for ArcadeDrive """ def __init__(self): super().__init__("Arcade") # motors and the channels they are connected to self.frontLeftMotor = wpilib.PWMVictorSPX(0) self.rearLeftMotor = wpilib.PWMVictorSPX(1) self.frontRightMotor = wpilib.PWMVictorSPX(2) self.rearRightMotor = wpilib.PWMVictorSPX(3) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(False) def speed(self, xSpeed, zRotation): self.drive.arcadeDrive(xSpeed, zRotation) def speed2(self, leftSpeed, rightSpeed): self.drive.tankDrive(leftSpeed, rightSpeed) def initDefaultCommand(self): self.setDefaultCommand(ThrottleMixer())
class MyRobot(wpilib.IterativeRobot): def robotInit(self): self.frontLeftMotor = wpilib.Talon(1) self.rearLeftMotor = wpilib.Talon(12) self.frontRightMotor = wpilib.Talon(2) self.rearRightMotor = wpilib.Talon(5) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) # joysticks 1 & 2 on the driver station self.leftStick = wpilib.Joystick(0) self.rightStick = wpilib.Joystick(1) def teleopInit(self): '''Executed at the start of teleop mode''' self.myRobot.setSafetyEnabled(True) def teleopPeriodic(self): '''Runs the motors with tank steering''' self.myRobot.tankDrive(self.leftStick.getY() * -1, self.rightStick.getY() * -1)
class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" # object that handles basic drive operations self.frontLeftMotor = wpilib.Talon(0) self.rearLeftMotor = wpilib.Talon(1) self.frontRightMotor = wpilib.Talon(2) self.rearRightMotor = wpilib.Talon(3) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) # joysticks 1 & 2 on the driver station self.leftStick = wpilib.Joystick(0) self.rightStick = wpilib.Joystick(1) def teleopInit(self): """Executed at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) def teleopPeriodic(self): """Runs the motors with tank steering""" self.myRobot.tankDrive(self.leftStick.getY() * -1, self.rightStick.getY() * -1)
class MyRobot(wpilib.SampleRobot): # Defines the channels on the RoboRio that the motors are plugged into. There can be up to eight. FLChannel = 3 FRChannel = 4 RLChannel = 1 RRChannel = 2 # Defines the order that the sticks that are plugged in are assigned. DriveStickChannel = 0 # ExtraStickChannel = 1 def robotInit(self): # Launches the camera server so that we can have video through any cameras on the robot. wpilib.CameraServer.launch() # Defines the motors that will actually be on the robot for use in the drive function. self.FLMotor = wpilib.Spark(self.FLChannel) self.FRMotor = wpilib.Spark(self.FRChannel) self.RLMotor = wpilib.Spark(self.RLChannel) self.RRMotor = wpilib.Spark(self.RRChannel) # Puts the motors into groups so that they fit the parameters of the function. self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor) self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor) # The drive function that tells the computer what kind of drive to use and where the motors are. self.drive = DifferentialDrive(self.LMG, self.RMG) # Tells the computer how long to wait without input to turn off the motors self.drive.setExpiration(0.1) # Defines the Joystick that we will be using for driving. self.DriveStick = wpilib.Joystick(self.DriveStickChannel) self.components = {"drive": self.drive} self.automodes = robotpy_ext.autonomous.AutonomousModeSelector( "autonomous", self.components) def autonomous(self): self.automodes.run() def operatorControl(self): # Enables the safety on the drive. Very important. DO NOT FORGET! self.drive.setSafetyEnabled(True) # Checks to see if the robot is activated and that operator control is active, so your robot does not move # when it is not supposed to. while self.isOperatorControl() and self.isEnabled(): # drives the robot with the arcade drive, which uses one joystick and is a bit easier to use. # It is a part of DifferentialDrive self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False) wpilib.Timer.delay(0.005)
class DriveTrain(Subsystem): def __init__(self): # Ensures a single-time initialization Subsystem.__init__(self, "DriveTrain") # Front Motor Controllers self.front_cont_right = ctre.WPI_TalonSRX(3) self.rear_cont_right = ctre.WPI_TalonSRX(2) self.right = wpilib.SpeedControllerGroup(self.front_cont_right, self.rear_cont_right) # Rear Motor Controllers self.front_cont_left = ctre.WPI_TalonSRX(4) self.rear_cont_left = ctre.WPI_TalonSRX(5) self.left = wpilib.SpeedControllerGroup(self.front_cont_left, self.rear_cont_left) # Drive Type self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) # enable safety self.drive.setSafetyEnabled(False) def engageDrive(self, speed, rotation): self.drive.arcadeDrive(speed, rotation) #print(speed) def initDefaultCommand(self): self.setDefaultCommand(Drive())
class MyRobot(TimedRobot): # Defines the channels that are used on the inputs. In the simulator, they have to match up with the physics.py file # This is really useful when you have a variable used hundreds of times and you want to have it set so you can # change it all in one go. RLMotorChannel = 2 RRMotorChannel = 0 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 # RobotInit is where all of the things we are using is initialized. def robotInit(self): # Note the lack of the camera server initialization here. # Initializing drive motors RLMotor = Spark(self.RLMotorChannel) RRMotor = Spark(self.RRMotorChannel) FLMotor = Spark(self.FLMotorChannel) FRMotor = Spark(self.FRMotorChannel) # Grouping drive motors into left and right. self.Left = SpeedControllerGroup(RLMotor, FLMotor) self.Right = SpeedControllerGroup(RRMotor, FRMotor) # Initializing drive Joystick. self.DriveStick = Joystick(self.DriveStickChannel) # Initializing drive function. self.drive = DifferentialDrive(self.Left, self.Right) # Sets the right side motors to be inverted. self.drive.setRightSideInverted(rightSideInverted=True) # Turns the drive off after .1 seconds of inactivity. self.drive.setExpiration(0.1) # Components is a dictionary that contains any variables you want to put into it. All of the variables put into # components dictionary is given to the autonomous modes. self.components = {"drive": self.drive} # Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous # modes should inherit. self.automodes = autonomous.AutonomousModeSelector( "Sim_Autonomous", self.components) # Autonomous and teleop periodic both run the code on a fixed loop time. A part of TimedRobot. def autonomousPeriodic(self): # runs the autonomous modes when Autonomous is activated. self.automodes.run() def teleopPeriodic(self): # Turns on drive safety and checks to se if operator control and the robot is enabled. self.drive.setSafetyEnabled(True) if self.isOperatorControl() and self.isEnabled(): # Drives the robot with controller inputs. You can use buttons with self.DriveStick.getRawButton(ButtonNum). self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False)
class MyRobot(wpilib.IterativeRobot): def robotInit(self): '''Robot initialization function''' # object that handles basic drive operations self.leftMotor = wpilib.Victor(0) self.rightMotor = wpilib.Victor(1) #self.myRobot = wpilib.RobotDrive(0, 1) self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor) # joysticks 1 & 2 on the driver station self.leftStick = wpilib.Joystick(0) self.rightStick = wpilib.Joystick(1) self.myRobot.setExpiration(0.1) self.myRobot.setSafetyEnabled(True) def autonomousInit(self): ''' :return: ''' pass def autonomousPeriodic(self): pass def teleopInit(self): """Executed at the start of teleop mode :return: """ pass def teleopPeriodic(self): """Runs the motors with tank steering :return: """ self.myRobot.tankDrive(self.leftStick.getY(), self.rightStick.getY())
class DriveArcade(wpilib.command.Command): """ Command to control drive using gamepad in arcade mode. """ diffDrive: DifferentialDrive gamePad: GenericHID nominal: float def __init__(self, drive: Drive, leftMotors: SpeedControllerGroup, rightMotors: SpeedControllerGroup, gamePad: GenericHID): super().__init__("DriveArcade") self.requires(drive) self.gamePad = gamePad self.diffDrive = DifferentialDrive(leftMotors, rightMotors) self.nominal = 3.0 / 8.0 self.deadband = 0.15 # XBox360 controller has a lot of slop def tweak(self, val, scale): tweaked: float = DifferentialDrive.applyDeadband(val, self.deadband) if tweaked == 0.0: # User input value in deadband, just return 0.0 return tweaked tweaked *= scale * (1 - self.nominal) if tweaked > 0: tweaked = (tweaked * tweaked) + self.nominal else: tweaked = (tweaked * -tweaked) - self.nominal return tweaked def initialize(self): self.diffDrive.setSafetyEnabled(True) def execute(self): throttle: float = self.tweak(-self.gamePad.getY(GenericHID.Hand.kLeft), 1.0) rotation: float = self.tweak(self.gamePad.getX(GenericHID.Hand.kRight), 0.5) #print("Throttle", throttle, "Rotation", rotation) self.diffDrive.arcadeDrive(throttle, rotation, False) def isFinished(self): return False def interrupted(self): self.diffDrive.stopMotor() self.diffDrive.setSafetyEnabled(False) def end(self): self.interrupted()
class DriveTrain(Subsystem): def __init__(self): super().__init__('DriveTrain') map = wpilib.command.Command.getRobot().robotMap #create motors here motors = {} for name in map.CAN.driveMotors: motors[name] = wpilib.command.Command.getRobot( ).motorHelper.createMotor(map.CAN.driveMotors[name]) print(motors['leftMotor'].getSensorCollection()) self.motors = motors self.drive = DifferentialDrive(motors['leftMotor'], motors['rightMotor']) self.drive.setSafetyEnabled(False) self.minSpeedChange = 0.001 self.timeRate = 0.2 self.desired = {} self.desired['left'] = 0 self.desired['right'] = 0 self.current = {} self.current['left'] = 0 self.current['right'] = 0 self.lastUpdateTime = time.time() self.deadband = 0.1 def setDeadband(self, deadband): self.drive.deadband = deadband def move(self, spd, rot): #print("Spd" ,spd, "rot", rot) self.drive.arcadeDrive(spd, rot, True) #print("Left Position: %f"%(self.motors['leftMotor'].getSelectedSensorPosition(0))) #print("Right Position: %f"%(self.motors['rightMotor'].getSelectedSensorPosition(0))) def initDefaultCommand(self): self.setDefaultCommand(commands.driveControlled.DriveControlled()) def moveTank(self, left, right): self.drive.tankDrive(left, right, True)
class MyRobot(wpilib.TimedRobot): RLMotorChannel = 1 RRMotorChannel = 2 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 def robotInit(self): RLMotor = wpilib.Spark(self.RLMotorChannel) RRMotor = wpilib.Spark(self.RRMotorChannel) FLMotor = wpilib.Spark(self.FLMotorChannel) FRMotor = wpilib.Spark(self.FRMotorChannel) self.Left = wpilib.SpeedControllerGroup(RLMotor, FLMotor) self.Right = wpilib.SpeedControllerGroup(RRMotor, FRMotor) self.DriveStick = wpilib.Joystick(self.DriveStickChannel) self.drive = DifferentialDrive(self.Left, self.Right) self.drive.setExpiration(0.1) def operatorControl(self): self.drive.setSafetyEnabled(True) while self.isEnabled() and self.isOperatorControl(): self.drive.arcadeDrive( self.DriveStick.getX(), self.DriveStick.getY(), squareInputs=True ) f
class Apollo(wpilib.TimedRobot): def __init__(self): super().__init__() self.joystick = wpilib.XboxController(0) self.left_drive_motor = wpilib.Talon(0) self.left_drive_motor_2 = wpilib.Talon(1) self.right_drive_motor = wpilib.Talon(2) self.right_drive_motor_2 = wpilib.Talon(3) self.left_drive_motor_group = wpilib.SpeedControllerGroup( self.left_drive_motor, self.left_drive_motor_2) self.right_drive_motor_group = wpilib.SpeedControllerGroup( self.right_drive_motor, self.right_drive_motor_2) self.drive = DifferentialDrive(self.left_drive_motor_group, self.right_drive_motor_group) self.drive_rev = False self.lift_motor = wpilib.Spark(4) self.lift_motor_2 = wpilib.Spark(5) self.lift_motor_group = wpilib.SpeedControllerGroup( self.lift_motor, self.lift_motor_2) self.lift_motor_speed = 0 self.lock_controls = False self.front_motor = wpilib.Spark(6) self.front_motor_2 = wpilib.Spark(7) self.front_motor_2.setInverted(True) self.front_motor_group = wpilib.SpeedControllerGroup( self.front_motor, self.front_motor_2) self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1) self.encoder = wpilib.Encoder(aChannel=0, bChannel=1) def reset(self): self.drive_rev = False while self.lift_motor_speed > 0: self.lift_motor_speed -= 0.002 self.lift_motor_group.set(self.lift_motor_speed) if self.lift_motor_speed < 0: self.lift_motor_speed = 0 self.lift_motor_group.set(self.lift_motor_speed) self.front_speed = 0 self.front_motor_group.set(self.front_speed) self.lock_controls = False self.hatch_solenoid.set(0) self.encoder.reset() def drive_control(self): lh_y = self.joystick.getY(self.joystick.Hand.kLeft) * (1 / 2) rh_x = self.joystick.getX(self.joystick.Hand.kRight) * (1 / 2) if self.joystick.getStickButtonPressed(self.joystick.Hand.kLeft) or \ self.joystick.getStickButtonPressed(self.joystick.Hand.kRight): self.drive_rev = not self.drive_rev if self.drive_rev: self.drive.arcadeDrive(lh_y, rh_x, True) else: self.drive.arcadeDrive(lh_y * -1, rh_x, True) def lift_control(self): low_volt = 0.2 if self.joystick.getYButtonPressed(): self.lock_controls = not self.lock_controls if not self.lock_controls: if self.joystick.getTriggerAxis(self.joystick.Hand.kLeft) > 0.9: # self.lift_motor_speed = 0.6 if self.lift_motor_speed == 0.0 or self.lift_motor_speed == low_volt: self.lift_motor_speed = 0.6 elif int(abs(self.encoder.getRate())) == 0: self.lift_motor_speed += 0.01 elif (self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9)\ and self.joystick.getBButton(): self.lift_motor_speed -= 0.01 if self.lift_motor_speed > 0.0 else 0.0 elif self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9: self.lift_motor_speed = low_volt elif (self.lift_motor_speed > low_volt and abs(self.encoder.getRate()) > 0): self.lift_motor_speed -= 0.0025 elif (0.0 < self.lift_motor_speed < low_volt): self.lift_motor_speed -= 0.0025 if self.lift_motor_speed > 0.0 else 0.00 elif self.lift_motor_speed < 0: self.lift_motor_speed = 0 self.lift_motor_group.set(self.lift_motor_speed) def grab_control(self): if self.joystick.getBumper(self.joystick.Hand.kLeft) and not \ self.joystick.getBumper(self.joystick.Hand.kRight): self.front_motor_group.set(0.33) elif self.joystick.getBumperPressed(self.joystick.Hand.kRight) and not \ self.joystick.getBumper(self.joystick.Hand.kLeft): self.front_motor_group.set(-0.7) elif self.joystick.getBumperReleased(self.joystick.Hand.kLeft) or \ self.joystick.getBumperReleased(self.joystick.Hand.kRight): self.front_motor_group.set(0) def hatch_control(self): if self.joystick.getXButtonPressed(): self.hatch_solenoid.set(2) elif self.joystick.getXButtonReleased(): self.hatch_solenoid.set(1) def robotInit(self): self.reset() wpilib.CameraServer.launch('vision.py:main') def robotPeriodic(self): pass def autonomousInit(self): self.reset() def autonomousPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control() def disabledInit(self): self.reset() def disabledPeriodic(self): self.reset() def teleopInit(self): self.reset() self.drive.setSafetyEnabled(True) def teleopPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control()
class Drivetrain: navx = navx.AHRS left_motor_master = WPI_TalonFX left_motor_slave = WPI_TalonFX left_motor_slave2 = WPI_TalonFX right_motor_master = WPI_TalonFX right_motor_slave = WPI_TalonFX right_motor_slave2 = WPI_TalonFX shifter_solenoid = Solenoid arcade_cutoff = tunable(0.1) angle_correction_cutoff = tunable(0.05) angle_correction_factor_low_gear = tunable(0.1) angle_correction_factor_high_gear = tunable(0.1) angle_correction_max = tunable(0.2) little_rotation_cutoff = tunable(0.1) def __init__(self): self.pending_differential_drive = None self.force_differential_drive = False self.pending_gear = LOW_GEAR self.pending_position = None self.pending_reset = False self.og_yaw = None self.pending_manual_drive = None self.is_manual_mode = False # Set encoders self.left_motor_master.configSelectedFeedbackSensor( ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0) self.right_motor_master.configSelectedFeedbackSensor( ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0) self.left_motor_master.setSensorPhase(True) # Set slave motors self.left_motor_slave.set(ctre.TalonFXControlMode.Follower, self.left_motor_master.getDeviceID()) self.left_motor_slave2.set(ctre.TalonFXControlMode.Follower, self.left_motor_master.getDeviceID()) self.right_motor_slave.set(ctre.TalonFXControlMode.Follower, self.right_motor_master.getDeviceID()) self.right_motor_slave2.set(ctre.TalonFXControlMode.Follower, self.right_motor_master.getDeviceID()) # Set up drive control self.robot_drive = DifferentialDrive(self.left_motor_master, self.right_motor_master) self.robot_drive.setDeadband(0) self.robot_drive.setSafetyEnabled(False) def is_left_encoder_connected(self): return self.left_motor_master.getPulseWidthRiseToRiseUs() != 0 def is_right_encoder_connected(self): return self.right_motor_master.getPulseWidthRiseToRiseUs() != 0 def reset_position(self): self.left_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) self.right_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) def get_position(self): ''' Returns averaged quadrature position in inches. ''' left_position = self.get_left_encoder() right_position = self.get_right_encoder() position = (((left_position + right_position) / 2) * (1 / UNITS_PER_REV) * DISTANCE_PER_REV) return position def drive(self, *args): self.differential_drive(*args) def differential_drive(self, y, rotation=0, squared=True, force=False, quick_turn=False, use_curvature=False): if not self.force_differential_drive: self.pending_differential_drive = DifferentialDriveConfig( y=y, rotation=rotation, squared=squared, quick_turn=quick_turn, use_curvature=use_curvature) self.force_differential_drive = force def turn(self, rotation=0, force=False): self.differential_drive(0, rotation, squared=False, force=force) def reset_angle_correction(self): self.navx.reset() def angle_corrected_differential_drive(self, y, rotation=0): ''' Heading must be reset first. (drivetrain.reset_angle_correction()) ''' # Scale angle to reduce max turn rotation = util.scale(rotation, -1, 1, -0.65, 0.65) # Scale y-speed in high gear if self.pending_gear == HIGH_GEAR: y = util.scale(y, -1, 1, -0.75, 0.75) use_curvature = False quick_turn = False # Small rotation at lower speeds - and also do a quick_turn instead of # the normal curvature-based mode. if abs(y) <= self.little_rotation_cutoff: rotation = util.abs_clamp(rotation, 0, 0.7) quick_turn = True # NEVER USE CURVATURE # Curvature drive for high gear and high speedz # if self.pending_gear == HIGH_GEAR and abs(y) >= 0.5: # use_curvature = True # Angle correction if abs(rotation) <= self.angle_correction_cutoff: heading = self.navx.getYaw() if not self.og_yaw: self.og_yaw = heading factor = self.angle_correction_factor_high_gear if \ self.pending_gear == HIGH_GEAR else \ self.angle_correction_factor_low_gear rotation = util.abs_clamp(-factor * (heading - self.og_yaw), 0, self.angle_correction_max) else: self.og_yaw = None self.differential_drive(y, rotation, quick_turn=quick_turn, use_curvature=use_curvature) def shift_low_gear(self): self.pending_gear = LOW_GEAR def shift_high_gear(self): self.pending_gear = HIGH_GEAR def shift_toggle(self): if self.pending_gear == HIGH_GEAR: self.pending_gear = LOW_GEAR else: self.pending_gear = HIGH_GEAR def manual_drive(self, left, right): self.pending_manual_drive = [left, right] def get_left_encoder(self): return -self.left_motor_master.getQuadraturePosition() def get_right_encoder(self): return self.right_motor_master.getQuadraturePosition() def get_left_encoder_meters(self): return self.get_left_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_meters(self): return self.get_right_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_left_encoder_velocity(self): return -self.left_motor_master.getQuadratureVelocity() def get_right_encoder_velocity(self): return self.right_motor_master.getQuadratureVelocity() def get_left_encoder_velocity_meters(self): return self.get_left_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_velocity_meters(self): return self.get_right_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def set_manual_mode(self, is_manual): self.is_manual_mode = is_manual if not is_manual: self.pending_manual_drive = None def execute(self): # print('exec drivetrain', self.pending_differential_drive) # print('dist_traveled_meters', self.get_left_encoder_meters(), # self.get_right_encoder_meters()) # Shifter self.shifter_solenoid.set(self.pending_gear) # Manual if self.is_manual_mode: if self.pending_manual_drive: left, right = self.pending_manual_drive # left = self.robot_drive.applyDeadband(left, DEADBAND) # right = self.robot_drive.applyDeadband(right, DEADBAND) self.left_motor_master.set(-left) self.right_motor_master.set(right) self.pending_manual_drive = None return # Drive if self.pending_differential_drive: if self.pending_differential_drive.use_curvature and \ abs(self.pending_differential_drive.y) > \ self.arcade_cutoff and \ USE_CURVATURE_DRIVE: self.robot_drive.curvatureDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, isQuickTurn=self.pending_differential_drive.quick_turn) else: self.robot_drive.arcadeDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, squareInputs=self.pending_differential_drive.squared) self.pending_differential_drive = None self.force_differential_drive = False def on_disable(self): self.robot_drive.arcadeDrive(0, 0) def get_state(self): return { 'pending_gear': self.pending_gear, 'pending_differential_drive': self.pending_differential_drive } def put_state(self, state): self.pending_gear = state['pending_gear'] self.pending_differential_drive = DifferentialDriveConfig._make( state['pending_differential_drive']) def limelight_turn(self): self.llt = NetworkTables.getTable('limelight') self.tv = self.llt.getNumber('tv', 0) self.tx = self.llt.getNumber('tx', 0) if self.tv: self.turn(self.get_position() + self.tx)
class Gemini(wpilib.TimedRobot): def __init__(self): """ Initialization of internal class variables and software-bases only """ super().__init__() # global button status list construction self.buttonToggleStatus = [ False, False, False, False, False, False, False ] from networktables import NetworkTables # connection for logging & Smart Dashboard self.sd = NetworkTables.getTable('SmartDashboard') NetworkTables.initialize(server='10.55.49.2') self.sd.putString(" ", "Connection") def robotInit(self): ''' Initialization of robot systems. ''' logging.info( '-( ͡° ͜ʖ ͡°)╯╲_-^o=o\ \"Don\'t mind me, just walking the robot.\"' ) from wpilib.drive import DifferentialDrive from ctre import WPI_TalonSRX, WPI_VictorSPX # drive train motors self.frontRightMotor = WPI_TalonSRX(4) self.rearRightMotor = WPI_TalonSRX(3) self.frontLeftMotor = WPI_TalonSRX(1) self.rearLeftMotor = WPI_TalonSRX(2) # lift encoder construction self.liftEncoder = wpilib.Encoder(8, 9) # liftArm encoder construction self.liftArmEncoder = wpilib.Encoder(5, 6) # drive train motor groups assignment self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) # drive train drive group assignment self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) # lift motor system initialization self.liftOne = WPI_VictorSPX(1) self.liftTwo = WPI_VictorSPX(2) self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo) # lift arm motor system initialization self.liftArmOne = WPI_VictorSPX(3) self.liftArmTwo = WPI_VictorSPX(4) self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne, self.liftArmTwo) # cargo intake motor initialization self.cargo = WPI_VictorSPX(5) # game and joystick controller construction # joystick - 0, 1 | controller - 2 self.leftStick = wpilib.Joystick(0) self.rightStick = wpilib.Joystick(1) self.xbox = wpilib.Joystick(2) self.buttonBox = wpilib.Joystick(3) # pneumatic and compressor system initialization self.Compressor = wpilib.Compressor(0) self.Compressor.setClosedLoopControl(True) self.enable = self.Compressor.getPressureSwitchValue() self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2, 3) # hatch panel claw self.DoubleSolenoidThree = wpilib.DoubleSolenoid( 4, 5) # hatch panel ejection self.Compressor.start() # Smart Dashboard and NetworkTables initialization and construction self.PDP = wpilib.PowerDistributionPanel() self.roboController = wpilib.RobotController() self.DS = wpilib.DriverStation.getInstance() # proximity detection sensors self.Hall = wpilib.DigitalInput(7) self.ultrasonic = wpilib.AnalogInput(2) self.cargoUltrasonic = wpilib.AnalogInput(3) # timer construction self.timer = wpilib.Timer() # initialization of the HTTP camera wpilib.CameraServer.launch('vision.py:main') self.sd.putString("", "Top Camera") self.sd.putString(" ", "Bottom Camera") from sensors import REV_Color_Sensor_V2 # Initialization and configuration of I2C interface with color sensor. self.colorSensor = REV_Color_Sensor_V2(wpilib.I2C.Port.kOnboard) def autonomousInit(self): ''' Executed each time the robot enters autonomous. ''' # pre-auto timer configuration self.timer.reset() self.timer.start() # drive train encoder reset self.frontRightMotor.setQuadraturePosition(0, 0) self.frontLeftMotor.setQuadraturePosition(0, 0) self.liftEncoder.reset() def autonomousPeriodic(self): '''' Called periodically during autonomous. ''' if self.DS.getGameSpecificMessage(): # Test Methods if self.DS.getGameSpecificMessage() is 'encoder_test': # Drives robot set encoder distance away self.rightPos = fabs( self.frontRightMotor.getQuadraturePosition()) self.leftPos = fabs( self.frontLeftMotor.getQuadraturePosition()) self.distIn = (( (self.leftPos + self.rightPos) / 2) / 4096) * 18.84955 if 0 <= self.distIn <= 72: self.drive.tankDrive(0.5, 0.5) else: self.drive.tankDrive(0, 0) if self.DS.getGameSpecificMessage() is 'diagnostics': # Smart Dashboard Tests self.sd.putNumber("Temperature: ", self.PDP.getTemperature()) self.sd.putNumber("Battery Voltage: ", self.roboController.getBatteryVoltage()) self.sd.putBoolean(" Browned Out?", self.roboController.isBrownedOut) # Smart Dashboard diagnostics self.sd.putNumber( "Right Encoder Speed: ", abs(self.frontRightMotor.getQuadratureVelocity())) self.sd.putNumber( "Left Encoder Speed: ", abs(self.frontLeftMotor.getQuadratureVelocity())) self.sd.putNumber("Lift Encoder: ", self.liftEncoder.getDistance()) if self.DS.getGameSpecificMessage() is 'pressure': self.Compressor.start() elif self.Compressor.enabled() is True: self.Compressor.stop() if not self.DS.getGameSpecificMessage( ) is 'pressure' and not self.DS.getGameSpecificMessage( ) is 'encoder_test': # begin normal periodic # get all required data once per frame # toggle button management per frame if self.buttonBox.getRawButtonPressed(1): self.buttonToggleStatus = [ not self.buttonToggleStatus[0], False, False, False, False, False, False ] elif self.buttonBox.getRawButtonPressed(2): self.buttonToggleStatus = [ False, not self.buttonToggleStatus[1], False, False, False, False, False ] elif self.buttonBox.getRawButtonPressed(3): self.buttonToggleStatus = [ False, False, not self.buttonToggleStatus[2], False, False, False, False ] elif self.buttonBox.getRawButtonPressed(4): self.buttonToggleStatus = [ False, False, False, not self.buttonToggleStatus[3], False, False, False ] elif self.buttonBox.getRawButtonPressed(5): self.buttonToggleStatus = [ False, False, False, False, not self.buttonToggleStatus[4], False, False ] elif self.buttonBox.getRawButtonPressed(6): self.buttonToggleStatus = [ False, False, False, False, False, not self.buttonToggleStatus[5], False ] elif self.buttonBox.getRawButtonPressed(7): self.buttonToggleStatus = [ False, False, False, False, False, False, not self.buttonToggleStatus[6] ] liftTicks = self.liftEncoder.get() hallState = self.Hall.get() compressorState = self.Compressor.enabled() solenoidStateOne = self.DoubleSolenoidOne.get() solenoidStateTwo = self.DoubleSolenoidTwo.get() solenoidStateThree = self.DoubleSolenoidThree.get() # robot ultrasonic self.ultraValue = self.ultrasonic.getVoltage() # cargo ultrasonic self.cargoUltraValue = self.cargoUltrasonic.getVoltage() # xbox value states xboxButtonStates = [ self.xbox.getRawButton(1), self.xbox.getRawButton(2), self.xbox.getRawButton(3), self.xbox.getRawButton(4), self.xbox.getRawButton(5), self.xbox.getRawButton(6), self.xbox.getRawButton(7), self.xbox.getRawButton(8), self.xbox.getRawButton(9), self.xbox.getRawButton(10) ] xboxAxisStates = [ self.xbox.getRawAxis(1), self.xbox.getRawAxis(2), self.xbox.getRawAxis(3), self.xbox.getRawAxis(4), self.xbox.getRawAxis(5) ] # joystick value states rJoystickButtonStates = [self.rightStick.getRawButton(1)] rJoystickAxisStates = [ self.rightStick.getRawAxis(1), self.rightStick.getRawAxis(2), self.rightStick.getRawAxis(3) ] lJoystickButtonStates = [self.leftStick.getRawButton(1)] lJoystickAxisStates = [ self.leftStick.getRawAxis(1), self.leftStick.getRawAxis(2), self.leftStick.getRawAxis(3) ] # define lift stages def cargo_one(): if liftTicks <= 133: # Cargo 1 self.lift.set(0.5) elif liftTicks > 133: self.lift.set(0.05) def cargo_two(): if liftTicks <= 270: # Cargo 2 self.lift.set(0.5) elif liftTicks > 270: self.lift.set(0.05) def cargo_three(): if liftTicks <= 415: # Cargo 3 self.lift.set(0.5) elif liftTicks > 415: self.lift.set(0.05) def hatch_one(): if liftTicks <= 96: # Hatch 1 self.lift.set(0.5) elif liftTicks > 96: self.lift.set(0.05) def hatch_two(): if liftTicks <= 237: # Hatch 2 self.lift.set(0.5) elif liftTicks > 237: self.lift.set(0.05) def hatch_three(): if liftTicks <= 378: # Hatch 3 self.lift.set(0.5) elif liftTicks > 378: self.lift.set(0.05) def lift_encoder_reset(): self.lift.set(0.01) if hallState is True: self.liftEncoder.reset() if self.buttonToggleStatus[0] is True: cargo_three() elif self.buttonToggleStatus[1] is True: hatch_three() elif self.buttonToggleStatus[2] is True: cargo_two() elif self.buttonToggleStatus[3] is True: hatch_two() elif self.buttonToggleStatus[4] is True: cargo_one() elif self.buttonToggleStatus[5] is True: hatch_one() elif self.buttonToggleStatus[6] is True: lift_encoder_reset() # compressor state self.sd.putString( "Compressor Status: ", "Enabled" if compressorState is True else "Disabled") # gear state self.sd.putString( "Gear Shift: ", "High Speed" if solenoidStateOne is 1 else "Low Speed") # ejector state self.sd.putString( "Ejector Pins: ", "Ejected" if solenoidStateThree is 2 else "Retracted") # claw state self.sd.putString("Claw: ", "Open" if solenoidStateTwo is 2 else "Closed") # hatch station range state self.sd.putString( "PLAYER STATION RANGE: ", "YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!") # self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue) # hatch spaceship range self.sd.putString( "HATCH RANGE: ", "HATCH IN RANGE" if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE") # compressor if xboxButtonStates[8]: self.Compressor.stop() elif xboxButtonStates[9]: self.Compressor.start() elif rJoystickButtonStates[0]: # shift right self.DoubleSolenoidOne.set( wpilib.DoubleSolenoid.Value.kForward) elif lJoystickButtonStates[0]: # shift left self.DoubleSolenoidOne.set( wpilib.DoubleSolenoid.Value.kReverse) elif xboxButtonStates[2]: # open claw self.DoubleSolenoidTwo.set( wpilib.DoubleSolenoid.Value.kForward) elif xboxButtonStates[1]: # close claw self.DoubleSolenoidTwo.set( wpilib.DoubleSolenoid.Value.kReverse) elif xboxButtonStates[3]: # eject self.DoubleSolenoidThree.set( wpilib.DoubleSolenoid.Value.kForward) elif xboxButtonStates[0]: # retract self.DoubleSolenoidThree.set( wpilib.DoubleSolenoid.Value.kReverse) # lift control if True in self.buttonToggleStatus is False: if xboxButtonStates[4]: # hold self.lift.set(0.05) elif xboxAxisStates[2] > 0.1: # up self.lift.set(xboxAxisStates[2] / 1.5) elif xboxAxisStates[1] > 0.1: # down self.lift.set(-xboxAxisStates[1] * 0.25) else: self.lift.set(0) # four-bar control if xboxButtonStates[5]: self.liftArm.set(0.05) elif not xboxButtonStates[5]: self.liftArm.set(-xboxAxisStates[0] / 4.0) else: self.liftArm.set(0) # cargo intake control if xboxButtonStates[6]: self.cargo.set(0.12) elif xboxAxisStates[4]: # take in self.cargo.set(xboxAxisStates[4] * 0.75) # controller mapping for tank steering rightAxis = rJoystickAxisStates[0] leftAxis = lJoystickAxisStates[0] # drives drive system using tank steering if solenoidStateOne is 1: # if on high gear divisor = 1.2 # then 90% of high speed elif solenoidStateOne is 2: # if on low gear divisor = 1.2 # then normal slow speed else: divisor = 1.0 leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0 rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0 self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2), -rightSign * (1 / divisor) * (rightAxis**2)) def teleopInit(self): ''' Executed at the start of teleop mode. ''' self.drive.setSafetyEnabled(True) # drive train encoder reset self.frontRightMotor.setQuadraturePosition(0, 0) self.frontLeftMotor.setQuadraturePosition(0, 0) # lift encoder rest self.liftEncoder.reset() # compressor self.Compressor.start() def teleopPeriodic(self): ''' Periodically executes methods during the teleop mode. ''' # begin normal periodic # get all required data once per frame # toggle button management per frame if self.buttonBox.getRawButtonPressed(1): self.buttonToggleStatus = [ not self.buttonToggleStatus[0], False, False, False, False, False, False ] elif self.buttonBox.getRawButtonPressed(2): self.buttonToggleStatus = [ False, not self.buttonToggleStatus[1], False, False, False, False, False ] elif self.buttonBox.getRawButtonPressed(3): self.buttonToggleStatus = [ False, False, not self.buttonToggleStatus[2], False, False, False, False ] elif self.buttonBox.getRawButtonPressed(4): self.buttonToggleStatus = [ False, False, False, not self.buttonToggleStatus[3], False, False, False ] elif self.buttonBox.getRawButtonPressed(5): self.buttonToggleStatus = [ False, False, False, False, not self.buttonToggleStatus[4], False, False ] elif self.buttonBox.getRawButtonPressed(6): self.buttonToggleStatus = [ False, False, False, False, False, not self.buttonToggleStatus[5], False ] elif self.buttonBox.getRawButtonPressed(7): self.buttonToggleStatus = [ False, False, False, False, False, False, not self.buttonToggleStatus[6] ] liftTicks = self.liftEncoder.get() hallState = self.Hall.get() compressorState = self.Compressor.enabled() solenoidStateOne = self.DoubleSolenoidOne.get() solenoidStateTwo = self.DoubleSolenoidTwo.get() solenoidStateThree = self.DoubleSolenoidThree.get() # robot ultrasonic self.ultraValue = self.ultrasonic.getVoltage() # cargo ultrasonic self.cargoUltraValue = self.cargoUltrasonic.getVoltage() # xbox value states xboxButtonStates = [ self.xbox.getRawButton(1), self.xbox.getRawButton(2), self.xbox.getRawButton(3), self.xbox.getRawButton(4), self.xbox.getRawButton(5), self.xbox.getRawButton(6), self.xbox.getRawButton(7), self.xbox.getRawButton(8), self.xbox.getRawButton(9), self.xbox.getRawButton(10) ] xboxAxisStates = [ self.xbox.getRawAxis(1), self.xbox.getRawAxis(2), self.xbox.getRawAxis(3), self.xbox.getRawAxis(4), self.xbox.getRawAxis(5) ] # joystick value states rJoystickButtonStates = [self.rightStick.getRawButton(1)] rJoystickAxisStates = [ self.rightStick.getRawAxis(1), self.rightStick.getRawAxis(2), self.rightStick.getRawAxis(3) ] lJoystickButtonStates = [self.leftStick.getRawButton(1)] lJoystickAxisStates = [ self.leftStick.getRawAxis(1), self.leftStick.getRawAxis(2), self.leftStick.getRawAxis(3) ] # define lift stages def cargo_one(): if liftTicks <= 133: # Cargo 1 self.lift.set(0.5) elif liftTicks > 133: self.lift.set(0.05) def cargo_two(): if liftTicks <= 270: # Cargo 2 self.lift.set(0.5) elif liftTicks > 270: self.lift.set(0.05) def cargo_three(): if liftTicks <= 415: # Cargo 3 self.lift.set(0.5) elif liftTicks > 415: self.lift.set(0.05) def hatch_one(): if liftTicks <= 96: # Hatch 1 self.lift.set(0.5) elif liftTicks > 96: self.lift.set(0.05) def hatch_two(): if liftTicks <= 237: # Hatch 2 self.lift.set(0.5) elif liftTicks > 237: self.lift.set(0.05) def hatch_three(): if liftTicks <= 378: # Hatch 3 self.lift.set(0.5) elif liftTicks > 378: self.lift.set(0.05) def lift_encoder_reset(): self.lift.set(0.01) if hallState is True: self.liftEncoder.reset() if self.buttonToggleStatus[0] is True: cargo_three() elif self.buttonToggleStatus[1] is True: hatch_three() elif self.buttonToggleStatus[2] is True: cargo_two() elif self.buttonToggleStatus[3] is True: hatch_two() elif self.buttonToggleStatus[4] is True: cargo_one() elif self.buttonToggleStatus[5] is True: hatch_one() elif self.buttonToggleStatus[6] is True: lift_encoder_reset() # compressor state self.sd.putString("Compressor Status: ", "Enabled" if compressorState is True else "Disabled") # gear state self.sd.putString( "Gear Shift: ", "High Speed" if solenoidStateOne is 1 else "Low Speed") # ejector state self.sd.putString( "Ejector Pins: ", "Ejected" if solenoidStateThree is 2 else "Retracted") # claw state self.sd.putString("Claw: ", "Open" if solenoidStateTwo is 2 else "Closed") # hatch station range state self.sd.putString( "PLAYER STATION RANGE: ", "YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!") # self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue) # hatch spaceship range self.sd.putString( "HATCH RANGE: ", "HATCH IN RANGE" if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE") # compressor if xboxButtonStates[8]: self.Compressor.stop() elif xboxButtonStates[9]: self.Compressor.start() elif rJoystickButtonStates[0]: # shift right self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward) elif lJoystickButtonStates[0]: # shift left self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse) elif xboxButtonStates[2]: # open claw self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward) elif xboxButtonStates[1]: # close claw self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse) elif xboxButtonStates[3]: # eject self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward) elif xboxButtonStates[0]: # retract self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse) # lift control if True in self.buttonToggleStatus is False: if xboxButtonStates[4]: # hold self.lift.set(0.05) elif xboxAxisStates[2] > 0.1: # up self.lift.set(xboxAxisStates[2] / 1.5) elif xboxAxisStates[1] > 0.1: # down self.lift.set(-xboxAxisStates[1] * 0.25) else: self.lift.set(0) # four-bar control if xboxButtonStates[5]: self.liftArm.set(0.05) elif not xboxButtonStates[5]: self.liftArm.set(-xboxAxisStates[0] / 4.0) else: self.liftArm.set(0) # cargo intake control if xboxButtonStates[6]: self.cargo.set(0.12) elif xboxAxisStates[4]: # take in self.cargo.set(xboxAxisStates[4] * 0.75) # controller mapping for tank steering rightAxis = rJoystickAxisStates[0] leftAxis = lJoystickAxisStates[0] # drives drive system using tank steering if solenoidStateOne is 1: # if on high gear divisor = 1.2 # then 90% of high speed elif solenoidStateOne is 2: # if on low gear divisor = 1.2 # then normal slow speed else: divisor = 1.0 leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0 rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0 self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2), -rightSign * (1 / divisor) * (rightAxis**2))
class MyRobot(wpilib.IterativeRobot): # Defines the channels that are used on the inputs.This is really useful when you have a variable used hundreds # of times and you want to have it set so you can change it all in one go. FLChannel = 4 FRChannel = 2 RLChannel = 3 RRChannel = 1 DriveStickChannel = 0 # ExtraStickChannel = 1 # RobotInit is where everything is initialized. def robotInit(self): # Initializes the network table # NetworkTables.initialize(server="10.56.13.2") # self.table = NetworkTables.getTable("limelight") # f is a control constant that will be used to change variable values quickly # self.f = 1 # self.ControlConstant = -0.1 * self.f # self.minCommand = -0.05 * self.f # self.Blue = (RGB value or blue color) # self.Green = (RGB value of green color) # self.Red = (RGB value of red color) # self.Yellow = (RGB value of Yellow color) # Defines the Joystick that we will be using for driving. self.DriveStick = wpilib.Joystick(self.DriveStickChannel) # Initializing drive motors self.FLMotor = wpilib.Spark(self.FLChannel) self.FRMotor = wpilib.Spark(self.FRChannel) self.RLMotor = wpilib.Spark(self.RLChannel) self.RRMotor = wpilib.Spark(self.RRChannel) self.LoaderGrab = ctre.VictorSPX(1) # Puts the motors into groups so that they fit the parameters of the function. self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor) self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor) # The drive function that tells the computer what kind of drive to use and where the motors are. self.drive = DifferentialDrive(self.LMG, self.RMG) # self.ColorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) # Components is a dictionary that contains any variables you want to put into it. All of the variables put into # components dictionary is given to the autonomous modes. # self.components = {"drive": self.drive} # Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous # modes should inherit. # self.automodes = autonomous.AutonomousModeSelector("autonomous", self.components) self.timer = wpilib.Timer() # def autonomousPeriodic(self): # Runs the autonomous mode selector. # self.automodes.run() def operatorControl(self): self.drive.setSafetyEnabled(True) while self.isOperatorControlEnabled(): # Checks to see if you are holding button 2 and if so automatically aims the robot. Else, normal drive. # if self.DriveStick.getrawButton(2): # Tx and Ty are variables marking the angle to the target. # If they are 0 then the calibrated crosshair is right on the target. # tx = self.table.getNumber("tx") # ty = self.table.getNumber("ty") # Angle to the target based off of the angle the camera is mounted at and the angle the camera measures. # targetAngle = 20 + ty # Distance from the target as calculated off of the equation # (TargetHeight - CameraHeight) / Tan(MountAngle + TargetAngle) = D # For a more in-depth explanation look a the limelight docs at docs.limelightvision.io # distance = 8 / tan(targetAngle) # The error in your robot's heading based off of the angle to the target. # headingError = tx # SteerAdjust = 0 # Turns the robot if it is more than one degree off. # if tx > 1.0: # SteerAdjust = self.ControlConstant * headingError + self.minCommand # elif tx < -1.0: # SteerAdjust = self.ControlConstant * headingError - self.minCommand # Uses the calculations to turn the robot. # self.drive.tankDrive( # SteerAdjust, # -SteerAdjust, # squareInputs=False # ) # else: # drives the robot with the arcade drive, which uses one joystick and is a bit easier to use. It is a # part of DifferentialDrive self.drive.arcadeDrive( self.DriveStick.getY(), self.DriveStick.getX(), squareInputs=True ) wpilib.Timer.delay(.005)
class MyRobot(wpilib.IterativeRobot): def robotInit(self): '''Robot initialization function''' # object that handles basic drive operations self.frontLeftMotor = wpilib.Spark(3) self.middleLeftMotor = wpilib.Spark(4) self.rearLeftMotor = wpilib.Spark(5) self.frontRightMotor = wpilib.Spark(0) self.middleRightMotor = wpilib.Spark(1) self.rearRightMotor = wpilib.Spark(2) self.ihigh_motor = wpilib.Spark(6) self.ilow_motor = wpilib.Spark(9) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.middleLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.middleRightMotor, self.rearRightMotor) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) self.high = 0 self.low = 0 self.gameData = 'LRL' # joysticks 1 & 2 on the driver station self.Stick1 = wpilib.XboxController(0) self.Stick2 = wpilib.Joystick(1) self.aSolenoidLow = wpilib.DoubleSolenoid(2,3) self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1) self.iSolenoid = wpilib.DoubleSolenoid(4,5) def autonomousInit(self): self.iSolenoid.set(2) self.aSolenoidLow.set(2) self.aSolenoidHigh.set(2) self.gameData = wpilib.DriverStation.getInstance().getGameSpecificMessage() global timer timer = wpilib.Timer() timer.start() def autonomousPeriodic(self): if self.gameData[0:1] == "L": while timer.get() < 0.3: self.myRobot.tankDrive(0.5,0.5) while timer.get() < 1.2: self.ihigh_motor.set(0.6) # intake self.ilow_motor.set(-0.95) # intake self.aSolenoidLow.set(1) self.myRobot.tankDrive(0.7,0.7) while timer.get() < 3: self.myRobot.tankDrive(0,0) timer.reset() timer.start() while timer.get() < 0.7: # Right = 0.68 //original self.ihigh_motor.set(0.6) # intake self.ilow_motor.set(-0.95) # intake self.myRobot.tankDrive(-0.7,0.7) # Turning right, even though it looks like turning left while timer.get() < 1.2: self.myRobot.tankDrive(0,0) while timer.get() < 2: self.myRobot.tankDrive(0.7,0.7) while timer.get() < 2.5: self.myRobot.tankDrive(0,0) while timer.get() < 5: # Outtake self.ihigh_motor.set(-0.8) self.ilow_motor.set(0.8) self.iSolenoid.set(1) # while timer.get() <= 1: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 2: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 3: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 4: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 6.5: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 7.5: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 8: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 9: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 9.5: # self.myRobot.tankDrive(0.6,0.6) # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # if timer.get() <= 9.2: # self.aSolenoidLow.set(1) # while timer.get() > 11 and timer.get() <= 14: # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # self.aSolenoidHigh.set(1) # while timer.get() >14 and timer.get() <15: # self.iSolenoid.set(1) # self.ihigh_motor.set(-1) # self.ilow_motor.set(1) ''' Waiting to be tested''' # while timer.get() <= 1: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 1.5: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 2.5: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 3: # self.myRobot.tankDrive(0.6,0.6) # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # if timer.get() <= 2.7: # self.aSolenoidLow.set(1) # while timer.get() > 4.5 and timer.get() <= 7.5: # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # self.aSolenoidHigh.set(1) # while timer.get() >7.5 and timer.get() <8.5: # self.iSolenoid.set(1) # self.ihigh_motor.set(-1) # self.ilow_motor.set(1) elif self.gameData[0:1] == 'R': while timer.get() < 0.3: self.myRobot.tankDrive(0.5,0.5) while timer.get() < 1.2: self.ihigh_motor.set(0.6) # intake self.ilow_motor.set(-0.95) # intake self.aSolenoidLow.set(1) self.myRobot.tankDrive(0.7,0.7) # while timer.get() <= 1: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 2: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 3: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 4: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 6.5: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 7.5: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 8: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 9: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 9.5: # self.myRobot.tankDrive(0.6,0.6) # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # if timer.get() <= 9.2: # self.aSolenoidLow.set(1) # while timer.get() > 11 and timer.get() <= 14: # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # self.aSolenoidHigh.set(1) # while timer.get() >14 and timer.get() <15: # self.iSolenoid.set(1) # self.ihigh_motor.set(-1) # self.ilow_motor.set(1) '''Waiting to be tested''' # while timer.get() <= 1: # self.myRobot.tankDrive(0.6,-0.6) # while timer.get() <= 1.5: # self.myRobot.tankDrive(0.6,0.6) # while timer.get() <= 2.5: # self.myRobot.tankDrive(-0.6,0.6) # while timer.get() <= 3: # self.myRobot.tankDrive(0.6,0.6) # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # if timer.get() <= 2.7: # self.aSolenoidLow.set(1) # while timer.get() > 4.5 and timer.get() <= 7.5: # self.ihigh_motor.set(0.7) # self.ilow_motor.set(-0.8) # self.aSolenoidHigh.set(1) # while timer.get() >7.5 and timer.get() <8.5: # self.iSolenoid.set(1) # self.ihigh_motor.set(-1) # self.ilow_motor.set(1) def disabledInit(self): self.myRobot.tankDrive(0,0) self.iSolenoid.set(0) self.aSolenoidLow.set(0) self.aSolenoidHigh.set(0) def disabledPeriodic(self): pass def teleopInit(self): '''Execute at the start of teleop mode''' self.myRobot.setSafetyEnabled(True) def teleopPeriodic(self): if self.isOperatorControl() and self.isEnabled(): minv = 0.4 maxv = 0.6 forward = self.Stick1.getTriggerAxis(1) backward = self.Stick1.getTriggerAxis(0) sp = forward - backward steering = self.Stick1.getX(0) mod = minv + maxv*((1-abs(sp))**2) r = (steering**3)*mod if sp >= 0: self.myRobot.tankDrive(sp*0.85 - r, sp*0.85 + r) else: self.myRobot.tankDrive(sp*0.85 + r, (sp*0.85 - r)) # intake and outtake if self.Stick2.getRawButton(11)==True: # intake self.ihigh_motor.set(0.6) self.ilow_motor.set(-0.95) if self.Stick2.getRawButton(11)==False and self.Stick2.getRawButton(12)==False: self.ihigh_motor.set(0) self.ilow_motor.set(0) if self.Stick2.getRawButton(12)==True: # outtake self.ihigh_motor.set(-0.8) self.ilow_motor.set(0.8) if self.Stick2.getRawButton(9) == True: self.aSolenoidLow.set(1) self.iSolenoid.set(1) if self.Stick2.getRawButton(10) == True: self.aSolenoidLow.set(2) if self.Stick2.getRawButton(7) == True: self.aSolenoidHigh.set(1) if self.Stick2.getRawButton(8) == True: self.aSolenoidHigh.set(2) if self.Stick2.getRawButton(3) == True: self.iSolenoid.set(1) # push intake if self.Stick2.getRawButton(4) == True: self.iSolenoid.set(2) # pull intake
class MyRobot(wpilib.IterativeRobot): def robotInit(self): """Robot initialization function""" LEFT = 1 RIGHT = 2 CENTER1 = 3 CENTER2 = 4 # object that handles basic drive operations self.leftTalon = ctre.WPI_TalonSRX(LEFT) self.rightTalon = ctre.WPI_TalonSRX(RIGHT) self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1) self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2) self.left = wpilib.SpeedControllerGroup(self.leftTalon) self.right = wpilib.SpeedControllerGroup(self.rightTalon) self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1) self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) # joysticks 1 & 2 on the driver station # self.leftStick = wpilib.Joystick(0) # self.rightStick = wpilib.Joystick(1) self.DEADZONE = 0.4 self.LEFT = GenericHID.Hand.kLeft self.RIGHT = GenericHID.Hand.kRight self.driver = wpilib.XboxController(0) def autonomousInit(self): self.myRobot.tankDrive(0.8, 0.8) def autonomousPeriodic(self): self.myRobot.tankDrive(1, 0.5) def teleopInit(self): """Executed at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) def setCenters(self, speed_value): self.center1.set(speed_value) self.center2.set(-speed_value) def deadzone(self, val, deadzone): if abs(val) < deadzone: return 0 return val def teleopPeriodic(self): """Runs the motors with tank steering""" right = self.driver.getY(self.RIGHT) left = self.driver.getY(self.LEFT) self.myRobot.tankDrive(right, left) center_speed = self.driver.getX(self.RIGHT) self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class MyRobot(wp.IterativeRobot): def robotInit(self): '''Robot initialization function''' self.leftMotor1 = wp.VictorSP(1) #motor initialization self.leftMotor2 = wp.VictorSP(3) self.leftMotor3 = wp.VictorSP(5) self.rightMotor1 = wp.VictorSP(2) self.rightMotor2 = wp.VictorSP(4) self.rightMotor3 = wp.VictorSP(6) self.armMotor = wp.VictorSP(7) self.leftIntakeMotor = wp.VictorSP(8) self.rightIntakeMotor = wp.VictorSP(9) self.leftSide = wp.SpeedControllerGroup( self.leftMotor1, self.leftMotor2, self.leftMotor3) #speed controller groups self.rightSide = wp.SpeedControllerGroup(self.rightMotor1, self.rightMotor2, self.rightMotor3) self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor, self.rightIntakeMotor) self.myRobot = DifferentialDrive(self.leftSide, self.rightSide) self.myRobot.setExpiration(0.1) self.leftStick = wp.Joystick(0) self.rightStick = wp.Joystick(1) self.armStick = wp.XboxController(2) self.gyro = wp.ADXRS450_Gyro(0) self.rightEncd = wp.Encoder(2, 3) self.leftEncd = wp.Encoder(0, 1) self.armPot = wp.AnalogPotentiometer(0, 270, -135) self.compressor = wp.Compressor() self.shifter = wp.DoubleSolenoid(0, 1) wp.SmartDashboard.putNumber("Straight Position", 15000) wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000) wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000) wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000) wp.SmartDashboard.putNumber("Left Switch Angle", 90) wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000) wp.SmartDashboard.putNumber("Switch Score Arm Position", 70) wp.SmartDashboard.putNumber("Front Position 1", 74) wp.SmartDashboard.putNumber("Front Position 2", 16) wp.SmartDashboard.putNumber("Front Position 3", -63) wp.SmartDashboard.putNumber("lvl2 Position 1", 60) wp.SmartDashboard.putNumber("lvl2 Position 3", -50) wp.SmartDashboard.putNumber("lvl3 Position 3", -38) wp.SmartDashboard.putNumber("lvl3 Position 1", 45) wp.SmartDashboard.putBoolean("switchScore?", False) self.chooser = wp.SendableChooser() self.chooser.addDefault("None", 4) self.chooser.addObject("Straight/Enc", 1) self.chooser.addObject("Left Side Switch Auto", 2) self.chooser.addObject("Right Side Switch Auto (switch)", 3) self.chooser.addObject("Center Auto", 5) wp.SmartDashboard.putData("Choice", self.chooser) wp.CameraServer.launch("vision.py:main") def robotPeriodic(self): wp.SmartDashboard.putNumber("Gyro:", round(self.gyro.getAngle(), 2)) wp.SmartDashboard.putNumber("Right Encoder:", self.rightEncd.get()) wp.SmartDashboard.putNumber("Left Encoder:", self.leftEncd.get()) wp.SmartDashboard.putNumber("Arm Postition", self.armPot.get()) calGyro = wp.SmartDashboard.getBoolean("calGyro:", True) resetGyro = wp.SmartDashboard.getBoolean("resetGyro:", True) encodeReset = wp.SmartDashboard.getBoolean("resetEnc:", True) self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True) self.straightPos = wp.SmartDashboard.getNumber("Straight Position", 4000) self.leftAutoPos1 = wp.SmartDashboard.getNumber( "Left Switch Pos 1", 4000) self.leftAutoPos2 = wp.SmartDashboard.getNumber( "Left Switch Angle", 90) self.leftAutoPos3 = wp.SmartDashboard.getNumber( "Left Switch Pos 2", 4000) self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78) self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22) self.frontArmPos3 = wp.SmartDashboard.getNumber( "Front Position 3", -63) self.frontArmLvl2Pos1 = wp.SmartDashboard.getNumber( "lvl2 Position 1", 68) self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber( "lvl2 Position 3", -50) self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber( "lvl3 Position 3", -38) self.frontArmLvl3Pos1 = wp.SmartDashboard.getNumber( "lvl3 Position 1", 57) if (resetGyro): self.gyro.reset() wp.SmartDashboard.putBoolean("resetGyro:", False) if (calGyro): self.gyro.calibrate() wp.SmartDashboard.putBoolean("calGyro:", False) if (encodeReset): self.rightEncd.reset() self.leftEncd.reset() wp.SmartDashboard.putBoolean("resetEnc:", False) self.auto = self.chooser.getSelected() def autonomousInit(self): self.auto = self.chooser.getSelected() self.leftMotorVal = 0 self.rightMotorVal = 0 self.gyroFuncGain = 40 self.angleFuncGain = 40 self.leftMotVal = 0 self.rightMotVal = 0 self.armSet = 0 self.intakeSet = 0 self.posGain = 18 self.armSpeed = 1 self.armPos = self.armPot.get() self.startTime = tm.time() self.gamePlacement = wp.DriverStation.getInstance( ).getGameSpecificMessage() self.straightPos = wp.SmartDashboard.getNumber("Straight Position", 16000) self.leftAutoPos1 = wp.SmartDashboard.getNumber( "Left Switch Pos 1", 4000) self.leftAutoPos2 = wp.SmartDashboard.getNumber( "Left Switch Angle", 90) self.leftAutoPos3 = wp.SmartDashboard.getNumber( "Left Switch Pos 2", 4000) self.leftMiddleAutoStraight = wp.SmartDashboard.getNumber( "leftMiddleAutoStraight", 21) self.leftMiddleAutoLateral = wp.SmartDashboard.getNumber( "leftMiddleAutoLateral", 21) self.switchScoreArmPos = wp.SmartDashboard.getNumber( "Switch Score Arm Position", 70) self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78) self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22) self.frontArmPos3 = wp.SmartDashboard.getNumber( "Front Position 3", -63) self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True) self.StartButtonPos = wp.SmartDashboard.getNumber( "lvl3 Position 1", 57) self.stage1 = True self.stage2 = False self.stage3 = False self.stage4 = False self.stage5 = False self.stage6 = False def autonomousPeriodic(self): #autonomous code will go here armPos = self.armPot.get() self.gamePlacement = wp.DriverStation.getInstance( ).getGameSpecificMessage() if (self.auto == 1): if (abs(self.rightEncd.get()) < self.straightPos and self.stage1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = 0, 0 if (self.switchScore and self.gamePlacement != "" and self.gamePlacement == "RRR" or self.gamePlacement == "RRL" or self.gamePlacement == "RLR" or self.gamePlacement == "RLL"): self.intakeSet = -1 else: self.instakeSet = 0 if (self.auto == 2): #Guidlines for switch placement on left side: #check switch placement #Move Forward #Turn right 90 degrees #Move Forward to wall #place Cube if (self.gamePlacement != "" and self.gamePlacement == "LLL" or self.gamePlacement == "LRL" or self.gamePlacement == "LLR" or self.gamePlacement == "LRR"): if (abs(self.rightEncd.get()) < self.leftAutoPos1 and self.stage1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.gyro.getAngle() < self.leftAutoPos2 + 0.1 and self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = rf.angleFunc( self.gyro.getAngle(), self.leftAutoPos2, self.angleFuncGain) self.leftEncd.reset() self.rightEncd.reset() self.stage3 = True elif (abs(self.rightEncd.get()) < self.leftAutoPos3 and self.stage3): self.stage2 = False self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), self.leftAutoPos2, 0.65, self.gyroFuncGain) else: self.leftMotVal, self.rightMotVal = 0, 0 self.intakeSet = -1 else: if (abs(self.rightEncd.get()) <= self.leftAutoPos1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = 0, 0 if (self.auto == 3): if (self.gamePlacement != "" and self.gamePlacement == "RRR" or self.gamePlacement == "RRL" or self.gamePlacement == "RLR" or self.gamePlacement == "RLL"): if (abs(self.rightEncd.get()) < self.leftAutoPos1 and self.stage1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1 and self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = rf.angleFunc( self.gyro.getAngle(), (self.leftAutoPos2 * -1), self.angleFuncGain) self.leftEncd.reset() self.rightEncd.reset() self.stage3 = True elif (abs(self.rightEncd.get()) < self.leftAutoPos3 and self.stage3): self.stage2 = False self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65, self.gyroFuncGain) else: self.leftMotVal, self.rightMotVal = 0, 0 self.intakeSet = -1 else: if (abs(self.rightEncd.get()) <= self.leftAutoPos1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = 0, 0 if (self.auto == 5): if (self.gamePlacement != "" and self.gamePlacement == "LLL" or self.gamePlacement == "LRL" or self.gamePlacement == "LLR" or self.gamePlacement == "LRR"): if (abs(self.rightEncd.get()) < (self.leftMiddleAutoStraight / 2) and self.stage1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) self.stage2 = True elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1 and self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = rf.angleFunc( self.gyro.getAngle(), (self.leftAutoPos2 * -1), self.angleFuncGain) self.leftEncd.reset() self.rightEncd.reset() self.stage3 = True elif (abs(self.rightEncd.get()) < self.leftMiddleAutoLateral and self.stage3): self.stage2 = False self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65, self.gyroFuncGain) self.stage4 = True elif (self.gyro.getAngle() < (0) + 0.1 and self.stage4): self.stage3 = False self.leftMotVal, self.rightMotVal = rf.angleFunc( self.gyro.getAngle(), 0, self.angleFuncGain) self.leftEncd.reset() self.rightEncd.reset() self.stage5 = True elif (abs(self.rightEncd.get()) < (self.leftMiddleAutoStraight / 1.50) and self.stage5): self.stage4 = False self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) if (armPos < (self.StartButtonPos - 1) or armPos > (self.StartButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.StartButtonPos, self.armSpeed, self.posGain) else: self.armSet = 0 else: self.leftMotVal, self.rightMotVal = 0, 0 self.intakeSet = -1 else: if (abs(self.rightEncd.get()) < self.straightPos and self.stage1): self.leftMotVal, self.rightMotVal = rf.gyroFunc( self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain) if (armPos < (self.StartButtonPos - 1) or armPos > (self.StartButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.StartButtonPos, self.armSpeed, self.posGain) else: self.armSet = 0 if (abs(self.rightEncd.get()) > (self.straightPos - 1000)): self.intakeSet = -1 self.stage2 = True elif (self.stage2): self.stage1 = False self.leftMotVal, self.rightMotVal = 0, 0 self.intakeSet = -1 self.leftIntakeMotor.set(self.intakeSet) self.rightIntakeMotor.set(self.intakeSet * -1) self.armMotor.set(self.armSet) self.myRobot.tankDrive(self.rightMotVal, self.leftMotVal) def teleopInit(self): self.myRobot.setSafetyEnabled(True) self.pastFrontFlipButton = False self.flip = True self.armSet = 0 self.shiftSet = 1 self.posGain = 27 self.posGain2 = 27 self.armSpeed = 1 self.lastXButton = False self.lastYButton = False self.lastBButton = False self.lastAButton = False self.lastLBumper = False self.lastRBumper = False self.lastStartButton = False self.XButtonPos = wp.SmartDashboard.getNumber("Front Position 1", 77.6) self.YButtonPos = wp.SmartDashboard.getNumber("Front Position 2", 22) self.BButtonPos = wp.SmartDashboard.getNumber("Front Position 3", -62.8) self.AButtonPos = wp.SmartDashboard.getNumber("lvl2 Position 1", 30) self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber( "lvl2 Position 3", -50) self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber( "lvl3 Position 3", -38) self.StartButtonPos = wp.SmartDashboard.getNumber( "lvl3 Position 1", 57) #wp.SmartDashboard.getNumber("Back Position 1", -90) #wp.SmartDashboard.getNumber("Back Position 2", -45) #wp.SmartDashboard.getNumber("Back Position 3", -30) def teleopPeriodic(self): #Joystick Variables leftJoyValY = self.leftStick.getY() rightJoyValY = self.rightStick.getY() armJoyValY = self.armStick.getRawAxis(3) frontFlipButton = self.rightStick.getRawButton(2) armPos = self.armPot.get() highShiftButton = self.rightStick.getRawButton(4) lowShiftButton = self.rightStick.getRawButton(5) self.compressorButtonOn = self.rightStick.getRawButton(9) self.compressorButtonOff = self.rightStick.getRawButton(8) self.intakeInButton = self.armStick.getRawButton(7) self.intakeOutButton = self.armStick.getRawButton(8) self.buttonX = self.armStick.getRawButton(1) self.buttonY = self.armStick.getRawButton(4) self.buttonB = self.armStick.getRawButton(3) self.buttonA = self.armStick.getRawButton(2) self.buttonStart = self.armStick.getRawButton(10) self.buttonLBumper = self.armStick.getRawButton(5) self.buttonRBumper = self.armStick.getRawButton(6) #Automatic arm positioning if (self.buttonX == True and self.lastXButton == False): self.lastXButton = True self.lastYButton = False self.lastBButton = False self.lastAButton = False self.lastStartButton = False if (self.buttonY == True and self.lastYButton == False): self.lastXButton = False self.lastYButton = True self.lastBButton = False self.lastAButton = False self.lastStartButton = False if (self.buttonB == True and self.lastBButton == False): self.lastXButton = False self.lastYButton = False self.lastBButton = True self.lastAButton = False self.lastStartButton = False if (self.buttonA and self.lastAButton == False): self.lastXButton = False self.lastYButton = False self.lastBButton = False self.lastAButton = True self.lastStartButton = False if (self.buttonStart and self.lastStartButton == False): self.lastXButton = False self.lastYButton = False self.lastBButton = False self.lastAButton = False self.lastStartButton = True if (armJoyValY > 0.075 or armJoyValY < -0.075): self.lastXButton = False self.lastYButton = False self.lastBButton = False if (self.lastXButton): if (armPos < (self.XButtonPos - 1) or armPos > (self.XButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.XButtonPos, self.armSpeed, self.posGain) else: self.lastXButton = False elif (self.lastYButton): if (armPos < (self.YButtonPos - 1) or armPos > (self.YButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.YButtonPos, self.armSpeed, self.posGain) else: self.lastYButton = False elif (self.lastBButton): if (armPos < (self.BButtonPos - 1) or armPos > (self.BButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.BButtonPos, self.armSpeed, self.posGain) else: self.lastBButton = False elif (self.lastAButton): if (armPos < (self.AButtonPos - 1) or armPos > (self.AButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.AButtonPos, self.armSpeed, self.posGain) else: self.lastAButton = False elif (self.lastStartButton): if (armPos < (self.StartButtonPos - 1) or armPos > (self.StartButtonPos + 1)): self.armSet = rf.goToPos(armPos, self.StartButtonPos, self.armSpeed, self.posGain) else: self.lastBButton = False else: self.armSet = (armJoyValY * 0.75) #Intake Motor Control if (self.intakeInButton): self.intakeSet = 1 elif (self.intakeOutButton): self.intakeSet = -1 else: self.intakeSet = 0 if (highShiftButton): self.shiftSet = 1 elif (lowShiftButton): self.shiftSet = 2 #FrontFlip if (self.pastFrontFlipButton == False and frontFlipButton): self.flip = not self.flip self.pastFrontFlipButton = frontFlipButton leftMotVal, rightMotVal = rf.flip(self.flip, leftJoyValY, rightJoyValY) self.armMotor.set(self.armSet) self.shifter.set(self.shiftSet) self.leftIntakeMotor.set(self.intakeSet) self.rightIntakeMotor.set(self.intakeSet * -1) self.myRobot.tankDrive(rightMotVal, leftMotVal)
class MyRobot(wpilib.IterativeRobot): def robotInit(self): """Robot initialization function""" # object that handles basic drive operations self.left = wpilib.Victor(0) self.right = wpilib.Victor(1) self.gyro = wpilib.AnalogGyro(0) self.gyro.reset() self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) NetworkTables.initialize(server='127.0.0.1') self.smartdash = NetworkTables.getTable('SmartDashboard') self.gearshift = wpilib.DoubleSolenoid(0, 0, 1) wpilib.CameraServer.launch("vision.py:main") self.ll = NetworkTables.getTable("limelight") self.ll.putNumber('ledStatus', 1) # joysticks 1 & 2 on the driver station self.leftStick = wpilib.Joystick(2) self.rightStick = wpilib.Joystick(1) self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5) self.smartdash.putNumber('tx', 1) self.gearshift.set(1) self.pdp = wpilib.PowerDistributionPanel(0) self.accelerometer = wpilib.BuiltInAccelerometer() self.gyro.initSendable self.myRobot.initSendable self.gearshift.initSendable self.pdp.initSendable self.accelerometer.initSendable self.acc = wpilib.AnalogAccelerometer(3) self.setpoint = 90.0 self.P = .3 self.I = 0 self.D = 0 self.integral = 0 self.previous_error = 0 self.rcw = 0 #wpilib.DriverStation.reportWarning(str(self.myRobot.isRightSideInverted()),False) def PID(self): error = self.setpoint - self.gyro.getAngle() self.integral = self.integral + (error * .02) derivative = (error - self.previous_error) / .02 self.rcw = self.P * error + self.I * self.integral + self.D * derivative def autonomousInit(self): self.myRobot.setSafetyEnabled(False) def autonomousPeriodic(self): pass def teleopInit(self): """Executed at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) def teleopPeriodic(self): """Runs the motors with tank steering""" self.myRobot.arcadeDrive(self.rightStick.getY(), -self.rightStick.getZ() * .4) self.smartdash.putNumber('gyrosensor', self.gyro.getAngle()) if (self.rightStick.getRawButton(4)): self.ll.putNumber('ledMode', 1) if (self.rightStick.getRawButton(3)): self.right.set(.5) if (self.trigger.get()): if (self.gearshift.get() == 1): self.gearshift.set(2) elif (self.gearshift.get() == 2): self.gearshift.set(1) if (self.rightStick.getRawButton(2)): self.tx = self.ll.getNumber("tx", None) if (not type(self.tx) == type(0.0)): self.tx = 0.0 if (self.tx > 1.0): self.myRobot.arcadeDrive(0, self.tx * .03) elif (self.tx < -1.0): self.myRobot.tankDrive(0, self.tx * .03) else: self.myRobot.tankDrive(0, 0) self.pdp.getVoltage()
class Chassis: # Variable Injection right_master: WPI_TalonSRX left_master: WPI_TalonSRX right_slave: WPI_TalonSRX left_slave: WPI_TalonSRX gyro: AHRS # enabled = will_reset_to(False) def __init__(self): self.teleop = False self.auto = False self.z_speed = 0 self.y_speed = 0 def setup(self): # Setting Masters self.left_slave.follow(self.left_master) self.right_slave.follow(self.right_master) # Setting safety self.left_master.setSafetyEnabled(False) self.left_slave.setSafetyEnabled(False) self.right_master.setSafetyEnabled(False) self.right_slave.setSafetyEnabled(False) # Setting inversion self.left_master.setInverted(False) self.left_slave.setInverted(False) self.right_master.setInverted(False) self.right_slave.setInverted(False) # Setting the Encoders and configuring the Talons self.right_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) self.left_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) self.left_master.configVoltageCompSaturation(11) self.right_master.configVoltageCompSaturation(11) self.left_master.enableVoltageCompensation(True) self.right_master.enableVoltageCompensation(True) # Creating a drive self.drive = DifferentialDrive(self.left_master, self.right_master) self.drive.setSafetyEnabled(True) def set_teleop(self): """ Sets current mode for teleop. """ self.teleop = True self.auto = False def set_auto(self): """ Sets current mode for auto. """ self.teleop = False self.auto = True def set_motors_values(self, left_power: float, right_power: float): """ Sets the current motor power. """ # print("motors", left_power, right_power) self.left_master.set(left_power) self.right_master.set(right_power) def get_right_encoder(self): """ Returns the right encoder position. """ return self.right_master.getSelectedSensorPosition() def get_left_encoder(self): """ Returns the left encoder position. """ return -self.left_master.getSelectedSensorPosition() def set_right_motor(self, power: float): """ Sets the velocity of the right motor. """ self.right_master.set(power) def set_left_motor(self, power: float): """ Sets the velocity of the left motor. """ self.left_master.set(power) def set_speed(self, y_speed, z_speed): """ Sets the speed of the robot. """ self.y_speed = y_speed self.z_speed = z_speed def set_drive(self, y_speed, z_speed): """ Sets the speed of the robot. """ self.drive.arcadeDrive(y_speed, z_speed) def reset_encoders(self): """ Resets the values of both left and right encoders. """ self.left_master.setSelectedSensorPosition(0) self.right_master.setSelectedSensorPosition(0) def get_angle(self): """ Returns Current robot angle. """ return self.gyro.getAngle() def get_encoder_ticks(self): """ Returns the current encoder ticks. """ left_pos = self.left_master.getSelectedSensorPosition() right_pos = self.right_master.getSelectedSensorPosition() return left_pos, right_pos def disable(self): """ Disables the chassis. """ self.teleop = False self.auto = False def reset_navx(self): """ Resets the navx. """ self.gyro.zeroYaw() def is_auto(self): """ Returns True if current 'state' is auto, else returns False""" return self.auto def execute(self): if self.teleop: self.drive.arcadeDrive(self.y_speed, self.z_speed)
class MyRobot(wpilib.TimedRobot): def robotInit(self): ''' Initialization of robot objects. ''' ''' Talon SRX Initialization ''' # drive train motors self.frontRightMotor = WPI_TalonSRX(4) self.rearRightMotor = WPI_TalonSRX(3) self.frontLeftMotor = WPI_TalonSRX(1) self.rearLeftMotor = WPI_TalonSRX(2) ''' Motor Groups ''' # drive train motor groups self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) # drive train drive group self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) ''' Victor SPX Initialization ''' # lift motors self.liftOne = WPI_VictorSPX(1) self.liftTwo = WPI_VictorSPX(2) self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo) # lift arm motors self.liftArmOne = WPI_VictorSPX(3) self.liftArmTwo = WPI_VictorSPX(4) self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne, self.liftArmTwo) # cargo intake motor self.cargo = WPI_VictorSPX(5) ''' Encoders ''' # drive train encoders self.rightEncoder = self.frontRightMotor self.leftEncoder = self.frontLeftMotor # lift encoder self.liftEncoder = wpilib.Encoder(8, 9) # liftArm encoder self.liftArmEncoder = wpilib.Encoder(5, 6, True) ''' Sensors ''' # Hall Effect Sensor self.minHall = wpilib.DigitalInput(7) self.maxHall = wpilib.DigitalInput(4) self.limitSwitch = wpilib.DigitalInput(3) self.ultrasonic = wpilib.AnalogInput(2) self.cargoUltrasonic = wpilib.AnalogInput(3) ''' Controller Initialization and Mapping ''' # joystick - 0, 1 | controller - 2 self.joystick = wpilib.Joystick(1) self.xbox = wpilib.Joystick(2) ''' Pneumatic Button Status ''' self.clawButtonStatus = Toggle(self.xbox, 2) self.gearButtonStatus = Toggle(self.joystick, 1) self.ejectorPinButtonStatus = Toggle(self.xbox, 1) self.compressorButtonStatus = Toggle(self.xbox, 9) self.liftHeightButtonStatus = Toggle(self.xbox, 3) ''' Pneumatic Initialization ''' self.Compressor = wpilib.Compressor(0) self.Compressor.setClosedLoopControl(True) self.enable = self.Compressor.getPressureSwitchValue() self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2, 3) # hatch panel claw self.DoubleSolenoidThree = wpilib.DoubleSolenoid( 4, 5) # hatch panel ejection self.Compressor.start() ''' Smart Dashboard ''' # connection for logging & Smart Dashboard logging.basicConfig(level=logging.DEBUG) self.sd = NetworkTables.getTable('SmartDashboard') NetworkTables.initialize(server='10.55.49.2') self.sd.putString(" ", "Connection") # Smart Dashboard classes self.PDP = wpilib.PowerDistributionPanel() self.roboController = wpilib.RobotController() self.DS = wpilib.DriverStation.getInstance() ''' Timer ''' self.timer = wpilib.Timer() ''' Camera ''' # initialization of the HTTP camera wpilib.CameraServer.launch('vision.py:main') self.sd.putString("", "Top Camera") self.sd.putString(" ", "Bottom Camera") ''' PID settings for lift ''' self.kP = 0.03 self.kI = 0.0 self.kD = 0.0 self.kF = 0.1 self.PIDLiftcontroller = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.liftEncoder, output=self) self.PIDLiftcontroller.setInputRange(0, 400) self.PIDLiftcontroller.setOutputRange(-0.5, 0.5) self.PIDLiftcontroller.setAbsoluteTolerance(1.0) self.PIDLiftcontroller.setContinuous(True) self.encoderRate = 0 def pidWrite(self, output): self.encoderRate = output def robotCode(self): if self.liftHeightButtonStatus.on: self.PIDLiftcontroller.setSetpoint(200) self.liftToHeight = True elif self.liftHeightButtonStatus.off: self.PIDLiftcontroller.setSetpoint(0) self.liftToHeight = False def hatchOne(): if self.liftEncoder.getDistance() < 80: # Hatch 2 self.lift.set(0.3) elif self.liftEncoder.getDistance() >= 80: self.lift.set(0.07) def hatchTwo(): if self.liftEncoder.getDistance() < 275: # Hatch 2 self.lift.set(0.5) elif self.liftEncoder.getDistance() >= 275: self.lift.set(0.07) def cargoOne(): if self.liftEncoder.getDistance() < 150: # Cargo 1 self.lift.set(0.5) elif self.liftEncoder.getDistance() >= 150: self.lift.set(0.05) def cargoTwo(): if self.liftEncoder.getDistance() < 320: # Cargo 2 self.lift.set(0.5) elif self.liftEncoder.getDistance() >= 320: self.lift.set(0.05) def cargoShip(): if self.liftEncoder.getDistance() < 280: # Cargo ship self.lift.set(0.5) elif self.liftEncoder.getDistance() >= 280: self.lift.set(0.07) # ''' Button Box Level Mapping ''' # if self.buttonStatusOne.on: # # hatchOne() # cargoOne() # elif self.buttonStatusTwo.on: # comment out for hatch # cargoTwo() # elif self.buttonStatusThree.on: # # hatchTwo() # cargoShip() if self.minHall.get() is False: self.liftEncoder.reset() if self.limitSwitch.get() is False: self.liftArmEncoder.reset() ''' Smart Dashboard ''' # compressor state if self.Compressor.enabled() is True: self.sd.putString("Compressor Status: ", "Enabled") elif self.Compressor.enabled() is False: self.sd.putString("Compressor Status: ", "Disabled") ''' Pneumatics Dashboard States ''' # gear state if self.DoubleSolenoidOne.get() == 1: self.sd.putString("Gear Shift: ", "HIGH SPEED!!!") elif self.DoubleSolenoidOne.get() == 2: self.sd.putString("Gear Shift: ", "Low") # ejector state if self.DoubleSolenoidThree.get() == 2: self.sd.putString("Ejector Pins: ", "Ejected") elif self.DoubleSolenoidThree.get() == 1: self.sd.putString("Ejector Pins: ", "Retracted") # claw state if self.DoubleSolenoidTwo.get() == 2: self.sd.putString("Claw: ", "Open") elif self.DoubleSolenoidTwo.get() == 1: self.sd.putString("Claw: ", "Closed") ''' Ultrasonic Range Detection ''' # robot ultrasonic self.ultraValue = self.ultrasonic.getVoltage() if 0.142 <= self.ultraValue <= 0.146: self.sd.putString("PLAYER STATION RANGE: ", "YES!!!!") else: self.sd.putString("PLAYER STATION RANGE: ", "NO!") # cargo ultrasonic self.cargoUltraValue = self.cargoUltrasonic.getVoltage() if 0.70 <= self.cargoUltraValue <= 1.56: self.sd.putString("HATCH RANGE: ", "HATCH IN RANGE") else: self.sd.putString("HATCH RANGE: ", "NOT IN RANGE") ''' Pneumatics Toggles ''' # Compressor if self.compressorButtonStatus.on: self.Compressor.start() elif self.compressorButtonStatus.off: self.Compressor.stop() # Claw Toggle if self.clawButtonStatus.on: self.DoubleSolenoidTwo.set( wpilib.DoubleSolenoid.Value.kForward) # open claw elif self.clawButtonStatus.off: self.DoubleSolenoidTwo.set( wpilib.DoubleSolenoid.Value.kReverse) # close claw # Ejector Pins Toggle if self.ejectorPinButtonStatus.on: self.DoubleSolenoidThree.set( wpilib.DoubleSolenoid.Value.kForward) # eject elif self.ejectorPinButtonStatus.off: self.DoubleSolenoidThree.set( wpilib.DoubleSolenoid.Value.kReverse) # retract # Gear Shift Toggle if self.gearButtonStatus.on: self.DoubleSolenoidOne.set( wpilib.DoubleSolenoid.Value.kForward) # shift right elif self.gearButtonStatus.off: self.DoubleSolenoidOne.set( wpilib.DoubleSolenoid.Value.kReverse) # shift left ''' Victor SPX (Lift, Lift Arm, Cargo) ''' # lift control if self.liftHeightButtonStatus.get() is False: if self.xbox.getRawButton(5): # hold self.lift.set(0.05) elif self.xbox.getRawAxis(3): # up self.lift.set(self.xbox.getRawAxis(3) * 0.85) elif self.xbox.getRawAxis(2): # down self.lift.set(-self.xbox.getRawAxis(2) * 0.45) else: self.lift.set(0) # else: # if self.liftToHeight is True: # self.PIDLiftcontroller.enable() # self.liftHeight = self.encoderRate # self.lift.set(self.liftHeight) # else: # self.PIDLiftcontroller.disable() # self.lift.set(0) # # four-bar control # if self.xbox.getRawButton(6): # hold # self.liftArm.set(0.12) # elif not self.xbox.getRawButton(6): # self.liftArm.set(-self.xbox.getRawAxis(1) * 0.35) # else: # self.liftArm.set(0) # cargo intake control if self.xbox.getRawButton(7): # hold self.cargo.set(0.12) elif self.xbox.getRawAxis(5): # take in self.cargo.set(self.xbox.getRawAxis(5) * 0.75) # controller mapping for arcade steering self.driveAxis = self.joystick.getRawAxis(1) self.rotateAxis = self.joystick.getRawAxis(2) # drives drive system using tank steering if self.DoubleSolenoidOne.get() == 1: # if on high gear self.divisor = 1.0 # 90% of high speed self.turnDivisor = 0.8 elif self.DoubleSolenoidOne.get() == 2: # if on low gear self.divisor = 0.85 # normal slow speed self.turnDivisor = 0.75 else: self.divisor = 1.0 if self.driveAxis != 0: self.leftSign = self.driveAxis / fabs(self.driveAxis) else: self.leftSign = 0 if self.rotateAxis != 0: self.rightSign = self.rotateAxis / fabs(self.rotateAxis) else: self.rightSign = 0 self.drive.arcadeDrive(-self.driveAxis * self.divisor, self.rotateAxis * 0.75) def autonomousInit(self): ''' Executed each time the robot enters autonomous. ''' # timer config self.timer.reset() self.timer.start() # drive train encoder reset self.rightEncoder.setQuadraturePosition(0, 0) self.leftEncoder.setQuadraturePosition(0, 0) self.liftEncoder.reset() self.liftArmEncoder.reset() def autonomousPeriodic(self): self.sd.putBoolean("LIFT RESET ", self.minHall.get()) ''' Called periodically during autonomous. ''' '''Test Methods''' def encoder_test(): ''' Drives robot set encoder distance away ''' self.rightPos = fabs(self.rightEncoder.getQuadraturePosition()) self.leftPos = fabs(self.leftEncoder.getQuadraturePosition()) self.distIn = (( (self.leftPos + self.rightPos) / 2) / 4096) * 18.84955 if 0 <= self.distIn <= 72: self.drive.tankDrive(0.5, 0.5) else: self.drive.tankDrive(0, 0) def Diagnostics(): ''' Smart Dashboard Tests''' self.sd.putNumber("Temperature: ", self.PDP.getTemperature()) self.sd.putNumber("Battery Voltage: ", self.roboController.getBatteryVoltage()) self.sd.putBoolean(" Browned Out?", self.roboController.isBrownedOut) # Smart Dashboard diagnostics self.sd.putNumber("Right Encoder Speed: ", abs(self.rightEncoder.getQuadratureVelocity())) self.sd.putNumber("Left Encoder Speed: ", abs(self.leftEncoder.getQuadratureVelocity())) self.sd.putNumber("Lift Encoder: ", self.liftEncoder.getDistance()) def Pressure(): self.Compressor.start() ''' Test Execution ''' if self.DS.getGameSpecificMessage() == "pressure": Pressure() elif self.DS.getGameSpecificMessage() == "diagnostics": Diagnostics() self.robotCode() def teleopInit(self): ''' Executed at the start of teleop mode. ''' self.drive.setSafetyEnabled(True) # drive train encoder reset self.rightEncoder.setQuadraturePosition(0, 0) self.leftEncoder.setQuadraturePosition(0, 0) # lift encoder rest self.liftEncoder.reset() # compressor self.Compressor.start() def teleopPeriodic(self): ''' Periodically executes methods during the teleop mode. ''' self.robotCode()
class Drivetrain: def __init__(self): self.lDriveF = WPI_TalonSRX(TALON_DRIVE_LF) self.lDriveR = WPI_TalonSRX(TALON_DRIVE_LR) self.rDriveF = WPI_TalonSRX(TALON_DRIVE_RF) self.rDriveR = WPI_TalonSRX(TALON_DRIVE_RR) #self.rDriveF.setInverted(True) #self.rDriveR.setInverted(True) self.lSpeedGroup = wpilib.SpeedControllerGroup(self.lDriveF, self.lDriveR) self.rSpeedGroup = wpilib.SpeedControllerGroup(self.rDriveF, self.rDriveR) self.drive = DifferentialDrive(self.lSpeedGroup, self.rSpeedGroup) self.drive.setSafetyEnabled(False) self.drivePID = ProtoPID(1/5.0, 0.0, 0.0, 0.0, 0.02) def arcadeDrive(self, speed: float, angle: float) -> None: #print(f'arcade drive: {speed}, {angle}') self.drive.setSafetyEnabled(True) self.drive.arcadeDrive(speed, angle) ''' encoder functions ''' def resetEncoders(self) -> None: self.lDriveF.setSelectedSensorPosition(0) self.rDriveR.setSelectedSensorPosition(0) def getDistance(self) -> float: return (-self.lDriveF.getSelectedSensorPosition(0) + \ self.rDriveR.getSelectedSensorPosition(0)) \ / 2 / DRIVE_ENCODER_COUNTS_PER_FOOT def getTurningDistance(self) -> float: return (fabs(-lDriveF.getSelectedSensorPosition(0)) + \ abs(rDriveR.getSelectedSensorPosition(0))) \ / 2 / DRIVE_ENCODER_COUNTS_PER_FOOT def getRDistance(self) -> float: return self.rDriveR.getSelectedSensorPosition(0) / DRIVE_ENCODER_COUNTS_PER_FOOT def getLDistance(self) -> float: return -self.lDriveF.getSelectedSensorPosition(0) / DRIVE_ENCODER_COUNTS_PER_FOOT def getRVelocity(self) -> float: return self.rDriveR.getSelectedSensorVelocity(0) def getLVelocity(self) -> float: return -self.lDriveR.getSelectedSensorVelocity(0) def getYaw(self) -> float: pass def encoderWrite(self, rightDistance, leftDistance) -> None: pass ''' auton functions ''' def autonDrivetrain(self, rVelocity: float, lVelocity: float, rDistance: float, lDistance: float) -> bool: self.drive.setSafetyEnabled(False) if fabs(self.getRDistance()) < rDistance: self.rDriveF.set(-rVelocity) self.rDriveR.set(-rVelocity) elif fabs(self.getRDistance()) < rDistance: self.rDriveF.set(-.1) self.rDriveR.set(-.1) else: self.rDriveF.set(0) self.rDriveR.set(0) if fabs(self.getLDistance()) < (lDistance - .2): self.lDriveF.set(lVelocity) self.lDriveR.set(lVelocity) elif fabs(self.getLDistance()) < lDistance: self.lDriveF.set(.1) self.lDriveR.set(.1) else: self.lDriveF.set(0) self.lDriveR.set(0) if fabs(self.getRDistance()) >= rDistance and fabs(self.getLDistance()) >= lDistance: return True else: return False def autonLimeDrive(self, speed: float, angle: float, area: float) -> bool: self.drive.arcadeDrive(speed, angle) if area > 8: return True else: return False def autonTurning(self, distance: float) -> bool: angle = 0 if distance > 0: angle = .5 else: angle = -.5 if self.getTurningDistance() < distance: self.drive.arcadeDrive(0, angle) else: self.drive.arcadeDrive(0, 0) return True return False def autonStraight(self, distance: float) -> bool: speed = 0 if distance > 0: speed = .6 else: speed = -.6 if self.getDistance() < distance: self.drive.arcadeDrive(speed , 0) else: self.drive.arcadeDrive(0,0) return True return False def autonPID(self, distance: float) -> bool: if fabs(self.getDistance() < fabs(distance)): command = self.drivePID.compute(self.getDistance(), distance) bias = 0 if distance > 0: bias = .22 else: bias = -.22 self.drive.arcadeDrive(command + bias, 0) else: self.drive.arcadeDrive(0,0) return True def velocityMultiplier(self, firstV: float, secondV: float, firstEncoderSpeed: float, secondEncoderSpeed: float) -> float: ratio = firstV / secondV vRatio = firstEncoderSpeed / secondEncoderSpeed if (vRatio / ratio) < 1: return (ratio / vRatio) * firstV else: return 1.0 * firstV '''teleop PID drive for testing''' def pidDrive(self, angle: float, distance: float) -> float: command = self.drivePID.compute(self.getDistance(), distance) bias = 0 if distance > 0: bias = .22 else: bias = -.22 self.drive.arcadeDrive(command + bias, angle)
class Apollo(wpilib.TimedRobot): def robotInit(self): self.xbox = wpilib.XboxController(0) self.lift_lock = Toggle(self.xbox, 4, 0.5) self.left_drive_motor_group = wpilib.SpeedControllerGroup( wpilib.Talon(0), wpilib.Talon(1)) self.right_drive_motor_group = wpilib.SpeedControllerGroup( wpilib.Talon(2), wpilib.Talon(3)) self.drive = DifferentialDrive(self.left_drive_motor_group, self.right_drive_motor_group) self.drive_rev = False self.speedRatio = 0.5 self.lift_motor = wpilib.SpeedControllerGroup(wpilib.Spark(4), wpilib.Spark(5)) self.lift_motor_speed = 0.0 # ball grab motors, need to spin in opposite directions self.front_motor_1 = wpilib.Spark(6) self.front_motor_2 = wpilib.Spark(7) self.front_motor_2.setInverted(True) self.front_motor = wpilib.SpeedControllerGroup(self.front_motor_1, self.front_motor_2) self.hatch_solenoid = wpilib.DoubleSolenoid( 0, 1) # pneumatic channels 0 & 1 self.encoder = wpilib.Encoder(aChannel=0, bChannel=1) # DIO 0 & 1 wpilib.CameraServer.launch( 'vision.py:main') # setup cameras: usb1 & usb2 self.xbox_axis = {} # init xbox axis dict self.debug = False # enable to print debug info self.reset() # i.e. ensure defaults are set self.loops = 0 # counter for program loops self.timer = wpilib.Timer() # init timer def reset(self): # safely lower lift while self.lift_motor_speed > 0.0: self.lift_motor_speed -= 0.002 if self.lift_motor_speed > 0.0 else 0.0 self.lift_motor.set(self.lift_motor_speed) self.front_motor.set(0) self.hatch_solenoid.set(0) self.encoder.reset() # init xbox axis info for i in range(0, self.xbox.getAxisCount()): self.xbox_axis[i] = self.xbox.getRawAxis(i) def drive_control(self): if self.xbox.getAButtonPressed(): if self.speedRatio == 2 / 3: self.speedRatio = 0.5 elif self.speedRatio == 0.5: self.speedRatio = 2 / 3 lh_y = self.xbox.getY(self.xbox.Hand.kLeft ) * self.speedRatio # incr speed from 1/2 to 2/3 rh_x = self.xbox.getX(self.xbox.Hand.kRight) * 2 / 3 if self.xbox.getStickButtonPressed(self.xbox.Hand.kLeft) \ or self.xbox.getStickButtonPressed(self.xbox.Hand.kRight): self.drive_rev = not self.drive_rev if self.drive_rev: self.drive.arcadeDrive(lh_y, rh_x, True) else: self.drive.arcadeDrive(lh_y * -1, rh_x, True) def button_status(self): # output to logs button or axis # and value for i in range(0, self.xbox.getAxisCount()): cur_axis = self.xbox.getRawAxis(i) if self.xbox_axis[i] != cur_axis: self.xbox_axis[i] = cur_axis self.logger.info("Axis {} = {}".format(i, cur_axis)) for j in range(1, self.xbox.getButtonCount() + 1): if self.xbox.getRawButtonPressed(j): self.logger.info("Button {} pressed".format(j)) def lift_control(self): high_volt = 0.9 # volt high limit init_volt = 0.45 # voltage to start raising low_volt = 0.2 # volt low limit volt_rate_raise = 0.01 # step voltage down volt_rate_low = 0.005 # step voltage up def trigger_pressed(hand): # wrapper case to detect a Xbox Trigger hard press return self.xbox.getTriggerAxis(hand) > 0.9 def auto_lower(): # redundant logic to reach max holding voltage while falling if (round(self.lift_motor_speed, 2) > low_volt) and (int( abs(self.encoder.getRate())) > 0): self.lift_motor_speed -= volt_rate_low # when voltage is below lower limit, then slowly reduce to zero # slow fall to bottom out elif (0.0 < round(self.lift_motor_speed, 2) < low_volt) and (int( abs(self.encoder.getRate())) == 0): self.lift_motor_speed -= volt_rate_low if round( self.lift_motor.get(), 2) > 0.0 else 0.00 # fail-safe for when the speed goes negative # shouldn't happen, but was left in case elif self.lift_motor_speed < 0: self.lift_motor_speed = 0 if self.lift_lock.on: # disable trigger input and allow the voltage to reduce to holding state auto_lower() else: # Left Trigger press if trigger_pressed(self.xbox.Hand.kLeft): # when starting, or below our low volt limit, then set speed to initial ramp voltage if (self.lift_motor_speed is 0.0) or (self.lift_motor_speed <= low_volt): self.lift_motor_speed = init_volt # while the lift is not moving, slowly increase the voltage # don't pass high voltage limit elif (int(abs(self.encoder.getRate())) == 0) and (self.lift_motor_speed <= high_volt): self.lift_motor_speed += volt_rate_raise # Right Trigger press # with B button held, lower to 0 volts elif trigger_pressed( self.xbox.Hand.kRight) and self.xbox.getBButton(): self.lift_motor_speed -= volt_rate_low if self.lift_motor.get( ) > 0.0 else 0.0 # while voltage is positive, then lower at lower rate, then bottom out at low volt limit elif trigger_pressed( self.xbox.Hand.kRight) and (self.lift_motor_speed > 0): self.lift_motor_speed -= volt_rate_low if round( self.lift_motor.get(), 2) > low_volt else low_volt # no trigger pressed, then run auto set code # will either keep lowest holding voltage or lower to zero else: auto_lower() # set lift motor speed self.lift_motor.set(self.lift_motor_speed) def grab_control(self): if self.xbox.getBumper(self.xbox.Hand.kLeft) \ and not self.xbox.getBumper(self.xbox.Hand.kRight): self.front_motor.set(0.33) elif self.xbox.getBumperPressed(self.xbox.Hand.kRight) \ and not self.xbox.getBumper(self.xbox.Hand.kLeft): self.front_motor.set(-0.7) elif self.xbox.getBumperReleased(self.xbox.Hand.kLeft) \ or self.xbox.getBumperReleased(self.xbox.Hand.kRight): self.front_motor.set(0) def hatch_control(self): if self.xbox.getXButtonPressed(): self.hatch_solenoid.set(2) elif self.xbox.getXButtonReleased(): self.hatch_solenoid.set(1) def robotPeriodic(self): pass def autonomousInit(self): self.reset() def autonomousPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control() if self.debug: self.button_status() def disabledInit(self): self.reset() def disabledPeriodic(self): self.reset() def teleopInit(self): self.reset() self.drive.setSafetyEnabled(True) self.loops = 0 self.timer.reset() self.timer.start() def teleopPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control() if self.debug: self.button_status()
class Robot(wpilib.IterativeRobot): def __init__(self): super().__init__() # Initiate up robot drive self.left_motor = wpilib.Spark(LEFT_PORT) self.right_motor = wpilib.Spark(RIGHT_PORT) self.drive = DifferentialDrive(self.left_motor, self.right_motor) self.drive.setExpiration(.1) # Initiate arm and lift self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT) self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2) # Initiate button stuff self.buttons = set() self.button_toggles = None self.pressed_buttons = {} self.rumble_time = None self.rumble_length = RUMBLE_LENGTH self.reset_buttons() # Initiate gamepad self.game_pad = wpilib.Joystick(GAMEPAD_NUM) self.max_enhancer = 0 self.smart_dashboard = wpilib.SmartDashboard # auto_chooser = wpilib.SendableChooser() # auto_chooser.addDefault("Enable Auto", "Enable Auto") # auto_chooser.addObject("Disable Auto", "Disable Auto") self.smart_dashboard.putBoolean("Enable Auto", True) self.auto_start_time = None self.current_speed = [0, 0] self.last_tank = 0 self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel self.tank_dir = None def reset_buttons(self): """Resets the values of the button toggles to default likewise defined here """ self.button_toggles = { "REVERSE": False, "LOCK": True, "STOP": False, "SPEED": False, "GRAB": False } self.pressed_buttons = {} self.rumble_time = None def stop(self): self.reset_buttons() self.last_tank = get_millis() self.drive.stopMotor() self.robot_lift.stopMotor() self.robot_lift_2.stopMotor() self.current_speed = [0, 0] # Events def robotInit(self): """Runs at the same time as __init__. Appears to be redundant in this case """ self.stop() def robotPeriodic(self): pass def disabledInit(self): self.stop() def disabledPeriodic(self): self.stop() def autonomousInit(self): self.stop() self.auto_start_time = None self.gyro.calibrate() # Takes five seconds self.stop() # Have to reset tank time to correct acceleration def tank(self, left, right, accel=None): if left == right: if isinstance(self.tank_dir, type(None)): self.tank_dir = self.gyro.getAngle() else: self.tank_dir = None turn = None current_angle = None if not isinstance(self.tank_dir, type(None)): current_angle = self.gyro.getAngle() if current_angle < self.tank_dir - ANGLE_MARGIN: turn = "right" elif current_angle > self.tank_dir + ANGLE_MARGIN: turn = "left" if isinstance(accel, type(None)): accel = ACCEL speed1 = self.current_speed[0] speed2 = self.current_speed[1] if not isinstance(turn, type(None)) and GYRO_ENABLE: if turn == "left": left -= ANGLE_CHANGE right += ANGLE_CHANGE elif turn == "right": right -= ANGLE_CHANGE left += ANGLE_CHANGE print((round(self.tank_dir, 3) if not isinstance(self.tank_dir, type(None)) else None), (round(current_angle, 3) if not isinstance(current_angle, type(None)) else None), round(left, 3), round(right, 3), turn) if .4 > speed1 > 0: if left <= 0: speed1 = 0 else: speed1 = .4 if -.4 < speed1 < 0: if left >= 0: speed1 = 0 else: speed1 = -.4 if .4 > speed2 > 0: if right <= 0: speed2 = 0 else: speed2 = .4 if -.4 < speed2 < 0: if right >= 0: speed2 = 0 else: speed2 = -.4 if isinstance(accel, type(None)): self.drive.tankDrive(left, right) return None vel_change = ((get_millis() - self.last_tank) / 1000) * accel if abs(speed1 - left) < WHEEL_LOCK: speed1 = left else: if speed1 < left: speed1 += vel_change elif speed1 > left: speed1 -= vel_change if abs(speed2 - right) < WHEEL_LOCK: speed2 = right else: if speed2 < right: speed2 += vel_change elif speed2 > right: speed2 -= vel_change self.current_speed = [speed1, speed2] # print([round(left, 2), round(right, 2)], [round(speed1, 2), round(speed2, 2)], round(vel_change, 4)) self.drive.tankDrive(*self.current_speed) # print([round(x, 2) for x in self.current_speed], round(vel_change, 2), accel) self.last_tank = get_millis() def autonomousPeriodic(self): if self.smart_dashboard.getBoolean("Auto Enabled", True): if not isinstance(self.auto_start_time, type(None)): # Auto enabled and running # percent = (get_millis() - self.auto_start_time) / AUTO_DUR # if percent < .5: # speed = (percent/.5) * AUTO_SPEED # elif percent < 1: # speed = ((1 - percent)/.5) * AUTO_SPEED # else: # speed = 0 if get_millis() >= self.auto_start_time + AUTO_DUR / 2: self.tank(0, 0, AUTO_ACCEL) else: self.tank(AUTO_SPEED, AUTO_SPEED, AUTO_ACCEL) else: # Auto enabled and not started self.auto_start_time = get_millis() def teleopInit(self): self.stop() self.drive.setSafetyEnabled(True) def set_rumble(self, left=True, value=1, length=RUMBLE_LENGTH): """Sets given rumble to given value""" if not left: self.game_pad.setRumble(wpilib.Joystick.RumbleType.kLeftRumble, value) else: self.game_pad.setRumble(wpilib.Joystick.RumbleType.kRightRumble, value) self.rumble_time = get_millis() self.rumble_length = length def check_rumble(self): """Checks if rumbling has surpassed RUMBLE_LENGTH""" if not isinstance(self.rumble_time, type(None)) and get_millis( ) > self.rumble_time + self.rumble_length: # This rumbling has gone on long enough! self.set_rumble(True, 0) self.set_rumble(False, 0) self.rumble_time = None def execute_buttons(self): """Check whether each button was just pressed and updates relevant toggle """ arms = 0 button_states = self.get_buttons() for button_name in BUTTON_PORTS: port = BUTTON_PORTS[button_name] angle = -1 if isinstance(port, list): angle = port[1] port = port[0] if port in button_states and button_states[port] is True: # button was just pressed if button_name in self.button_toggles: # needs toggled if button_name == "GRAB": pass # print(button_name, port == POV, # self.get_raw_buttons()[POV], angle) if not (port == POV and not self.get_raw_buttons()[POV] == angle): new_state = not self.button_toggles[button_name] self.button_toggles[button_name] = new_state self.set_rumble(new_state) # print(button_name, new_state) elif self.get_raw_buttons()[POV] == angle: # Button angle correct, check button name if button_name == "INC SPEED": self.max_enhancer += SPEED_ADJUST self.set_rumble(True, 1) elif button_name == "DEC SPEED": self.max_enhancer -= SPEED_ADJUST self.set_rumble(False, 1) elif button_name == "RES SPEED": self.max_enhancer = 0 self.set_rumble(True, length=200) elif port in button_states: # BUTTON BEING PRESSED if button_name == "ARM IN": arms = max(-1, min(-(ARM_SPEED_IN), 1)) self.button_toggles["GRAB"] = False elif button_name == "ARM OUT": arms = max(-1, min(ARM_SPEED_OUT, 1)) self.button_toggles["GRAB"] = False # if arms == 0 and self.button_toggles["GRAB"]: # self.robot_lift_2.set(-ARM_SPEED_IN) # else: # self.robot_lift_2.set(arms) self.check_rumble() def execute_axes(self): """Checks each axis and updates attached motor""" axes = self.get_axes() if not self.button_toggles["STOP"]: self.tank(axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS]) # self.left_motor.set(axes[LEFT_WHEELS_AXIS]) # self.right_motor.set(axes[RIGHT_WHEELS_AXIS]) left_trigger = axes[LIFT_AXIS1] right_trigger = axes[LIFT_AXIS2] if left_trigger > 0: if right_trigger <= 0: self.robot_lift.set(-left_trigger) self.robot_lift_2.set(-left_trigger) elif right_trigger > 0: self.robot_lift.set(right_trigger) # print(right_trigger) else: self.robot_lift.set(0) self.robot_lift_2.set(0) def teleopPeriodic(self): """Runs repeatedly while robot is in teleop""" self.execute_buttons() self.execute_axes() def get_raw_axes(self, _round=False): """Return a dictionary of controller axes""" i = 0 axes = {} while True: try: value = self.game_pad.getRawAxis(i) if i == RIGHT_WHEELS_AXIS: if value < 0: value /= .9 # Right stick does not go full forward value = min(value, 1) if _round: axes[i] = round(value, 2) else: axes[i] = value except IndexError: break i += 1 return axes def get_axes(self, _round=False): """Returns the current axes and values with deadzones and scaled""" axes = self.get_raw_axes(_round) # Correct deadzones and scale for axis in axes: if axis != LIFT_AXIS1 and axis != LIFT_AXIS2: if axis in DEADZONES: value = axes[axis] neg = -1 if value < 0 else 1 deadzone = DEADZONES[axis] if abs(value) > deadzone: value -= deadzone * neg value /= 1 - deadzone if not self.button_toggles[ "SPEED"]: # Do not impose caps value *= max( -1, min( SPEED_CAPS[axis][value >= 0] + self.max_enhancer, 1)) else: value = 0 if REVERSES[axis]: value *= -1 axes[axis] = value else: # TODO: Clean up # TRIGGERS FOR LIFT value = axes[axis] deadzone = DEADZONES[LIFT_AXIS1] if value > deadzone: value -= deadzone value /= 1 - deadzone if not self.button_toggles["SPEED"]: # Do not impose caps value *= max( -1, min( SPEED_CAPS[LIFT_AXIS1][axis == LIFT_AXIS1] + self.max_enhancer, 1)) if self.button_toggles["REVERSE"]: axes[LEFT_WHEELS_AXIS] *= -1 axes[RIGHT_WHEELS_AXIS] *= -1 dif, avg = None, None if self.button_toggles["LOCK"]: left = axes[LEFT_WHEELS_AXIS] right = axes[RIGHT_WHEELS_AXIS] dif = abs(abs(left) - abs(right)) if dif <= WHEEL_LOCK and left != 0 and right != 0: lneg = left < 0 rneg = right < 0 smaller = min(abs(left), abs(right)) avg = smaller + dif / 2 axes[LEFT_WHEELS_AXIS] = avg axes[RIGHT_WHEELS_AXIS] = avg if lneg: axes[LEFT_WHEELS_AXIS] *= -1 if rneg: axes[RIGHT_WHEELS_AXIS] *= -1 # print(self.button_toggles["LOCK"], left, right, # axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS], dif, avg) return axes def get_raw_buttons(self): """Return a dictionary of raw button states""" i = 1 # Controller buttons start at 1 buttons = {} while i <= 10: # Gets ten buttons or stops at index error <-- shouldn't happen try: buttons[i] = self.game_pad.getRawButton(i) except IndexError: break i += 1 buttons[i] = self.game_pad.getPOV(0) return buttons def get_buttons(self): """Returns a dictionary of bools representing whether each button was just pressed """ raw_buttons = self.get_raw_buttons() for button in raw_buttons: being_pressed = ( not raw_buttons[button] is False) and raw_buttons[button] != -1 if button not in self.pressed_buttons and being_pressed: # If button not already accounted for and being pressed self.pressed_buttons[button] = True elif button in self.pressed_buttons: if being_pressed: # Being pressed, already used self.pressed_buttons[button] = False else: # Was pressed, no longer being pressed del self.pressed_buttons[button] return self.pressed_buttons
class MyRobot(wpilib.TimedRobot): def robotInit(self): """Init is called once when the robot is turned on.""" self.efacing = 1 """efacing is used to invert our controls.""" self.CarEncoder = wpilib.Encoder(0, 1) #self.HatchEncoder = wpilib.Encoder(3, 4) self.chooser = wpilib.SendableChooser() self.chooser.addObject('cargo', '1') self.chooser.addObject('hatch panel', '2') self.left = wpilib.VictorSP(0) self.right = wpilib.VictorSP(1) """Motors used for driving""" self.myRobot = DifferentialDrive(self.left, self.right) """DifferentialDrive is the main object that deals with driving""" self.RotServo = wpilib.Servo(2) self.TiltServo = wpilib.Servo(3) """Our two servos will rotate (RotServo) and tilt (TiltServo) our vision cameras.""" self.motor1 = wpilib.Talon(5) self.motor2 = wpilib.Talon(6) """I mostly just added these motor controllers anticipating some sort of intake system that uses motors.""" self.Punch = wpilib.DoubleSolenoid(0, 1) self.DPunch = wpilib.DoubleSolenoid(3, 2) """The punching mechanism for removal of the hatch panels can use a DoubleSolenoid or regular Solenoid. The Solenoid only needs the channel it's plugged into (4) whereas the Double Solenoid needs the module number, forward channel number, and reverse channel order in that order.""" self.XBox0 = wpilib.XboxController(0) self.XBox1 = wpilib.XboxController(1) """Xbox controllers 1 and 2 on the driver station.""" self.myRobot.setExpiration(0.1) """If communication is cut off between the driver station and the robot for over 0.1 seconds, the robot will emergency stop.""" def teleopInit(self): """Executed once at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) """Motor Safety is another version of setExperiation, except motor safety is from the WPI Library.""" def teleopPeriodic(self): """Called every 20 ms during teleop""" if self.XBox0.getStartButtonPressed(): self.efacing *= -1 """This will invert our controls.""" if self.XBox0.getAButton(): self.Punch.set(DoubleSolenoid.Value.kForward) print ("Punch is Forward") else: self.Punch.set(DoubleSolenoid.Value.kReverse) if self.XBox0.getBButton(): self.DPunch.set(DoubleSolenoid.Value.kForward) print ("DPunch is Forward") else: self.DPunch.set(DoubleSolenoid.Value.kReverse) self.myRobot.arcadeDrive(self.XBox0.getY(0)* -self.efacing, self.XBox0.getX(0)* self.efacing) """The efacing variable is here to invert our controls. It's negative
class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" self.frontLeft = wpilib.Spark(1) self.rearLeft = wpilib.Spark(2) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight = wpilib.Spark(3) self.rearRight = wpilib.Spark(4) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.drive = DifferentialDrive(self.left, self.right) # object that handles basic drive operations #self.myRobot = wpilib.Drive.DifferentialDrive(0, 1) #self.myRobot.setExpiration(0.1) # joystick #0 self.stick = wpilib.Joystick(0) def disabledInit(self): self.drive.setSafetyEnabled(False) def disabledPeriodic(self): self.drive.setSafetyEnabled(False) def robotPeriodic(self): self.drive.setSafetyEnabled(False) def testInit(self): print("Test Init") def testPeriodic(self): if (self.stick.getRawButtonPressed(1) == True): print("Test Button 1 Pressed.") def autonomousInit(self): self.drive.setSafetyEnabled(False) def autonomousPeriodic(self): """Called when autonomous mode is enabled""" while self.isAutonomous() and self.isEnabled(): #wpilib.Timer.delay(0.01) self.drive.arcadeDrive(0.20, 0.1) def teleopInit(self): """Executed at the start of teleop mode""" self.drive.setSafetyEnabled(False) def teleopPeriodic(self): """Runs the motors with tank steering""" # timer = wpilib.Timer() # timer.start() while self.isOperatorControl() and self.isEnabled(): # Move a motor with a Joystick #wpilib.Timer.delay(0.02) self.drive.arcadeDrive(self.stick.getRawButton(1), True)
class MyRobot(wpilib.IterativeRobot): def robotInit(self): '''Robot initialization function''' # object that handles basic drive operations self.leftMotor = wpilib.Victor(0) self.rightMotor = wpilib.Victor(1) #self.myRobot = wpilib.RobotDrive(0, 1) self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor) # joyStick 0 self.joyStick = wpilib.Joystick(0) self.myRobot.setExpiration(0.1) self.myRobot.setSafetyEnabled(True) # encoders self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k4X) self.leftEncoder = wpilib.Encoder(2,3, False, wpilib.Encoder.EncodingType.k4X) # set up encoder self.drivePulsePerRotation = 1024 self.driveWheelRadius = 3.0 self. driveGearRatio = 1.0/1.0 self.driveEncoderPulsePerRot = self.drivePulsePerRotation * self.driveGearRatio self.driveEncoderDistPerPulse = (math.pi*2*self.driveWheelRadius)/self.driveEncoderPulsePerRot; self.leftEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse) self.leftEncoder.setReverseDirection(False) self.rightEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse) self.rightEncoder.setReverseDirection(False) self.timer = wpilib.Timer() def autonomousInit(self): ''' :return: ''' self.timer.reset() self.timer.start() print("START") self.printEncoderValues("Left Encoder",self.leftEncoder) self.printEncoderValues("Right Encoder", self.rightEncoder) def autonomousPeriodic(self): if not self.timer.hasPeriodPassed(5): self.leftMotor.set(0.5) self.rightMotor.set(0.5) else: self.leftMotor.set(0) self.rightMotor.set(0) self.timer.stop() print("FINISH") self.printEncoderValues("Left Encoder", self.leftEncoder) self.printEncoderValues("Right Encoder", self.rightEncoder) def teleopInit(self): """Executed at the start of teleop mode :return: """ pass def teleopPeriodic(self): """Runs the motors with tank steering :return: """ move = self.joyStick.getRawAxis(1) turn = self.joyStick.getRawAxis(4) self.myRobot.arcadeDrive(move, turn) def printEncoderValues(self, name,encoder): print(name) print() print("{0}: {1}".format("DistancePerPulse",encoder.getDistancePerPulse())) print("{0}: {1}".format("Distance", encoder.getDistance())) print("{0}: {1}".format("Raw Count", encoder.getRaw()))
class MyRobot(wpilib.IterativeRobot): def robotInit(self): '''Robot initialization function''' # object that handles basic drive operations self.frontLeftMotor = wpilib.Spark(1) # self.middleLeftMotor = wpilib.Spark(4) self.rearLeftMotor = wpilib.Spark(2) self.frontRightMotor = wpilib.Spark(3) # self.middleRightMotor = wpilib.Spark(1) self.rearRightMotor = wpilib.Spark(4) self.ihigh_motor = wpilib.Spark(6) self.ilow_motor = wpilib.Spark(9) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) self.high = 0 self.low = 0 self.gameData = '' # joysticks 1 & 2 on the driver station self.Stick1 = wpilib.XboxController(0) self.Stick2 = wpilib.Joystick(1) self.aSolenoidLow = wpilib.DoubleSolenoid(2, 3) self.aSolenoidHigh = wpilib.DoubleSolenoid(0, 1) self.iSolenoid = wpilib.DoubleSolenoid(4, 5) self.gyro = wpilib.ADXRS450_Gyro() def autonomousInit(self): self.iSolenoid.set(2) self.aSolenoidLow.set(2) self.aSolenoidHigh.set(2) self.gameData = wpilib.DriverStation.getInstance( ).getGameSpecificMessage() global timer timer = wpilib.Timer() timer.start() global firstTime firstTime = True def autonomousPeriodic(self): # This program tests 90 degree turn with gyro global firstTime, maxV, minV while firstTime: global fD, fluc, sD, v sD = self.gyro.getAngle() fD = sD - 90 firstTime = False v = 0.65 cD = self.gyro.getAngle() # left smaller right bigger if cD > fD: cD = self.gyro.getAngle() speed_turn = v self.myRobot.tankDrive(-speed_turn, speed_turn) print(cD) else: self.myRobot.tankDrive(0, 0) def disabledInit(self): self.myRobot.tankDrive(0, 0) self.iSolenoid.set(0) self.aSolenoidLow.set(0) self.aSolenoidHigh.set(0) def disabledPeriodic(self): pass def teleopInit(self): '''Execute at the start of teleop mode''' self.myRobot.setSafetyEnabled(True) self.iSolenoid.set(1) lastspeed = 0 def teleopPeriodic(self): if self.isOperatorControl() and self.isEnabled(): forward = self.Stick1.getTriggerAxis(1) backward = self.Stick1.getTriggerAxis(0) sp = forward - backward if abs(self.Stick1.getX(0)) > 0.1: steering = (self.Stick1.getX(0) + 0.1 * sp) / 1.5 else: steering = 0 self.myRobot.tankDrive(sp + steering, sp - steering)
class MyRobot(wpilib.TimedRobot): '''Main robot class''' # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty('/robot/autospeed', defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty('/robot/telemetry', defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 360 def robotInit(self): '''Robot-wide initialization code should go here''' self.lstick = wpilib.Joystick(0) left_front_motor = wpilib.Spark(1) left_front_motor.setInverted(False) right_front_motor = wpilib.Spark(2) right_front_motor.setInverted(False) left_rear_motor = wpilib.Spark(3) left_rear_motor.setInverted(False) right_rear_motor = wpilib.Spark(4) right_rear_motor.setInverted(False) # # Configure drivetrain movement # l = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor) r = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor) self.drive = DifferentialDrive(l, r) self.drive.setSafetyEnabled(False) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ( 1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi l_encoder = wpilib.Encoder(0, 1) l_encoder.setDistancePerPulse(encoder_constant) self.l_encoder_getpos = l_encoder.getDistance self.l_encoder_getrate = l_encoder.getRate r_encoder = wpilib.Encoder(2, 3) r_encoder.setDistancePerPulse(encoder_constant) self.r_encoder_getpos = r_encoder.getDistance self.r_encoder_getrate = r_encoder.getRate # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber('l_encoder_pos', self.l_encoder_getpos()) sd.putNumber('l_encoder_rate', self.l_encoder_getrate()) sd.putNumber('r_encoder_pos', self.r_encoder_getpos()) sd.putNumber('r_encoder_rate', self.r_encoder_getrate()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): ''' If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. ''' # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() motor_volts = battery * abs(self.prior_autospeed) l_motor_volts = motor_volts r_motor_volts = motor_volts # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = (now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate)
class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" # object that handles basic drive operations self.leftVictor = ctre.WPI_VictorSPX(LEFT) self.rightVictor = ctre.WPI_VictorSPX(RIGHT) self.centerVictor1 = ctre.WPI_VictorSPX(CENTER1) self.centerVictor2 = ctre.WPI_VictorSPX(CENTER2) self.left = wpilib.SpeedControllerGroup(self.leftVictor) self.right = wpilib.SpeedControllerGroup(self.rightVictor) self.center1 = wpilib.SpeedControllerGroup(self.centerVictor1) self.center2 = wpilib.SpeedControllerGroup(self.centerVictor2) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) # joysticks 1 & 2 on the driver station # self.leftStick = wpilib.Joystick(0) # self.rightStick = wpilib.Joystick(1) self.DEADZONE = 0.4 self.LEFT = GenericHID.Hand.kLeft self.RIGHT = GenericHID.Hand.kRight self.driver = wpilib.XboxController(0) self.ballManipulator = BallManipulator( ctre.WPI_VictorSPX(BALL_MANIP_ID)) def autonomousInit(self): self.myRobot.tankDrive(0.8, 0.8) def autonomousPeriodic(self): self.myRobot.tankDrive(1, 0.5) def teleopInit(self): """Executed at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) def setCenters(self, speed_value): self.center1.set(-speed_value) self.center2.set(speed_value) def deadzone(self, val, deadzone): if abs(val) < deadzone: return 0 return val def teleopPeriodic(self): ballMotorSetPoint = 0 if self.driver.getBumper(self.LEFT): ballMotorSetPoint = 1.0 elif self.driver.getBumper(self.RIGHT): ballMotorSetPoint = -1.0 else: ballMotorSetPoint = 0.0 self.ballManipulator.set(ballMotorSetPoint) """Runs the motors with tank steering""" #right = self.driver.getY(self.RIGHT) #left = self.driver.getY(self.LEFT) #self.myRobot.tankDrive(right, left) forward = -self.driver.getRawAxis(5) rotation_value = rotation_value = self.driver.getX(LEFT_HAND) forward = deadzone(forward, 0.2) self.myRobot.arcadeDrive(forward, rotation_value) center_speed = self.driver.getX(self.RIGHT) self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class MyRobot(wpilib.TimedRobot): def robotInit(self): """ Initializes all motors in the robot. """ self.leftTalon = ctre.WPI_TalonSRX(LEFT) self.rightTalon = ctre.WPI_TalonSRX(RIGHT) self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1) self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2) self.left = wpilib.SpeedControllerGroup(self.leftTalon) self.right = wpilib.SpeedControllerGroup(self.rightTalon) self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1) self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2) self.myRobot = DifferentialDrive(self.right, self.left) self.myRobot.setExpiration(0.1) # reasonable deadzone size self.DEADZONE = 0.1 self.driver = wpilib.XboxController(0) # Toggles whether or not robot can move self.emergencyStop = False def autonomousInit(self): pass #Do nothing for now in auton def autonomousPeriodic(self): pass #Do nothing for now in auton def teleopInit(self): """ Huh? """ self.myRobot.setSafetyEnabled(True) def deadzone(self, val, deadzone): if abs(val) < deadzone: return 0 return val def teleopPeriodic(self): """ What does this do? """ """ This function inverts the input from the stick for movement, and takes the input from the stick for rotation and sets them equal to the forward movement speed and rotation speed respecively. Then it calls a function to move the robot with the given parameters. """ # forward = self.driver.getY(RIGHT_HAND) # rotation_value = self.driver.getX(LEFT_HAND) forward = -self.driver.getRawAxis(1) rotation_value = -self.driver.getRawAxis(4) forward = deadzone(forward, 0.2) #Safety # Toggles emergency stop on A button pressed if self.driver.getAButtonPressed(): self.emergencyStop = not self.emergencyStop if self.emergencyStop: forward = 0 rotation_value = 0 #comment self.myRobot.arcadeDrive(forward, rotation_value) #Actualy move