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 __init__(self, robot):
self.robot = robot
# Configure drive motors
self.frontLeftCIM = wpilib.Victor(1)
self.frontRightCIM = wpilib.Victor(2)
self.backLeftCIM = wpilib.Victor(3)
self.backRightCIM = wpilib.Victor(4)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Left CIM", self.frontLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Right CIM", self.frontRightCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Left CIM", self.backLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Right CIM", self.backRightCIM)
# Configure the RobotDrive to reflect the fact that all our motors are
# wired backwards and our drivers sensitivity preferences.
self.drive = wpilib.RobotDrive(self.frontLeftCIM, self.frontRightCIM, self.backLeftCIM, self.backRightCIM)
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(.1)
self.drive.setSensitivity(.5)
self.drive.setMaxOutput(1.0)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# Configure encoders
self.rightEncoder = wpilib.Encoder(1, 2,
reverseDirection=True,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(3, 4,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.rightEncoder.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.leftEncoder.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
if robot.isReal():
# Converts to feet
self.rightEncoder.setDistancePerPulse(0.0785398)
self.leftEncoder.setDistancePerPulse(0.0785398)
else:
# Convert to feet 4in diameter wheels with 360 tick simulated encoders.
self.rightEncoder.setDistancePerPulse((4*math.pi)/(360*12))
self.leftEncoder.setDistancePerPulse((4*math.pi)/(360*12))
wpilib.LiveWindow.addSensor("DriveTrain", "Right Encoder", self.rightEncoder)
wpilib.LiveWindow.addSensor("DriveTrain", "Left Encoder", self.leftEncoder)
# Configure gyro
# -> the original pacgoat example is at channel 2, but that was before WPILib
# moved to zero-based indexing. You need to change the gyro channel in
# /usr/share/frcsim/models/PacGoat/robots/PacGoat.SDF, from 2 to 0.
self.gyro = wpilib.AnalogGyro(0)
if robot.isReal():
# TODO: Handle more gracefully
self.gyro.setSensitivity(0.007)
wpilib.LiveWindow.addSensor("DriveTrain", "Gyro", self.gyro)
super().__init__()
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(0)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.chassis_speeds = ChassisSpeeds()
self.chassis_speeds.vx = 0.0
self.chassis_speeds.omega = 0.0
if is_sim:
self.physics = physics.PhysicsEngine()
self.last_tm = time.time()
def __init__(self, robot):
super().__init__("DriveTrain")
self.robot = robot
self.front_left_motor = wpilib.Talon(1)
self.back_left_motor = wpilib.Talon(2)
self.front_right_motor = wpilib.Talon(3)
self.back_right_motor = wpilib.Talon(4)
left_motors = wpilib.SpeedControllerGroup(
self.front_left_motor, self.back_left_motor
)
right_motors = wpilib.SpeedControllerGroup(
self.front_right_motor, self.back_right_motor
)
self.drive = wpilib.drive.DifferentialDrive(left_motors, right_motors)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
# Encoders may measure differently in the real world and in
# simulation. In this example the robot moves 0.042 barleycorns
# per tick in the real world, but the simulated encoders
# simulate 360 tick encoders. This if statement allows for the
# real robot to handle this difference in devices.
if robot.isReal():
self.left_encoder.setDistancePerPulse(0.042)
self.right_encoder.setDistancePerPulse(0.042)
else:
# Circumference in ft = 4in/12(in/ft)*PI
self.left_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
self.right_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
self.rangefinder = wpilib.AnalogInput(6)
self.gyro = wpilib.AnalogGyro(1)
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 __init__(self) -> None:
super().__init__()
self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port)
self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port)
self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port)
self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port)
# The robot's drive
self.drive = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.left1, self.left2),
wpilib.SpeedControllerGroup(self.right1, self.right2),
)
# The left-side drive encoder
self.leftEncoder = wpilib.Encoder(
*constants.kLeftEncoderPorts,
reverseDirection=constants.kLeftEncoderReversed)
# The right-side drive encoder
self.rightEncoder = wpilib.Encoder(
*constants.kRightEncoderPorts,
reverseDirection=constants.kRightEncoderReversed)
# Sets the distance per pulse for the encoders
self.leftEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
self.rightEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
def __init__(self):
# Mapping object stores port numbers for all connected motors, sensors, and joysticks. See map.py.
Mapping = Map()
# Init drivetrain
self.driveTrain = [wpilib.Spark(Mapping.frontLeftM),
wpilib.Spark(Mapping.frontRightM),
wpilib.Spark(Mapping.backLeftM),
wpilib.Spark(Mapping.backRightM)]
self.driveTrain[0].setInverted(True)
self.driveTrain[2].setInverted(True)
# Init motors
self.elevatorM = wpilib.Spark(Mapping.elevatorM)
self.elevatorM.setInverted(True)
self.winchM = wpilib.Spark(Mapping.winchM)
self.intakeM = wpilib.Spark(Mapping.intakeM)
self.jawsM = wpilib.Spark(Mapping.jawsM)
# Soleniods
self.jawsSol = wpilib.DoubleSolenoid(Mapping.jawsSol['out'], Mapping.jawsSol['in'])
# Init sensors
self.gyroS = wpilib.AnalogGyro(Mapping.gyroS)
self.elevatorLimitS = wpilib.DigitalInput(Mapping.elevatorLimitS)
self.jawsLimitS = wpilib.DigitalInput(Mapping.jawsLimitS)
self.metaboxLimitS = wpilib.DigitalInput(Mapping.metaboxLimitS)
# Encoders
self.elevatorEncoderS = wpilib.Encoder(7, 8, True)
self.elevatorEncoderS.setDistancePerPulse(0.08078)
self.driveYEncoderS = wpilib.Encoder(2, 3)
self.driveYEncoderS.setDistancePerPulse(0.015708)
def __init__(self):
#Climber Motor Setup
self.climberRight = wpilib.Talon(4)
self.climberLeft = wpilib.Talon(3)
self.climberBack = wpilib.Talon(2)
self.climberWheel = wpilib.Talon(18)
#Encoders Setup
self.backEncoder = wpilib.Encoder(2, 3, False, wpilib.Encoder.EncodingType.k1X)
self.leftEncoder = wpilib.Encoder(4, 5, False, wpilib.Encoder.EncodingType.k1X)
self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k1X)
# These are commented out because I think they're redundant,
# encoder counts should automatically be set to zero upon initialization
# self.backEncoder.reset()
# self.leftEncoder.reset()
# self.rightEncoder.reset()
#Misc Variables Setup
self.extend19 = 247000 #the number of encoder counts to extend any actuator by 19 inches
self.extend6 = 78000 #the number of encoder counts to extend any actuator by 6 inches
self.fullRetract = 500
self.extendSpeed = .35
self.retactSpeed = -.25
self.climbWheelSpeed = .1
self.encoderMotor = {self.backEncoder : self.climberBack, self.rightEncoder : self.climberRight, self.leftEncoder : self.climberLeft}
def __init__(self, distance_per_pulse, robot): super().__init__() left_motors_instance = Left_Motors() right_motors_instance = Right_Motors() self.left_motors = left_motors_instance.left_motor_group self.right_motors = right_motors_instance.right_motor_group self.left_shifter = wpilib.Solenoid(0) self.right_shifter = wpilib.Solenoid(1) self.left_encoder = wpilib.Encoder(2,3) self.right_encoder = wpilib.Encoder(4,5) self.left_encoder.setDistancePerPulse(distance_per_pulse) self.right_encoder.setDistancePerPulse(distance_per_pulse) self.gyro = wpilib.ADXRS450_Gyro() self.robot_instance = robot self.drive = DifferentialDrive(self.left_motors, self.right_motors) self.gyro.reset() self.x = 0 self.y = 0 self.heading = math.pi/2 self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0
def createObjects(self):
#Creates the joystick objects
self.joystick = wpilib.Joystick(0)
#Creates the motor control objects
self.drive_l1 = ctre.WPI_VictorSPX(1) #
self.drive_l2 = ctre.WPI_VictorSPX(2) #
self.drive_r1 = ctre.WPI_VictorSPX(3) #
self.drive_r2 = ctre.WPI_VictorSPX(4) #
self.encoder_l = wpilib.Encoder(0, 1)
self.encoder_r = wpilib.Encoder(2, 3)
self.nav = navx.AHRS.create_spi(
) #Gyros can only be used on channels 0 or 1
self.intake_motor = ctre.WPI_TalonSRX(5)
self.intake_motor.setNeutralMode(ctre.NeutralMode.Brake)
self.shooter_motor = ctre.WPI_TalonSRX(6)
self.shooter_motor.setNeutralMode(ctre.NeutralMode.Brake)
if is_sim:
self.physics = physics.PhysicsEngine()
self.last_tm = time.time()
def robotInit(self):
"""Robot-wide initialization code should go here"""
# Basic robot chassis setup
self.stick = wpilib.Joystick(0)
# Create a robot drive with two PWM controlled Talon SRXs.
self.leftMotor = wpilib.PWMTalonSRX(1)
self.rightMotor = wpilib.PWMTalonSRX(2)
self.robot_drive = wpilib.drive.DifferentialDrive(
self.leftMotor, self.rightMotor)
self.leftEncoder = wpilib.Encoder(0, 1, reverseDirection=False)
# The right-side drive encoder
self.rightEncoder = wpilib.Encoder(2, 3, reverseDirection=True)
# Sets the distance per pulse for the encoders
self.leftEncoder.setDistancePerPulse((6 * math.pi) / 1024)
self.rightEncoder.setDistancePerPulse((6 * math.pi) / 1024)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(0)
# Use PIDController to control angle
turnController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD, self.kF)
turnController.setTolerance(self.kToleranceDegrees)
self.turnController = turnController
self.rotateToAngleRate = 0
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.leftEncoder = wpilib.Encoder(0, 1)
self.leftEncoder.setDistancePerPulse(
(self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.leftEncoder.setSimDevice(0)
self.rightEncoder = wpilib.Encoder(3, 4)
self.rightEncoder.setDistancePerPulse(
(self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name = "drivetrain")
self.robot = robot
# The digital gyro plugged into the SPI port on RoboRIO
self.gyro = wpilib.ADXRS450_Gyro()
# Motors used for driving
self.left = rev.CANSparkMax(1, rev.CANSparkMax.MotorType.kBrushless)
self.leftB = rev.CANSparkMax(3, rev.CANSparkMax.MotorType.kBrushless)
self.right = rev.CANSparkMax(2, rev.CANSparkMax.MotorType.kBrushless)
self.rightB = rev.CANSparkMax(4, rev.CANSparkMax.MotorType.kBrushless)
# TODO: switch to DifferentialDrive is the main object that deals with driving
self.drive = DifferentialDrive(self.left, self.right)
#TODO: These probably will not be the actual ports used
self.left_encoder = wpilib.Encoder(2, 3)
self.right_encoder = wpilib.Encoder(4, 5)
# Encoders may measure differently in the real world and in
# simulation. In this example the robot moves 0.042 barleycorns
# per tick in the real world, but the simulated encoders
# simulate 360 tick encoders. This if statement allows for the
# real robot to handle this difference in devices.
# TODO: Measure our encoder's distance per pulse
if robot.isReal():
self.left_encoder.setDistancePerPulse(0.042)
self.right_encoder.setDistancePerPulse(0.042)
else:
# Circumference in ft = 4in/12(in/ft)*PI
self.left_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
self.right_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
def __init__(self, robot):
self.robot = robot
# get drive motors
self.frontLeftCim = ctre.WPI_TalonSRX(robotmap.frontLeftDrive)
self.frontRightCim = ctre.WPI_TalonSRX(robotmap.frontRightDrive)
self.backLeftCim = ctre.WPI_TalonSRX(robotmap.backLeftDrive)
self.backRightCim = ctre.WPI_TalonSRX(robotmap.backRightDrive)
# configure motors
self.configureMotorCurrent(self.frontLeftCim)
self.configureMotorCurrent(self.frontRightCim)
self.configureMotorCurrent(self.backLeftCim)
self.configureMotorCurrent(self.backRightCim)
# reverse left side motors
self.frontLeftCim.setInverted(True)
self.backLeftCim.setInverted(True)
# make drivetrain
self.drivetrain = mecanumdrive.MecanumDrive(self.frontLeftCim, self.backLeftCim, self.frontRightCim, self.backRightCim)
# setup encoders
self.encoderLeft = wpilib.Encoder(aChannel=robotmap.leftEncoderChannelA, bChannel=robotmap.leftEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X)
self.encoderLeft.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.encoderLeft.setDistancePerPulse((6*math.pi)/(256))
self.encoderRight = wpilib.Encoder(aChannel=robotmap.rightEncoderChannelA, bChannel=robotmap.rightEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X)
self.encoderRight.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.encoderRight.setDistancePerPulse((6*math.pi)/(256))
# setup gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
super().__init__()
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)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTables.getTable("SmartDashboard")
# Setting up Kinematics (an algorithm to determine chassi speed from wheel speed)
# The 2d Translation tells the algorithm how far off center (in Meter) our wheels are
# Ours are about 11.3 (x) by 10.11(y) inches off, so this equates to roughly .288 X .257 Meters
# We use the X and Y Offsets above.
m_frontLeftLocation = Translation2d(XOffset, YOffset)
m_frontRightLocation = Translation2d(XOffset, (-1 * YOffset))
m_backLeftLocation = Translation2d((-1 * XOffset), (YOffset))
m_backRightLocation = Translation2d((-1 * XOffset), (-1 * YOffset))
# Creat our kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(
m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation,
)
# Create the Odometry Object
self.MecanumDriveOdometry = MecanumDriveOdometry(
self.m_kinematics,
Rotation2d.fromDegrees(-self.gyro.getAngle()),
Pose2d(0, 0, Rotation2d(0)),
)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.f_l_encoder = wpilib.Encoder(0, 1)
self.f_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_l_encoder = wpilib.Encoder(3, 4)
self.r_l_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.f_r_encoder = wpilib.Encoder(1, 2)
self.f_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.r_r_encoder = wpilib.Encoder(3, 4)
self.r_r_encoder.setDistancePerPulse(
(math.pi * self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Initialize Networktables
self.sd = NetworkTables.getTable('SmartDashboard')
#Set up motors to drive robot
self.M2 = wpilib.VictorSP(2)
self.M3 = wpilib.VictorSP(3)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M2, self.M3)
self.M0 = wpilib.VictorSP(0)
self.M1 = wpilib.VictorSP(1)
self.right = wpilib.SpeedControllerGroup(self.M0, self.M1)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
#Camera
wpilib.CameraServer.launch()
#Servo
self.SV1 = wpilib.Servo(9)
self.SV2 = wpilib.Servo(8)
#Dashboard
NetworkTables.initialize(server='10.61.62.2')
#Switches
self.SW0 = wpilib.DigitalInput(0)
self.SW1 = wpilib.DigitalInput(1)
#Elevator
self.E = wpilib.VictorSP(5)
self.prepareCubeFlag = 0
self.grabCubeFlag = 0
self.deliverCubeFlag = 0
self.adjustLeftFlag = 0
self.adjustRightFlag = 0
self.driveFlag = 0
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.reset()
#All possible autonomous routines in a sendable chooser
'''
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("None", '4')
self.chooser.addObject("left-LeftScale", '1')
self.chooser.addObject("Middle-LeftScale", '2')
self.chooser.addObject("Right-LeftScale", '3')
self.chooser.addObject("Left-RightScale", '5')
'''
#wpilib.SmartDashboard.putData('Choice', self.chooser)
#Encoders
self.EC1 = wpilib.Encoder(2, 3)
self.EC2 = wpilib.Encoder(4, 5)
self.EC1.reset()
self.EC2.reset()
def __init__(self):
self.leftEnc = wpilib.Encoder(
robot_map.LEFT_ENC_ONE, robot_map.LEFT_ENC_TWO, True, wpilib.Encoder.EncodingType.k4X)
self.rightEnc = wpilib.Encoder(
robot_map.RIGHT_ENC_ONE, robot_map.RIGHT_ENC_TWO, True, wpilib.Encoder.EncodingType.k4X)
self.elevatorEnc = wpilib.Encoder(
robot_map.ELEVATOR_ENC_ONE, robot_map.ELEVATOR_ENC_TWO, False, wpilib.Encoder.EncodingType.k4X)
self.ahrs = AHRS.create_i2c()
def createObjects(self):
"""
Create motors and stuff here
"""
# drivetrain motors
self.right_front_motor = ctre.WPI_TalonSRX(robotmap.right_front_drive)
self.right_back_motor = ctre.WPI_TalonSRX(robotmap.right_back_drive)
self.left_front_motor = ctre.WPI_TalonSRX(robotmap.left_front_drive)
self.left_back_motor = ctre.WPI_TalonSRX(robotmap.left_back_drive)
# configure motors - current limit, ramp rate, etc.
MotorConfigurator.bulk_config_drivetrain(self.right_front_motor,
self.right_back_motor,
self.left_front_motor,
self.left_back_motor)
# create motor groups based on side
self.left_drive_motors = wpilib.SpeedControllerGroup(
self.left_back_motor, self.left_front_motor)
self.right_drive_motors = wpilib.SpeedControllerGroup(
self.right_front_motor, self.right_back_motor)
# encoders
self.left_encoder = wpilib.Encoder(
aChannel=robotmap.left_encoder_a,
bChannel=robotmap.left_encoder_b,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.left_encoder.setPIDSourceType(
wpilib.Encoder.PIDSourceType.kDisplacement)
self.right_encoder = wpilib.Encoder(
aChannel=robotmap.right_encoder_a,
bChannel=robotmap.right_encoder_b,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.right_encoder.setPIDSourceType(
wpilib.Encoder.PIDSourceType.kDisplacement)
# create drivetrain based on groupings
self.drive = wpilib.drive.DifferentialDrive(self.left_drive_motors,
self.right_drive_motors)
# ahrs gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.calibrate()
# oi class
self.oi = OI.OI()
# launch automatic camera capturing for main drive cam
# TODO Mount camera
wpilib.CameraServer.launch()
# launch arduino code and start data server
self.arduino_server = ArduinoServer()
self.arduino_server.startServer()
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def __init__(self, robot):
super().__init__()
self.robot = robot
self.front_left_motor = wpilib.Talon(1)
self.back_left_motor = wpilib.Talon(2)
self.front_right_motor = wpilib.Talon(3)
self.back_right_motor = wpilib.Talon(4)
self.drive = wpilib.RobotDrive(
self.front_left_motor,
self.back_left_motor,
self.front_right_motor,
self.back_right_motor,
)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
# Encoders may measure differently in the real world and in
# simulation. In this example the robot moves 0.042 barleycorns
# per tick in the real world, but the simulated encoders
# simulate 360 tick encoders. This if statement allows for the
# real robot to handle this difference in devices.
if robot.isReal():
self.left_encoder.setDistancePerPulse(0.042)
self.right_encoder.setDistancePerPulse(0.042)
else:
# Circumference in ft = 4in/12(in/ft)*PI
self.left_encoder.setDistancePerPulse(
(4.0 / 12.0 * math.pi) / 360.0)
self.right_encoder.setDistancePerPulse(
(4.0 / 12.0 * math.pi) / 360.0)
self.rangefinder = wpilib.AnalogInput(6)
self.gyro = wpilib.AnalogGyro(1)
wpilib.LiveWindow.addActuator("Drive Train", "Front_Left Motor",
self.front_left_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Back Left Motor",
self.back_left_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Front Right Motor",
self.front_right_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Back Right Motor",
self.back_right_motor)
wpilib.LiveWindow.addSensor("Drive Train", "Left Encoder",
self.left_encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Encoder",
self.right_encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Rangefinder",
self.rangefinder)
wpilib.LiveWindow.addSensor("Drive Train", "Gyro", self.gyro)
def __init__(self):
#Flywheel motors setup
self.rightFly = wpilib.Talon(10) #Right Fly Wheel
self.leftFly = wpilib.Talon(11) #Left Fly Wheel
#Arm Shoulder and Wrist motor setup
self.shoulderMotor1 = wpilib.Talon(12) #Arm Shoulder Bottom
self.shoulderMotor2 = wpilib.Talon(14) #Arm Shoulder Top
self.wristMotor = wpilib.Talon(13) #Arm Wrist
#Encoders Setup
self.shoulderEncoder = wpilib.Encoder(6, 7, False,
wpilib.Encoder.EncodingType.k1X)
self.wristEncoder = wpilib.Encoder(8, 9, False,
wpilib.Encoder.EncodingType.k1X)
# These are commented out because I think they're redundant,
# encoder counts should automatically be set to zero upon initialization
# self.shoulderEncoder.reset()
# self.wristEncoder.reset()
#Misc Variables Setup
self.ballIntakeSpeed = 0.1
self.ballSpitSpeed = -0.1
self.shoulderZ = 0.005
self.wristZ = 0.005
self.shoulderSpeedUp = 0.2
self.shoulderSpeedDown = -0.1
self.wristSpeedUp = 0.2
self.wristSpeedDown = -0.1
self.armHeight1LowS = 14500
self.armHeight1HighS = 14600
self.armHeightLowG = 1800
self.armHeightHighG = 1900
self.armHeightHigh2 = 23700
self.armHeightLow2 = 23600
self.armHeightHigh3 = 32700
self.armHeightLow3 = 32800
#The setpoint for the shoulder/wrist
self.shoulderSetpoint = 0
self.wristSetpoint = 0
#The position the shoulder/wrist is currently in. Either that being start, 1st, 2nd, 3rd, or manual
self.shoulderPosition = 'Start'
self.wristPosition = 'Start'
#When the shoulder/wrist isn't being conroller manually then the value is False,
#but when the robot is driving then the value is True
self.shoulderIsDriving = False
self.wristIsDriving = False
def __init__(self):
self.leftMotor = wpilib.Talon(0)
self.rightMotor = wpilib.Talon(1)
self.leftSideMotor = wpilib.Talon(2)
self.rightSideMotor = wpilib.Talon(3)
self.leftEnc = wpilib.Encoder(0, 1)
self.rightEnc = wpilib.Encoder(2, 3)
self.leftSideEnc = wpilib.Encoder(4, 5)
self.rightSideEnc = wpilib.Encoder(6, 7)
self.arcDrive = wpilib.RobotDrive(self.leftMotor, self.rightMotor)
"""
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):
# 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)
def robotInit(self):
self.talon0 = ctre.WPI_TalonSRX(0) #CAN starts on #1 or 0? Don't make the same mistake as last year
self.talon1 = ctre.WPI_TalonSRX(1)#end of left side
self.talon2 = ctre.WPI_TalonSRX(2)
self.talon3 = ctre.WPI_TalonSRX(3)# end of right side
self.left_encoder = wpilib.Encoder(0, 1)
self.right_encoder = wpilib.Encoder(2, 3)
#self.talon4 = wpilib.Talon(4)
#self.talon5 = wpilib.Talon(5) #end of right side
self.stick = wpilib.Joystick(0) #Init joystick on port #0 TOP RIGHT on driverstation!
self.button0 = wpilib.buttons.JoystickButton(self.stick, 2)
self.button10 = wpilib.buttons.JoystickButton(self.stick, 10)
self.left = wpilib.SpeedControllerGroup(self.talon0, self.talon1)#, self.talon2)
self.right = wpilib.SpeedControllerGroup(self.talon2, self.talon3)#, self.talon5)
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right) #set up diff drive using all 6 talons
self.myRobot.setExpiration(0.1) #safety expiration of 10ms / 5ms without signal before stopping
def __init__(self):
print('Elevator: init called')
super().__init__('Elevator')
self.logPrefix = "Elevator: "
try:
self.elevatorSpdCtrl = wpilib.Spark(robotmap.elevator.motorPort)
except Exception as e:
print(
"{}Exception caught instantiating elevator speed controller. {}"
.format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.elevatorEncoder = wpilib.Encoder(robotmap.elevator.encAPort,
robotmap.elevator.encBPort,
robotmap.elevator.encReverse,
robotmap.elevator.encType)
self.elevatorEncoder.setDistancePerPulse(
robotmap.elevator.inchesPerTick)
except Exception as e:
print(
"{}Exception caught instantiating elevator encoder. {}".format(
self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
self.elevatorHeight = self.elevatorEncoder.getDistance()
self.btmLimitSwitch = wpilib.DigitalInput(
robotmap.elevator.btmLimitSwitchPort)
def robotInit(self):
self.joystick = wpilib.Joystick(0)
self.motors = wpilib.SpeedControllerGroup(WPI_VictorSPX(2),
WPI_VictorSPX(3))
self.priorAutospeed = 0
# TODO: Check if we need IdleMode.kBrake
# self.motor.setIdleMode(IdleMode.kBrake);
self.encoder = wpilib.Encoder(
8, 7, True, encodingType=wpilib.Encoder.EncodingType.k1X)
# //
# // Configure encoder related functions -- getDistance and getrate should
# // return units and units/s
# //
# % if units == 'Degrees':
# double encoderConstant = (1 / GEARING) * 360
# % elif units == 'Radians':
# double encoderConstant = (1 / GEARING) * 2. * Math.PI;
# % elif units == 'Rotations':
self.encoderConstant = (1 / (self.ENCODER_PULSES_PER_REV))
self.encoder.setDistancePerPulse(self.encoderConstant)
self.encoder.reset()
NetworkTablesInstance.getDefault().setUpdateRate(0.010)
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):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.arm_motor = wpilib.Spark(1)
self.arm_motor.setInverted(False)
#
# Configure encoder related functions -- getpos and getrate should return
# degrees and degrees/s
#
encoder_constant = (
(1 / self.ENCODER_PULSE_PER_REV) / self.GEARING * 360
)
encoder = wpilib.Encoder(0, 1)
encoder.setDistancePerPulse(encoder_constant)
self.encoder_getpos = (lambda: encoder.getDistance() + self.OFFSET)
self.encoder_getrate = 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)
