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 robotInit(self):
# Channels for the wheels
self.l_motor = wpilib.Spark(1)
self.r_motor = wpilib.Spark(2)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.stick = wpilib.Joystick(0)
#
# 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.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = wpilib.controller.PIDController(
self.kP,
self.kI,
self.kD,
)
turnController.enableContinuousInput(-180.0, 180.0)
turnController.setTolerance(self.kToleranceDegrees)
self.turnController = turnController
def robotInit(self): #This function is called upon program startup and should be used for mapping everything
#motors
self.motorL = wpilib.Spark(0) #channel 0
self.motorR = wpilib.Spark(1) #channel 1
self.drive = DifferentialDrive(self.motorL, self.motorR) #dive setup, differential is used with tank
#solenoids
self.arm = wpilib.DoubleSolenoid(0,0,1) #modul 0 channels 0 and 1
self.claw = wpilib.DoubleSolenoid(0,2,3) #modul 0 channels 2 and 3
#controller
self.controller = wpilib.Joystick(1)
#in code use the following for each button or joystick with Logitech in "D" mode
#left joystick horizontal - self.controller.getX()
#left joystick vertical - self.controller.getY()
#right joystick horizontal - self.controller.getZ()
#right joystick vertical - self.controller.getT()
#button X - self.controller.getButton(1)
#button A - self.controller.getButton(2)
#button B - self.controller.getButton(3)
#button Y - self.controller.getButton(4)
#trigger top left - self.controller.getButton(5)
#trigger top right - self.controller.getButton(6)
#bumper bottom left - self.controller.getButton(7)
#bumper bottom right - self.controller.getButton(8)
#button Back - self.controller.getButton(9)
#button Start - self.controller.getButton(10)
self.timer = wpilib.Timer()
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 __init__(self) -> None:
super().__init__()
# The Romi has the left and right motors set to
# PWM channels 0 and 1 respectively
self.leftMotor = wpilib.Spark(0)
self.rightMotor = wpilib.Spark(1)
# The Romi has onboard encoders that are hardcoded
# to use DIO pins 4/5 and 6/7 for the left and right
self.leftEncoder = wpilib.Encoder(4, 5)
self.rightEncoder = wpilib.Encoder(6, 7)
# Set up the differential drive controller
self.drive = wpilib.drive.DifferentialDrive(self.leftMotor, self.rightMotor)
# Set up the RomiGyro
self.gyro = RomiGyro()
# Set up the BuiltInAccelerometer
self.accelerometer = wpilib.BuiltInAccelerometer()
# Use inches as unit for encoder distances
self.leftEncoder.setDistancePerPulse(
(math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
)
self.rightEncoder.setDistancePerPulse(
(math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
)
self.resetEncoders()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Comments begin with a "#". I will be making liberal use of comments.
#
# These initializations won't do anything on our robot at the moment, but won't
# hinder a simulation.
# Sparks are motor controllers. The Spark class in the library contains code to talk to them.
# "self" refers to the object being defined by this class, or in other words our robot.
# This is saying that the variable "left_motor" is part of the class's definition and
# is accessible from any function in the class.
self.left_motor = wpilib.Spark(
0
) # Create an object of the Spark class on PWM channel 0 and call it left_motor.
self.right_motor = wpilib.Spark(
1) # Create another object of the Spark class.
# An object of the class DifferentialDrive helps us to control a drive train with all left wheels connected
# and all right wheels connected, similar to a Bobcat skid-steer or a bulldozer.
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.stick = wpilib.Joystick(
1
) # Joystick is a class that talks to the joysticks connected to the driver station computer.
# Timer to control periodic logging of messages.
self.print_timer = wpilib.Timer()
self.print_timer.start()
# Another timer, this one to help us measure time during play modes.
self.play_timer = wpilib.Timer()
self.play_timer.start()
def robotInit(self):
# Channels for the wheels
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
self.myRobot = wpilib.drive.DifferentialDrive(wpilib.Spark(0), wpilib.Spark(1))
self.myRobot.setExpiration(0.1)
self.stick = wpilib.Joystick(0)
#
# 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.ahrs = AHRS.create_spi()
self.ahrs = AHRS.create_i2c(0)
turnController = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.ahrs, output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-0.5, 0.5)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController", turnController)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
#self.lf_motor = wpilib.Victor(0b00) # => 0
#self.lr_motor = wpilib.Victor(0b01) # => 1
#self.rf_motor = wpilib.Victor(0b10) # => 2
#self.rr_motor = wpilib.Victor(0b11) # => 3
# TODO: This is not in any way an ideal numbering system.
# The PWM ports should be redone to use the old layout above.
self.lf_motor = wpilib.Victor(9)
self.lr_motor = wpilib.Victor(8)
self.rf_motor = wpilib.Victor(7)
self.rr_motor = wpilib.Victor(6)
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(
self.intake_wheel_left, self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def robotInit(self): #Velcrolastick self.lift_motor1 = wpilib.Spark(6) self.lift_motor2 = wpilib.Spark(7) self.sensor1 = wpilib.DigitalInput(7) self.sensor2 = wpilib.DigitalInput(8) # Ismafeeder self.cargo_motor = wpilib.Spark(4) self.lift_motor = wpilib.Spark(5) # Mecanum drive self.frontLeftMotor = wpilib.Talon(0) self.rearLeftMotor = wpilib.Talon(1) self.frontRightMotor = wpilib.Talon(2) self.rearRightMotor = wpilib.Talon(3) self.sensor_1_mec = wpilib.DigitalInput(9) self.frontLeftMotor.setInverted(True) self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.driver_stick = wpilib.Joystick(0)
self.codriver_stick = wpilib.Joystick(1)
self.left_shooter_motor = ctre.WPI_TalonSRX(0)
self.left_drivetrain_motor = ctre.WPI_TalonSRX(1)
self.right_drivetrain_motor = ctre.WPI_TalonSRX(2)
self.right_shooter_motor = ctre.WPI_TalonSRX(3)
self.intake_motor = wpilib.Spark(8)
self.arm_pivot_motor = wpilib.Spark(6)
self.arm_lock_motor = wpilib.Spark(5)
self.timer = wpilib.Timer()
#self.navx = navx.AHRS.create_spi()
self.duration = 20
self.drivetrain = DriveTrain(self.left_drivetrain_motor,
self.right_drivetrain_motor)
self.shooter = Shooter(self.intake_motor, self.left_shooter_motor,
self.right_shooter_motor)
self.arm = Arm(self.arm_pivot_motor, self.arm_lock_motor)
def createObjects(self):
# Inputs
# TODO: Update these dynamically
self.stick = wpilib.Joystick(2)
# self.gamepad = wpilib.XboxController(1)
# self.gamepad_alt = wpilib.XboxController(2)
self.gamepad = wpilib.XboxController(0)
self.gamepad_alt = wpilib.XboxController(1)
# Drive motors
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive_train = wpilib.drive.DifferentialDrive(
self.left_motor, self.right_motor)
self.drive_train.deadband = .04
# Elevator encoder (gearbox)
self.lift_encoder = ExternalEncoder(0, 1)
# TODO: Fix the pid
# self.lift_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Drive encoders
self.left_encoder = ExternalEncoder(2, 3)
self.right_encoder = ExternalEncoder(4, 5, True)
self.left_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
self.right_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Elevator motors
self.lift_master = CANTalon(2)
self.lift_follower1 = CANTalon(3)
self.lift_follower2 = CANTalon(4)
self.lift_follower1.follow(self.lift_master)
self.lift_follower2.follow(self.lift_master)
# Intake motors
self.left_intake_motor = wpilib.Spark(2)
self.right_intake_motor = wpilib.Spark(3)
self.right_intake_motor.setInverted(True)
# Intake grabbers
self.grabber_solenoid = wpilib.DoubleSolenoid(1, 0, 1)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
wpilib.SmartDashboard.putData("PowerDistributionPanel", self.pdp)
# Misc
self.navx = navx.AHRS.create_spi()
self.net_table = NetworkTables.getTable("SmartDashboard")
self.vision_table = NetworkTables.getTable("limelight")
self.limelight_x = self.vision_table.getEntry("tx")
self.limelight_valid = self.vision_table.getEntry("tv")
self.dashboard_has_target = self.vision_table.getEntry("hastarget")
# Launch camera server
wpilib.CameraServer.launch()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.driver_stick = wpilib.Joystick(0)
self.codriver_stick = wpilib.Joystick(1)
# Drivetrain Motors
self.left_drivetrain_motor = ctre.WPI_TalonSRX(4)
self.left_drivetrain_motor_2 = ctre.WPI_TalonSRX(3)
self.right_drivetrain_motor = ctre.WPI_TalonSRX(6)
self.right_drivetrain_motor_2 = ctre.WPI_TalonSRX(5)
#Encoders
self.left_drivetrain_encoder = self.left_drivetrain_motor.getSelectedSensorPosition()
self.right_drivetrain_encoder = self.right_drivetrain_motor.getSelectedSensorPosition()
# Box-mechanism Motors
self.intake_motor = wpilib.Spark(8)
self.box_lift_motor = ctre.WPI_TalonSRX(2)
# Lift mechanism Motors
self.elevator_motor = wpilib.Spark(9)
self.main_lift = rev.CANSparkMax(7, rev.MotorType.kBrushless)
#ControlPanel objects
self.arm = ctre.WPI_TalonSRX(1)
self.arm.configSelectedFeedbackSensor(ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.arm.config_kP(0, (0.3 * 1023) / 4161 , 0)
self.arm.config_kI(0, (0.5 * 1023) / (4161*30), 0)
self.arm.config_kD(0, 30*(0.5 * 1023) / 4161, 0)
self.arm.config_kF(0, 0, 0)
self.timer = wpilib.Timer()
self.duration = 20
self.drivetrain = DriveTrain(self.left_drivetrain_motor, self.left_drivetrain_motor_2, self.right_drivetrain_motor, self.right_drivetrain_motor_2)
self.box = Box(self.intake_motor,self.box_lift_motor)
self.robotlift = RLift(self.elevator_motor, self.main_lift)
self.Panel = Arm(self.arm)
wpilib.SmartDashboard.putString(keyName="Robot_Station", value= "1")
wpilib.CameraServer.launch()
self.auto_timer = wpilib.Timer()
self.navx = navx.AHRS.create_spi()
self.auto = 0
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
def robotInit(self):
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.stick = wpilib.Joystick(0)
self.drive.setExpiration(0.1)
self.tm = wpilib.Timer()
self.tm.start()
def __init__(self):
super().__init__()
self.diabloDrive = RobotDrive(wpilib.Spark(robotMap.L1DRIVE),
wpilib.Spark(robotMap.L2DRIVE),
wpilib.Spark(robotMap.R1DRIVE),
wpilib.Spark(robotMap.R2DRIVE))
self.diabloDrive.frontLeftMotor.enableDeadbandElimination(True)
self.diabloDrive.frontRightMotor.enableDeadbandElimination(True)
self.diabloDrive.rearLeftMotor.enableDeadbandElimination(True)
self.diabloDrive.rearRightMotor.enableDeadbandElimination(True)
def createObjects(self):
# Define Driving Motors
self.rightDrive = wpilib.VictorSP(0)
self.leftDrive = wpilib.VictorSP(1)
# Create Robot Drive
self.robotDrive = wpilib.drive.DifferentialDrive(
self.rightDrive, self.leftDrive)
# Create Shifter Pneumatics
self.shifterSolenoid = wpilib.DoubleSolenoid(0, 0, 1)
# Joysticks and Controllers
self.driveJoystick = wpilib.Joystick(0)
self.driveController = XboxController(0)
self.subsystemJoystick = wpilib.Joystick(1)
self.subsystemController = XboxController(1)
# Create NavX and Accel
self.navX = navx.AHRS.create_spi()
self.accel = wpilib.BuiltInAccelerometer()
# Set Drivespeed
self.driveSpeed = 0
# Intake Motors
self.intakeMotor = wpilib.VictorSP(2)
# Intake Lifter
self.intakeLifter = wpilib.Spark(6)
# Create Cube Intake Pneumatics
self.intakeSolenoid = wpilib.Solenoid(0, 2)
# Create Ramp Motors
self.rightRamp = wpilib.Spark(5)
self.leftRamp = wpilib.Spark(4)
# Create Ramp Deploy Pneumatics
self.rampSolenoid = wpilib.Solenoid(0, 3)
# Create Timer (For Making Timed Events)
self.timer = wpilib.Timer()
# Initialize Compressor
self.compressor = wpilib.Compressor()
# Create CameraServer
wpilib.CameraServer.launch("common/multipleCameras.py:main")
# Set Gear in Dashboard
wpilib.SmartDashboard.putString("Shift State", "Low Gear")
wpilib.SmartDashboard.putString("Cube State", "Unclamped")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.drive.MecanumDrive(wpilib.Spark(2),
wpilib.Spark(1),
wpilib.Spark(5),
wpilib.Spark(0))
self.stick = wpilib.Joystick(0)
self.ahrs = AHRS.create_i2c(0)
def initRobot(self):
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 = wpilib.drive.DifferentialDrive(self.left, self.right)
def robotInit(self):
self.frontleft_motor = wpilib.Spark(0)
self.backleft_motor = wpilib.Spark(1)
self.left = wpilib.SpeedControllerGroup(self.frontleft, self.backleft)
self.frontright_motor = wpilib.Spark(2)
self.backright_motor = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.frontright, self.backright)
self.drive = wpilib.drive.DifferentialDrive(self.frontleft_motor, self.frontright_motor, self.backleft_motor, self.backright_motor)
self.stick = wpilib.Joystick(0)
def robotInit(self):
gyroChannel = 0 # analog input
self.left = wpilib.Spark(0)
self.right = wpilib.Spark(1)
self.gyro = ctre.PigeonIMU(gyroChannel)
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.stick = wpilib.XboxController(0)
def robotInit(self):
self.frontLeft = wpilib.Spark(0)
self.rearLeft = wpilib.Spark(1)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight = wpilib.Spark(2)
self.rearRight = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight)
self.drive = DifferentialDrive(self.left, self.right)
self.stick2 = wpilib.Joystick(0)
self.stick = wpilib.Joystick(1)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontRightMotor = wpilib.Victor(0)
self.rearRightMotor = wpilib.Victor(1)
self.frontLeftMotor = wpilib.Victor(2)
self.rearLeftMotor = wpilib.Victor(3)
# object that handles basic intake operations
self.omnom_left_motor = wpilib.Spark(7) # make sure channels are correct
self.omnom_right_motor = wpilib.Spark(8)
# object that handles basic lift operations
self.liftMotor = wpilib.Spark(4) # make sure channel is correct
# object that handles basic climb operations
self.winch1 = wpilib.Spark(5)
self.winch2 = wpilib.Spark(6)
# defining motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
# setting up drive group for drive motors
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# defining omnom motor groups
self.omnom_left = wpilib.SpeedControllerGroup(self.omnom_left_motor)
self.omnom_right = wpilib.SpeedControllerGroup(self.omnom_right_motor)
# setting up omnom group for omnom motors
self.omnom = DifferentialDrive(self.omnom_left, self.omnom_right)
self.omnom.setExpiration(0.1)
# defines timer for autonomous
self.timer = wpilib.Timer()
# joystick 0 & on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.stick = wpilib.Joystick(2)
# initialization of the hall-effect sensors
self.DigitalInput = wpilib.DigitalInput(1)
# initialization of the FMS
self.DS = DriverStation.getInstance()
self.PS = DriverStation.getInstance()
# initialization of the camera server
wpilib.CameraServer.launch()
def createObjects(self):
# Motors
self.m_lfront = WPI_TalonSRX(1)
self.m_rfront = WPI_TalonSRX(2)
self.m_lback = WPI_TalonSRX(3)
self.m_rback = WPI_TalonSRX(4)
self.m_hatch = wpilib.Spark(0)
self.m_shooter = wpilib.Spark(1)
self.ls_shooter = wpilib.DigitalInput(0)
self.s_intake = wpilib.DoubleSolenoid(2, 3)
# Joysticks (PS4 Controller, in our case)
self.joystick = wpilib.Joystick(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()
def robotInit(self):
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor)
self.stick = wpilib.Joystick(0)
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = PIDController(
self.kP, self.kI, self.kD, period = 1.0
)
self.turnController = turnController
self.rotateToAngleRate = 0
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.table = NetworkTables.getTable("SmartDashboard")
self.robot_drive = wpilib.drive.DifferentialDrive(
wpilib.Spark(0), wpilib.Spark(1))
self.stick = wpilib.Joystick(0)
self.elevatorMotor = wpilib.VictorSP(5)
self.intakeMotor = wpilib.VictorSP(2)
self.intakeMotorLeft = wpilib.VictorSP(3)
self.intakeMotorRight = wpilib.VictorSP(4)
self.climbMotor = wpilib.VictorSP(6)
self.ahrs = AHRS.create_i2c(0)
#self.gearSpeed = .5
#self.lights = wpilib.Relay(1)
#self.lightToggle = False
#self.lightToggleBool = True
#self.togglev = 0
self.robot_drive.setSafetyEnabled(False)
wpilib.CameraServer.launch()
self.xb = wpilib.Joystick(1)
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enabled = self.Compressor.enabled()
self.PSV = self.Compressor.getPressureSwitchValue()
self.LeftSolenoid = wpilib.DoubleSolenoid(0, 1)
self.RightSolenoid = wpilib.DoubleSolenoid(2, 3)
self.Compressor.start()
self.wheel = wpilib.Encoder(0, 1)
self.wheel2 = wpilib.Encoder(2, 3, True)
self.encoder = Sensors.Encode(self.wheel, self.wheel2)
#wpilib.CameraServer.launch()
self.ultrasonic = wpilib.AnalogInput(0)
self.autoSchedule = Auto.Auto(self, )
self.intakeToggle = False
self.intakePos = False
self.openSwitch = wpilib.DigitalInput(9)
self.closedSwitch = wpilib.DigitalInput(8)
self.speed = 0.6
self.leftSpeed = 0.7
self.rightSpeed = 0.7
self.speedToggle = False
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 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 robotInit(self):
# Channels for the wheels
self.table = NetworkTables.getTable("SmartDashboard")
self.myRobot = wpilib.drive.DifferentialDrive(wpilib.Spark(0),
wpilib.Spark(1))
self.myRobot.setExpiration(0.1)
self.stick = wpilib.Joystick(0)
self.wheel = wpilib.Encoder(0, 1)
self.wheel2 = wpilib.Encoder(2, 3, True)
self.wheel.reset()
self.wheel2.reset()
self.wheel.setDistancePerPulse(0.8922)
self.wheel2.setDistancePerPulse(0.8922)
self.rate = AverageOutRateGen(self.wheel.getRate, self.wheel2.getRate)
self.sum = 0
self.toggle = True
self.control = Distance(300, 20)
self.tm = wpilib.Timer()
self.tm.start()
self.start_time = self.tm.getMsClock()
#
# 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.ahrs = AHRS.create_spi()
self.ahrs = AHRS.create_i2c(0)
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
AverageOutRateGen(
self.wheel.getRate,
self.wheel2.getRate),
output=self)
turnController.setInputRange(-500, 500.0)
turnController.setOutputRange(-0.008, 0.008)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController",
turnController)
