class Drivetrain: # We also use injection here to access the l_motor and r_motor declared in BruhBot's # createObjects method. m_lfront: WPI_TalonSRX m_rfront: WPI_TalonSRX m_lback: WPI_TalonSRX m_rback: WPI_TalonSRX xSpeed = 0 ySpeed = 0 zRotation = 0 gyroAngle = 0 # Declare the basic drivetrain setup. def setup(self): self.mec_drive = MecanumDrive(self.m_lfront, self.m_lback, self.m_rfront, self.m_rback) self.mec_drive.setExpiration(0.1) self.mec_drive.setSafetyEnabled(True) # Change x and y variables for movement. def move(self, x, y, z, gyroAngle): self.xSpeed = x if abs(x) > 0.03 else 0 self.ySpeed = y if abs(y) > 0.03 else 0 self.zRotation = z self.gyroAngle = gyroAngle # Each time move is called, call arcadeDrive with supplied # x and y coordinates. def execute(self): self.mec_drive.driveCartesian(self.xSpeed, self.ySpeed, self.zRotation, self.gyroAngle)
class Drivetrain(Subsystem): def __init__(self): # Verify motor ports when placed on frame self.motor_lf = WPI_TalonSRX(1) self.motor_lr = WPI_TalonSRX(2) self.motor_rf = WPI_TalonSRX(3) self.motor_rr = WPI_TalonSRX(4) self.motor_lf.setInverted(False) self.motor_lr.setInverted(False) self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf, self.motor_rr) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(True) def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle) def initDefaultCommand(self): self.setDefaultCommand(FollowJoystick()) def set(self, ySpeed, xSpeed, zRotation, gyroAngle): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class MyRobot(wpilib.SampleRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 1 rearLeftChannel = 3 frontRightChannel = 2 rearRightChannel = 4 # The channel on the driver station that the joystick is connected to xchannel0 = 0 xchannel1 = 1 def robotInit(self): """Robot initialization function""" self.frontLeftMotor = wpilib.VictorSP(self.frontLeftChannel) self.rearLeftMotor = wpilib.VictorSP(self.rearLeftChannel) self.frontRightMotor = wpilib.VictorSP(self.frontRightChannel) self.rearRightMotor = wpilib.VictorSP(self.rearRightChannel) # invert the left side motors # self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot # self.rearLeftMotor.setInverted(True) # self.frontRightMotor.setInverted(True) # self.rearRightMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.xbox0 = wpilib.XboxController(self.xchannel0) self.xbox1 = wpilib.XboxController(self.xchannel1) def operatorControl(self): """Runs the motors with Mecanum drive.""" self.drive.setSafetyEnabled(True) while self.isOperatorControl() and self.isEnabled(): # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(-self.xbox0.getX(0), -self.xbox0.getY(0), 0) wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class Drivetrain(Subsystem): """ Subsystem that holds everything for the robot's drivetrain. Provides methods for driving the robot on a Cartesian plane. """ def __init__(self): # Hardware self.motor_lf = WPI_TalonSRX(1) self.motor_lr = WPI_TalonSRX(2) self.motor_rf = WPI_TalonSRX(3) self.motor_rr = WPI_TalonSRX(4) self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf, self.motor_rr) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(True) def inputNoise(self, input): """ This function limits the joystick input noise by increasing the tolerance for the zero value. """ return input if abs(input) < 0.03 else 0 def driveCartesian(self, xSpeed, ySpeed, zRotation, gyroAngle=0.0): """ Wrapper method that the subsystem uses to input a truncated number into the drivetrain, along with any motor inversions. """ xSpeed = self.inputNoise(xSpeed) * axes.motor_inversion[0] ySpeed = self.inputNoise(ySpeed) * axes.motor_inversion[1] zRotation = zRotation * axes.motor_inversion[2] self.drive.driveCartesian(xSpeed, ySpeed, zRotation, gyroAngle) def initDefaultCommand(self): self.setDefaultCommand(FollowJoystick()) def set(self, ySpeed, xSpeed, zRotation, gyroAngle): """ Shorthand method for driveCartesian. """ self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class DriveTrain(Subsystem): ''' This example subsystem controls a single Talon in PercentVBus mode. ''' def __init__(self): '''Instantiates the motor object.''' super().__init__('SingleMotor') self.frontLeftMotor = WPI_TalonSRX(channels.frontLeftChannel) self.rearLeftMotor = WPI_TalonSRX(channels.rearLeftChannel) self.frontRightMotor = WPI_TalonSRX(channels.frontRightChannel) self.rearRightMotor = WPI_TalonSRX(channels.rearRightChannel) self.crossbow = wpilib.DoubleSolenoid(0, 1) self.crossbow.set(wpilib.DoubleSolenoid.Value.kOff) self.frontLeftMotor.setInverted(False) # self.rearLeftMotor.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 30) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(False) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(True) # self.motor = wpilib.Talon(1) def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle) def initDefaultCommand(self): self.setDefaultCommand(FollowJoystick()) def set_crossbow(self, setting): self.crossbow.set(setting) def set(self, ySpeed, xSpeed, zRotation, gyroAngle): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class MyRobot(wpilib.SampleRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 0 rearLeftChannel = 2 frontRightChannel = 1 rearRightChannel = 3 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): wpilib.CameraServer.launch() '''Robot initialization function''' self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel) self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel) self.frontRightMotor = wpilib.Spark(self.frontRightChannel) self.rearRightMotor = wpilib.Spark(self.rearRightChannel) # invert the left side motors #self.frontRightMotor.setInverted(True) # you may need to change or remove this to match your robot #self.rearRightMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor) self.drive.setExpiration(0.1) self.stick = wpilib.Joystick(self.joystickChannel) def operatorControl(self): '''Runs the motors with Mecanum drive.''' self.drive.setSafetyEnabled(True) while self.isOperatorControl() and self.isEnabled(): self.drive.driveCartesian( self.stick.getX() * (-self.stick.getZ() + 1) / 2, -self.stick.getY() * (-self.stick.getZ() + 1) / 2, self.stick.getThrottle() * (-self.stick.getZ() + 1) / 2) #Changed "Z" value to "Throttle" value to control turning on the robot wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.TimedRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): """Robot initialization function""" self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) self.frontRightMotor = wpilib.Talon(self.frontRightChannel) self.rearRightMotor = wpilib.Talon(self.rearRightChannel) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.Joystick(self.joystickChannel) def teleopInit(self): self.drive.setSafetyEnabled(True) def teleopPeriodic(self): """Runs the motors with Mecanum drive.""" # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(self.stick.getX(), self.stick.getY(), self.stick.getZ(), 0)
class MyRobot(wpilib.SampleRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): '''Robot initialization function''' self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel) self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel) self.frontRightMotor = wpilib.Spark(self.frontRightChannel) self.rearRightMotor = wpilib.Spark(self.rearRightChannel) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor) self.drive.setExpiration(0.1) self.stick = wpilib.Joystick(self.joystickChannel) def operatorControl(self): '''Runs the motors with Mecanum drive.''' self.drive.setSafetyEnabled(True) while self.isOperatorControl() and self.isEnabled(): # Use the joystick X axis for lateral movement, Y axis for forward movement, and Throttle axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(self.stick.getX(), self.stick.getY(), self.stick.getThrottle(), 0) #Changed "Z" value to "Throttle" value to control turning on the robot wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class DriveTrain(Subsystem): ''' This example subsystem controls a single Talon in PercentVBus mode. ''' def __init__(self): '''Instantiates the motor object.''' super().__init__('SingleMotor') self.frontLeftMotor = wpilib.Talon(channels.frontLeftChannel) self.rearLeftMotor = wpilib.Talon(channels.rearLeftChannel) self.frontRightMotor = wpilib.Talon(channels.frontRightChannel) self.rearRightMotor = wpilib.Talon(channels.rearRightChannel) self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(True) # self.motor = wpilib.Talon(1) def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle) def initDefaultCommand(self): self.setDefaultCommand(FollowJoystick()) def set(self, ySpeed, xSpeed, zRotation, gyroAngle): self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
class Mecanum(Subsystem): """ Subsystem to control the motors for MecanumDrive """ def __init__(self): super().__init__("Mecanum") # motors and the channels they are connected to self.frontLeftMotor = wpilib.VictorSP(1) self.rearLeftMotor = wpilib.PWMVictorSPX(2) self.frontRightMotor = wpilib.VictorSP(0) self.rearRightMotor = wpilib.PWMVictorSPX(3) # invert the left side motors self.frontLeftMotor.setInverted(False) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(False) self.rearRightMotor.setInverted(False) #added this to match motor self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(False) def speed(self, yaxis, xaxis, zaxis, gyro): self.drive.driveCartesian(yaxis, xaxis, zaxis, gyro) def initDefaultCommand(self): self.setDefaultCommand(ThrottleMixer())
class SpootnikDrives(PIDSubsystem): def __init__(self): super().__init__(p=0.015, i=0.0001, d=0.0) self._deadband = 0.1 self._turnOutput = 0.0 # Configure motors motors = [ WPI_TalonSRX(i) for i in [RM.DRIVE_LF, RM.DRIVE_LB, RM.DRIVE_RF, RM.DRIVE_RB] ] for i, motor in enumerate(motors): # motor.configFactoryDefault() motor.enableVoltageCompensation(True) motor.configOpenLoopRamp(1.4, 10) motor.setNeutralMode(NeutralMode.Brake) # Invert left side motors? # if i <= 1: # motor.setInverted(True) # Set up PIDSubsystem parameters self.setInputRange(0.0, 360.0) self.pidController = self.getPIDController() self.pidController.setContinuous(True) self.pidController.setAbsoluteTolerance(1.0) self.setSetpoint(0.0) # Enable this is you use the PID functionality # self.pidController.enable() self.drive = MecanumDrive(*motors) self.drive.setExpiration(1) self.drive.setSafetyEnabled(False) self.drive.setDeadband(0.1) def returnPIDInput(self): return 0.0 def usePIDOutput(self, output: float): self._turnOutput = output def initDefaultCommand(self): self.setDefaultCommand(self.DriveWithJoy(self)) # Define commands class DriveWithJoy(Command): def __init__(self, spootnikDrives): super().__init__("DriveWithJoy") self.logger = logging.getLogger("DriveWithJoy") self.requires(spootnikDrives) self._spootnikDrives = spootnikDrives self._driveSwitcherVal = Command.getRobot( ).driveSwitcher.getSelected() self._printDriveType() def execute(self): if self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.WPI_MECANUM: self._spootnikDrives.driveCartesianWithJoy() elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.MORPHEUS: self._spootnikDrives.morpheusDrive() elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.FIELD_ORIENTED: self._spootnikDrives.fieldOrientedDrive() elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.NO_JOY: self._spootnikDrives.noJoyDrive() def isFinished(self): return False def _printDriveType(self): if self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.WPI_MECANUM: self.logger.info("WPI Mecanum Drive selected") elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.MORPHEUS: self.logger.info("Morpheus Drive selected") elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.FIELD_ORIENTED: self.logger.info("Field Oriented Drive selected") elif self._driveSwitcherVal == Command.getRobot( ).DriveSwitcher.NO_JOY: self.logger.info("No Joy Drive selected") class DriveForTime(TimedCommand): def __init__(self, spootnikDrives, time): super().__init__(name="DriveForTime", timeoutInSeconds=time, subsystem=spootnikDrives) self.logger = logging.getLogger("DriveForTime") self._spootnikDrives = spootnikDrives self._angle = 0.0 def initialize(self): self.logger.info("initialize") # self._spootnikDrives.drivelyMoreBetterer(0.8, 0.0, 0.0, 0.0) self._spootnikDrives.drive.driveCartesian(0.0, 0.8, 0.0) def end(self): self.logger.info("end") self._spootnikDrives.drive.driveCartesian(0.0, 0.0, 0.0) # Define support functions def driveCartesianWithJoy(self): joy = Command.getRobot().oi.driveJoy ySpeed = joy.getX(GenericHID.Hand.kLeft) xSpeed = -joy.getY(GenericHID.Hand.kLeft) zRotation = joy.getX(GenericHID.Hand.kRight) self.drive.driveCartesian(ySpeed, xSpeed, zRotation) def morpheusDrive(self): print("INFO: morpheusDrive currently unimplemented") # def fieldOrientedDrive(self): # lx = Command.getRobot().jml.getLX() # ly = -Command.getRobot().jml.getLY() # rx = Command.getRobot().jml.getRX() lx = 0.0 ly = 0.0 rx = 0.0 lx = self.applyDeadband(lx) ly = self.applyDeadband(ly) rx = self.applyDeadband(rx) self.drivelyMoreBetterer(ly, lx, rx) def noJoyDrive(self): print("INFO: noJoyDrive currently unimplemented") def drivelyMoreBetterer(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0): pass # print("INFO: drivelyMoreBetterer currently unimplemented") def applyDeadband(self, val): return val if abs(val) > self._deadband else 0.0
class MyRobot(wpilib.TimedRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): """Robot initialization function. The Low level is to use the brushless function on the controllers.""" self.sd = NetworkTables.getTable("SmartDashboard") self.timer = wpilib.Timer() # # Communicate w/navX MXP via the MXP SPI Bus. # - Alternatively, use the i2c bus. # See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details # self.navx = navx.AHRS.create_spi() # self.navx = navx.AHRS.create_i2c() if wpilib.RobotBase.isSimulation(): self.frontLeftMotor = wpilib.Jaguar(self.frontLeftChannel) self.rearLeftMotor = wpilib.Jaguar(self.rearLeftChannel) self.frontRightMotor = wpilib.Jaguar(self.frontRightChannel) self.rearRightMotor = wpilib.Jaguar(self.rearRightChannel) else: self.frontLeftMotor = rev.CANSparkMax( self.frontLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearLeftMotor = rev.CANSparkMax( self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.frontRightMotor = rev.CANSparkMax( self.frontRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearRightMotor = rev.CANSparkMax( self.rearRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) # invert the left side motors self.rearRightMotor.setInverted(False) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(False) self.drive = MecanumDrive( self.frontLeftMotor, self.frontRightMotor, self.rearLeftMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.XboxController(self.joystickChannel) def robotPeriodic(self): self.logger.info("Entered disabled mode") self.timer.reset() self.timer.start() if self.timer.hasPeriodPassed(0.5): self.sd.putNumber("Displacement X", self.navx.getDisplacementX()) self.sd.putNumber("Displacement Y", self.navx.getDisplacementY()) self.sd.putBoolean("IsCalibrating", self.navx.isCalibrating()) self.sd.putBoolean("IsConnected", self.navx.isConnected()) self.sd.putNumber("Angle", self.navx.getAngle()) self.sd.putNumber("Pitch", self.navx.getPitch()) self.sd.putNumber("Yaw", self.navx.getYaw()) self.sd.putNumber("Roll", self.navx.getRoll()) def autonomousInit(self): """Runs Once during auto""" # self.counter = 0 def autonomousPeriodic(self): """Runs Periodically during auto""" self.sd.putNumber("Timestamp", self.navx.getLastSensorTimestamp()) self.drive.driveCartesian(0, 0.5, 0, 0)
class MyRobot(wpilib.TimedRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): """Robot initialization function""" self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) self.frontRightMotor = wpilib.Talon(self.frontRightChannel) self.rearRightMotor = wpilib.Talon(self.rearRightChannel) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.Joystick(self.joystickChannel) def autonomousInit(self): LS = wpilib.DigitalInput(9) if LS.get() == 1: print("Found it") def operatorControl(self): """Runs the motors with Mecanum drive.""" self.drive.setSafetyEnabled(True) while self.isOperatorControl() and self.isEnabled(): # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(self.stick.getX(), self.stick.getY(), self.stick.getZ(), 0) wpilib.Timer.delay(0.005) # wait 5ms to avoid hogging CPU cycles
class MyRobot(wpilib.SampleRobot): """Main robot class""" # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to lStickChannel = 0 rStickChannel = 1 def robotInit(self): """Robot initialization function""" self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) self.frontRightMotor = wpilib.Talon(self.frontRightChannel) self.rearRightMotor = wpilib.Talon(self.rearRightChannel) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.lstick = wpilib.Joystick(self.lStickChannel) self.rstick = wpilib.Joystick(self.rStickChannel) # Position gets automatically updated as robot moves self.gyro = wpilib.AnalogGyro(1) def disabled(self): """Called when the robot is disabled""" while self.isDisabled(): wpilib.Timer.delay(0.01) def autonomous(self): """Called when autonomous mode is enabled""" timer = wpilib.Timer() timer.start() while self.isAutonomous() and self.isEnabled(): if timer.get() < 2.0: self.drive.driveCartesian(0, -1, 1, 0) else: self.drive.driveCartesian(0, 0, 0, 0) wpilib.Timer.delay(0.01) def operatorControl(self): """Called when operation control mode is enabled""" while self.isOperatorControl() and self.isEnabled(): self.drive.driveCartesian(self.lstick.getX(), self.lstick.getY(), self.rstick.getX(), 0) wpilib.Timer.delay(0.04)
class MyRobot(wpilib.TimedRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): """Robot initialization function. The Low level is to use the brushless function on the controllers.""" '''if wpilib.RobotBase.isSimulation(): self.frontLeftMotor = ctre.WPI_VictorSPX(self.frontLeftChannel) self.rearLeftMotor = ctre.WPI_VictorSPX(self.rearLeftChannel) self.frontRightMotor = ctre.WPI_VictorSPX(self.frontRightChannel) self.rearRightMotor = ctre.WPI_VictorSPX(self.rearRightChannel) else: ''' self.frontLeftMotor = rev.CANSparkMax( self.frontLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearLeftMotor = rev.CANSparkMax( self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.frontRightMotor = rev.CANSparkMax( self.frontRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearRightMotor = rev.CANSparkMax( self.rearRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) # invert the left side motors self.rearRightMotor.setInverted(False) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(False) self.drive = MecanumDrive( self.frontLeftMotor, self.frontRightMotor, self.rearLeftMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.XboxController(self.joystickChannel) self.lift = ctre.WPI_VictorSPX(6) def autonomousInit(self): """Runs Once during auto""" def autonomousPeriodic(self): """Runs Periodically during auto""" self.drive.driveCartesian(0.5, 0, 0, 0) def teleopInit(self): """Runs Once during teleop""" self.drive.setSafetyEnabled(True) def teleopPeriodic(self): """Runs the motors with Mecanum drive.""" # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(self.stick.getRawAxis(2), self.stick.getRawAxis(3), self.stick.getRawAxis(0), 0) self.lift.set(self.stick.getRawButton(1))
class MyRobot(wpilib.TimedRobot): # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to joystickChannel = 0 def robotInit(self): # Pull in smart dashboard info... self.sd = NetworkTables.getTable("SmartDashboard") # Start a timer.... self.timer = wpilib.Timer() """Robot initialization function. The Low level is to use the brushless function on the controllers.""" if wpilib.RobotBase.isSimulation(): self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel) self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel) self.frontRightMotor = wpilib.Spark(self.frontRightChannel) self.rearRightMotor = wpilib.Spark(self.rearRightChannel) else: self.frontLeftMotor = rev.CANSparkMax( self.frontLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearLeftMotor = rev.CANSparkMax( self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.frontRightMotor = rev.CANSparkMax( self.frontRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearRightMotor = rev.CANSparkMax( self.rearRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.XboxController(self.joystickChannel) # # Communicate w/navX MXP via the MXP SPI Bus. # - Alternatively, use the i2c bus. # See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details # self.navx = navx.AHRS.create_spi() # self.navx = navx.AHRS.create_i2c() # Analog input # self.analog = wpilib.AnalogInput(navx.pins.getNavxAnalogInChannel(0)) <--It seems as though the analog channel is not currently supported. def robotPeriodic(self): self.timer.reset() self.timer.start() while self.isDisabled(): if self.timer.hasPeriodPassed(0.5): self.sd.putNumber("Displacement X", self.navx.getDisplacementX()) self.sd.putNumber("Displacement Y", self.navx.getDisplacementY()) self.sd.putBoolean("IsCalibrating", self.navx.isCalibrating()) self.sd.putBoolean("IsConnected", self.navx.isConnected()) self.sd.putNumber("Angle", self.navx.getAngle()) self.sd.putNumber("Pitch", self.navx.getPitch()) self.sd.putNumber("Yaw", self.navx.getYaw()) self.sd.putNumber("Roll", self.navx.getRoll()) # self.sd.putNumber("Analog", self.analog.getVoltage()) self.sd.putNumber("Timestamp", self.navx.getLastSensorTimestamp()) def autonomousInit(self): """Runs Once during auto""" def autonomousPeriodic(self): """Runs Periodically during auto""" self.drive.driveCartesian(.5, 0, .5, 0) def teleopInit(self): """Runs Once during teleop""" self.drive.setSafetyEnabled(True) def teleopPeriodic(self): """Runs the motors with Mecanum drive.""" # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. # This sample does not use field-oriented drive, so the gyro input is set to zero. self.drive.driveCartesian(self.stick.getRawAxis(1), self.stick.getRawAxis(3), self.stick.getRawAxis(2), self.navx.getAngle()) self.sd.putNumber("Field Angle", self.navx.getAngle())
class MyRobot(wpilib.TimedRobot): """Main robot class""" # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 1 rearLeftChannel = 2 frontRightChannel = 4 rearRightChannel = 3 ballGrabMotor = 1 ballTickler = 2 # The channel on the driver station that the joystick is connected to lStickChannel = 0 rStickChannel = 1 # solenoids PCM_CANID = 6 GEAR_ADJUST_RETRACT_SOLENOID = 0 GEAR_ADJUST_EXTEND_SOLENOID = 1 GEAR_DOOR_DROP_SOLENOID = 3 GEAR_DOOR_RAISE_SOLENOID = 2 GEAR_PUSHER_RETRACT_SOLENOID = 4 GEAR_PUSHER_EXTEND_SOLENOID = 5 BALL_DOOR_OPEN_SOLENOID = 6 BALL_DOOR_CLOSE_SOLENOID = 7 GRABBER_STATE = False TICKLER_STATE = False COMPRESSOR_STATE = False GEAR_DOOR_STATE = False GEAR_ADJUST_STATE = False GEAR_PUSHER_STATE = False AUTO_STATE = 0 WHEEL_CIRCUMFERENCE = 19 #inches def robotInit(self): self.timer = wpilib.Timer() self.timer.start() self.auto_timer = wpilib.Timer() self.gyro = AHRS.create_spi() self.gyro.reset() self.compressor = wpilib.Compressor(self.PCM_CANID) self.compressor.setClosedLoopControl(False) #self.compressor.setClosedLoopControl(True) #Solenoids galore self.gearAdjustExtend = wpilib.Solenoid( self.PCM_CANID, self.GEAR_ADJUST_EXTEND_SOLENOID) self.gearAdjustRetract = wpilib.Solenoid( self.PCM_CANID, self.GEAR_ADJUST_RETRACT_SOLENOID) self.gearPusherExtend = wpilib.Solenoid( self.PCM_CANID, self.GEAR_PUSHER_EXTEND_SOLENOID) self.gearPusherRetract = wpilib.Solenoid( self.PCM_CANID, self.GEAR_PUSHER_RETRACT_SOLENOID) self.gearDoorDrop = wpilib.Solenoid(self.PCM_CANID, self.GEAR_DOOR_DROP_SOLENOID) self.gearDoorRaise = wpilib.Solenoid(self.PCM_CANID, self.GEAR_DOOR_RAISE_SOLENOID) self.ballDoorOpen = wpilib.Solenoid(self.PCM_CANID, self.BALL_DOOR_OPEN_SOLENOID) self.ballDoorClose = wpilib.Solenoid(self.PCM_CANID, self.BALL_DOOR_CLOSE_SOLENOID) """Robot initialization function""" self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel) self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel) self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel) self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel) self.tickler = Spark(self.ballTickler) self.grabber = Victor(self.ballGrabMotor) # invert the left side motors self.frontLeftMotor.setInverted(True) self.frontRightMotor.setInverted(True) self.rearLeftMotor.setInverted(True) self.rearRightMotor.setInverted(True) self.talons = [ self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor ] for talon in self.talons: talon.configNominalOutputForward(0.0, 25) talon.configNominalOutputReverse(0.0, 25) talon.configPeakOutputForward(1.0, 25) talon.configPeakOutputReverse(-1.0, 25) talon.enableVoltageCompensation(True) talon.configVoltageCompSaturation(11.5, 25) talon.configOpenLoopRamp(0.125, 25) talon.config_kP(0, 0.375, 25) talon.config_kI(0, 0.0, 25) talon.config_kD(0, 0.0, 25) talon.config_kF(0, 0.35, 25) talon.selectProfileSlot(0, 0) talon.configClosedLoopPeakOutput(0, 1.0, 25) talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 25) talon.configSelectedFeedbackCoefficient(1.0, 0, 25) talon.set(ctre.ControlMode.PercentOutput, 0) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.drive.setSafetyEnabled(False) self.lstick = wpilib.Joystick(self.lStickChannel) self.rstick = wpilib.Joystick(self.rStickChannel) def disabled(self): """Called when the robot is disabled""" while self.isDisabled(): wpilib.Timer.delay(0.01) def DriveStraight(self, distance): pass def autonomousInit(self): self.auto_timer.start() def autonomousPeriodic(self): #self.auto_timer.delay(5) while self.AUTO_STATE != 4: """ self.frontLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4) self.frontRightMotor.set(ctre.ControlMode.PercentOutput, 0.4) self.rearLeftMotor.set(ctre.ControlMode.PercentOutput, -0.4) self.rearRightMotor.set(ctre.ControlMode.PercentOutput, 0.4) """ if self.AUTO_STATE == 0: self.drive.driveCartesian(0, .5, -.025) if self.auto_timer.hasPeriodPassed(1.7): self.AUTO_STATE = 1 #self.auto_timer.reset() if self.AUTO_STATE == 1: self.drive.driveCartesian(0, 0, 0) if self.auto_timer.hasPeriodPassed(1.5): self.AUTO_STATE = 2 if self.AUTO_STATE == 2: self.drive.driveCartesian(0, -.5, 0.025) if self.auto_timer.hasPeriodPassed(2): self.AUTO_STATE = 3 if self.AUTO_STATE == 3: self.drive.driveCartesian(0, 0, 0) #for talon in self.talons: #talon.stopMotor() #self.logger.info("STAWP") self.AUTO_STATE = 4 #self.drive.setExpiration(0.1) #pass def conditonAxis(self, axis, deadband, rate, expo, power, minimum, maximum): deadband = min(abs(deadband), 1) rate = max(0.1, min(abs(rate), 10)) expo = max(0, min(abs(expo), 1)) power = max(1, min(abs(power), 10)) if axis > -deadband and axis < deadband: axis = 0 else: axis = rate * (math.copysign(1, axis) * ((abs(axis) - deadband) / (1 - deadband))) if expo > 0.01: axis = ((axis * (abs(axis)**power) * expo) + (axis * (1 - expo))) axis = max(min(axis, maximum), minimum) return axis def teleopInit(self): self.compressor.setClosedLoopControl(False) self.gyro.reset() self.solenoid_timer = wpilib.Timer() self.solenoid_timer.start() for talon in self.talons: talon.setQuadraturePosition(0) self.GearAdjustRetract() self.GearPusherRetract() self.GearDoorDrop() #Solenoid functions def BallDoorOpen(self): self.ballDoorOpen.set(True) self.ballDoorClose.set(False) self.tickler.set(-0.4) def BallDoorClose(self): self.ballDoorOpen.set(False) self.ballDoorClose.set(True) self.tickler.set(0.0) def GearAdjustExtend(self): self.gearAdjustExtend.set(True) self.gearAdjustRetract.set(False) def GearAdjustRetract(self): self.gearAdjustExtend.set(False) self.gearAdjustRetract.set(True) def GearPusherExtend(self): self.gearPusherExtend.set(True) self.gearPusherRetract.set(False) def GearPusherRetract(self): self.gearPusherExtend.set(False) self.gearPusherRetract.set(True) def GearDoorDrop(self): self.gearDoorDrop.set(True) self.gearDoorRaise.set(False) def GearDoorRaise(self): self.gearDoorDrop.set(False) self.gearDoorRaise.set(True) #Ball Grab def BallGrabStart(self): self.grabber.set(-0.2) def BallGrabStop(self): self.grabber.set(0.0) def activate(self, extend, retract, state): self.solenoid_timer.reset() self.logger.info(state) if state: extend() else: retract() def teleopPeriodic(self): """Called when operation control mode is enabled""" while self.isEnabled(): ''' self.logger.info("FrontLeft: " + str(self.frontLeftMotor.getMotorOutputVoltage())) self.logger.info("RearLeft: " + str(self.rearLeftMotor.getMotorOutputVoltage())) self.logger.info("FrontRight: " + str(self.frontRightMotor.getMotorOutputVoltage())) self.logger.info("RearRight: " + str(self.rearRightMotor.getMotorOutputVoltage())) ''' self.drive.driveCartesian( -self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5, -1, 1), self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1, 1), -self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5, -1, 1) #self.gyro.getYaw() ) if self.solenoid_timer.hasPeriodPassed(0.5): if JoystickButton(self.lstick, 3).get(): self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE self.activate(self.compressor.start, self.compressor.stop, self.COMPRESSOR_STATE) if JoystickButton(self.lstick, 2).get(): self.TICKLER_STATE = not self.TICKLER_STATE self.activate(self.BallDoorOpen, self.BallDoorClose, self.TICKLER_STATE) if JoystickButton(self.lstick, 1).get(): self.GRABBER_STATE = not self.GRABBER_STATE self.activate(self.BallGrabStart, self.BallGrabStop, self.GRABBER_STATE) #pilotstick 2 if JoystickButton(self.rstick, 3).get(): self.GEAR_DOOR_STATE = not self.GEAR_DOOR_STATE self.activate(self.GearDoorRaise, self.GearDoorDrop, self.GEAR_DOOR_STATE) if JoystickButton(self.rstick, 5).get(): self.GEAR_ADJUST_STATE = not self.GEAR_ADJUST_STATE self.activate(self.GearAdjustExtend, self.GearAdjustRetract, self.GEAR_ADJUST_STATE) if JoystickButton(self.rstick, 4).get(): self.GEAR_PUSHER_STATE = not self.GEAR_PUSHER_STATE self.activate(self.GearPusherExtend, self.GearPusherRetract, self.GEAR_PUSHER_STATE) wpilib.Timer.delay(0.04)
class MyRobot(wpilib.SampleRobot): """Main robot class""" # Channels on the roboRIO that the motor controllers are plugged in to frontLeftChannel = 2 rearLeftChannel = 3 frontRightChannel = 1 rearRightChannel = 0 # The channel on the driver station that the joystick is connected to lStickChannel = 0 rStickChannel = 1 def robotInit(self): """Robot initialization function""" self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) self.frontRightMotor = wpilib.Talon(self.frontRightChannel) self.rearRightMotor = wpilib.Talon(self.rearRightChannel) # invert the left side motors self.frontLeftMotor.setInverted(True) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive( self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.lstick = wpilib.Joystick(self.lStickChannel) self.rstick = wpilib.Joystick(self.rStickChannel) def disabled(self): """Called when the robot is disabled""" while self.isDisabled(): wpilib.Timer.delay(0.01) def autonomous(self): """Called when autonomous mode is enabled""" timer = wpilib.Timer() timer.start() while self.isAutonomous() and self.isEnabled(): if timer.get() < 2.0: self.drive.driveCartesian(0, -1, 1, 0) else: self.drive.driveCartesian(0, 0, 0, 0) wpilib.Timer.delay(0.01) def operatorControl(self): """Called when operation control mode is enabled""" while self.isOperatorControl() and self.isEnabled(): self.drive.driveCartesian( self.lstick.getX(), self.lstick.getY(), -self.lstick.getZ(), 0 ) wpilib.Timer.delay(0.04)