def robotInit(self):
# SPARK MAX controllers are intialized over CAN by constructing a
# CANSparkMax object
#
# The CAN ID, which can be configured using the SPARK MAX Client, is passed
# as the first parameter
#
# The motor type is passed as the second parameter.
# Motor type can either be:
# rev.MotorType.kBrushless
# rev.MotorType.kBrushed
#
# The example below initializes four brushless motors with CAN IDs
# 1, 2, 3, 4. Change these parameters to match your setup
self.leftLeadMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rightLeadMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.leftFollowMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.rightFollowMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
# Passing in the lead motors into DifferentialDrive allows any
# commmands sent to the lead motors to be sent to the follower motors.
self.driveTrain = DifferentialDrive(self.leftLeadMotor, self.rightLeadMotor)
self.joystick = wpilib.Joystick(0)
# The RestoreFactoryDefaults method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.leftLeadMotor.restoreFactoryDefaults()
self.rightLeadMotor.restoreFactoryDefaults()
self.leftFollowMotor.restoreFactoryDefaults()
self.rightFollowMotor.restoreFactoryDefaults()
# In CAN mode, one SPARK MAX can be configured to follow another. This
# is done by calling the follow() method on the SPARK MAX you want to
# configure as a follower, and by passing as a parameter the SPARK MAX
# you want to configure as a leader.
#
# This is shown in the example below, where one motor on each side of
# our drive train is configured to follow a lead motor.
self.leftFollowMotor.follow(self.leftLeadMotor)
self.rightFollowMotor.follow(self.rightLeadMotor)
def __init__(self, driveID, turnID, encoderID, encoderOffset, name):
if name == "Front Left":
kPTurn = .006
kITurn = .002
elif name == "Front Right":
kPTurn = .008
kITurn = .002
elif name == "Rear Left":
kPTurn = .007
kITurn = .002
elif name == "Rear Right":
kPTurn = .0066
kITurn = 0
else:
kPTurn = .0066
kITurn = .0018
kDTurn = 0
self.driveMotor = rev.CANSparkMax(driveID, rev.MotorType.kBrushless)
self.driveMotor.restoreFactoryDefaults()
self.turnMotor = rev.CANSparkMax(turnID, rev.MotorType.kBrushless)
self.turnEncoder = self.turnMotor.getEncoder()
self.turnEncoder.setPositionConversionFactor(
self.turnMotorEncoderConversion) #now is 0-360
self.driveEncoder = self.driveMotor.getEncoder()
self.absoluteEncoder = wpilib.AnalogInput(encoderID)
self.encoderOffset = encoderOffset
self.turnController = wpilib.controller.PIDController(
kPTurn, kITurn, kDTurn)
self.turnController.enableContinuousInput(
-180, 180) #the angle range we decided to make standard
self.turnDeadband = .035 #this needs to be updated
self.moduleName = name
self.x = 0
self.y = 0
self.pastPosition = 0
def initDrivetrain(self):
drivetrain = sea.SuperHolonomicDrive()
leftSpark = rev.CANSparkMax(1, rev.MotorType.kBrushless)
rightSpark = rev.CANSparkMax(2, rev.MotorType.kBrushless)
for spark in [leftSpark, rightSpark]:
spark.restoreFactoryDefaults()
spark.setIdleMode(rev.IdleMode.kBrake)
leftWheel = sea.AngledWheel(leftSpark, -1, 0, math.pi / 2, 1, 16)
rightWheel = sea.AngledWheel(rightSpark, 1, 0, math.pi / 2, 1, 16)
drivetrain.addWheel(leftWheel)
drivetrain.addWheel(rightWheel)
for wheel in drivetrain.wheels:
wheel.driveMode = rev.ControlType.kVelocity
return drivetrain
def __init__(self, driveID, turnID, encoderID, encoderOffset, name):
self.driveMotor = rev.CANSparkMax(driveID, rev.MotorType.kBrushless)
self.turnMotor = rev.CANSparkMax(turnID, rev.MotorType.kBrushless)
self.turnEncoder = self.turnMotor.getEncoder()
self.turnEncoder.setPositionConversionFactor(
self.turnMotorEncoderConversion) #now is 0-360
self.absoluteEncoder = wpilib.AnalogInput(encoderID)
self.encoderOffset = encoderOffset
#the above line centers the absolute encoder and then changes its direction, as the absolute encoders spin
#the opposite direction of the NEO encoders
self.turnController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD)
self.turnController.enableContinuousInput(
-180, 180) #the angle range we decided to make standard
self.turnDeadband = .035
self.moduleName = name
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Joystick
self.stick = wpilib.Joystick(0)
# Rev through CAN(lift)
self.left_lift = rev.CANSparkMax(4, rev.MotorType.kBrushless)
#Timer
self.timer = wpilib.Timer()
def robotInit(self):
#Controllers
self.driver = wpilib.XboxController(0)
self.operator = wpilib.XboxController(1)
#Motors
self.left_motor_1 = rev.CANSparkMax(robotmap.LEFT_LEADER_ID, rev.MotorType.kBrushed)
self.left_motor_2 = rev.CANSparkMax(robotmap.LEFT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.right_motor_1 = rev.CANSparkMax(robotmap.RIGHT_LEADER_ID, rev.MotorType.kBrushed)
self.right_motor_2 = rev.CANSparkMax(robotmap.RIGHT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.left_motor_1.setClosedLoopRampRate(1.0)
self.left_motor_2.setClosedLoopRampRate(1.0)
self.right_motor_1.setClosedLoopRampRate(1.0)
self.right_motor_2.setClosedLoopRampRate(1.0)
self.left_side = wpilib.SpeedControllerGroup(self.left_motor_1, self.left_motor_2)
self.right_side = wpilib.SpeedControllerGroup(self.right_motor_1, self.right_motor_2)
#Drivetrain
self.drivetrain = wpilib.drive.DifferentialDrive(self.left_side, self.right_side)
#TODO: Add subsystems and sensors as the code is written
#TODO: SmartDashboard
#Pneumatics
self.colorPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.COLOR_SENSOR_EXTEND, robotmap.COLOR_SENSOR_RETRACT))
self.climberPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.CLIMBER_EXTEND, robotmap.CLIMBER_RETRACT))
self.gearshiftPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.DRIVE_SHIFT_EXTEND, robotmap.DRIVE_SHIFT_RETRACT))
self.climberWinchMotor1 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR1, rev.MotorType.kBrushed)
self.climberWinchMotor2 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR2, rev.MotorType.kBrushed)
# Color Sensor
self.colorSensor = color_sensor()
self.colorSensorMotor = rev.CANSparkMax(robotmap.COLOR_SENSOR_MOTOR, rev.MotorType.kBrushed)
self.stopColorMap = {"R":"B", "Y":"G", "B":"R", "G":"Y"}
self.gameData = ""
self.setupColorSensor()
#Shooter
self.shooter = shooter(robotmap.LOADER, robotmap.SHOOTER)
# Gyro
self.ahrs = AHRS.create_spi()
self.Aimer = Aimer(self.ahrs)
#network tables
self.sd = NetworkTables.getTable('VISION')
def __init__(self, robot, can_id):
super().__init__()
self.robot = robot
self.wristMotor = rev.CANSparkMax(can_id, rev.MotorType.kBrushless)
self.encoder = SwiftCanEncoder(self.wristMotor.getEncoder())
self.encoder.setDistancePerPulse(constants.WRIST_DEGREES_FACTOR)
self.encoder.setPosition(constants.WRIST_START_POSITION)
self.pid = wpilib.PIDController(.07, 0, 0, self.encoder, self.wristMotor)
self.pid.setAbsoluteTolerance(3)
self.pid.setEnabled(False)
def robotInit(self):
self.frontLeft: CANSparkMax = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.rearLeft: CANSparkMax = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight: CANSparkMax = rev.CANSparkMax(22, rev.MotorType.kBrushless)
self.rearRight: CANSparkMax = rev.CANSparkMax(10, rev.MotorType.kBrushless)
self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight)
for motor in [self.frontLeft, self.rearLeft, self.frontRight, self.rearRight]:
motor.clearFaults()
motor.setOpenLoopRampRate(0.5)
motor.setSmartCurrentLimit(60, 60, 6400) # >= 15 sec. stall tested
motor.setSecondaryCurrentLimit(100)
motor.getEncoder().setPositionConversionFactor(42)
#motor.setIdleMode(0)
SmartDashboard.putNumber("Left Power", 0)
SmartDashboard.putNumber("Right Power", 0)
self.leftStick = wpilib.Joystick(0)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.lt_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.lf_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.lb_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rt_motor = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.rf_motor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.rb_motor = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.left = wpilib.SpeedControllerGroup(self.lt_motor, self.lf_motor,
self.lb_motor)
self.right = wpilib.SpeedControllerGroup(self.rt_motor, self.rf_motor,
self.rb_motor)
self.drive = DifferentialDrive(self.left, self.right)
self.lt_motor.setInverted(True)
self.rt_motor.setInverted(True)
self.joystick = wpilib.Joystick(0)
self.previous_button = False
self.gear_switcher = wpilib.DoubleSolenoid(0, 1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.leftSpark1 = rev.CANSparkMax(15, rev.MotorType.kBrushless)
self.leftSpark2 = rev.CANSparkMax(14, rev.MotorType.kBrushless)
self.leftSpark3 = rev.CANSparkMax(13, rev.MotorType.kBrushless)
self.rightSpark1 = rev.CANSparkMax(20, rev.MotorType.kBrushless)
self.rightSpark2 = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rightSpark3 = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.leftSparks = wpilib.SpeedControllerGroup(self.leftSpark1,
self.leftSpark2,
self.leftSpark3)
self.rightSparks = wpilib.SpeedControllerGroup(self.rightSpark1,
self.rightSpark2,
self.rightSpark3)
self.leftSpark1.setIdleMode(rev.IdleMode.kBrake)
self.leftSpark2.setIdleMode(rev.IdleMode.kBrake)
self.leftSpark3.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark1.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark2.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark3.setIdleMode(rev.IdleMode.kBrake)
self.openCloseSolenoid = wpilib.DoubleSolenoid(2, 5)
self.inOutSolenoid = wpilib.DoubleSolenoid(3, 4)
def __init__(self, robot):
super().__init__("drivetrainsim")
self.robot = robot
self.counter = 0 # used for updating the log
self.x, self.y = 0, 0
# initialize sensors
self.navx = navx.AHRS.create_spi()
# initialize motors
self.spark_neo_left_front = wpilib.Jaguar(1)
self.spark_neo_left_rear = wpilib.Jaguar(2)
self.spark_neo_right_front = wpilib.Jaguar(3)
self.spark_neo_right_rear = wpilib.Jaguar(4)
motor_type = rev.MotorType.kBrushless
self.spark_neo_left_front1 = rev.CANSparkMax(1, motor_type)
self.spark_neo_left_rear1 = rev.CANSparkMax(2, motor_type)
self.spark_neo_right_front1 = rev.CANSparkMax(3, motor_type)
self.spark_neo_right_rear1 = rev.CANSparkMax(4, motor_type)
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(self.spark_neo_left_front1)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(self.spark_neo_left_rear1)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(self.spark_neo_right_front1)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(self.spark_neo_right_rear1)
# create drivetrain from motors
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front, self.spark_neo_left_rear)
self.speedgroup_right = SpeedControllerGroup(self.spark_neo_right_front, self.spark_neo_right_rear)
self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left, self.speedgroup_right)
# self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front)
self.drive.setMaxOutput(1.0)
# initialize encoders - doing this after motors because they may be part of the motor controller
self.l_encoder = wpilib.Encoder(0, 1, True)
self.r_encoder = wpilib.Encoder(2, 3, True)
self.l_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
self.r_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
# odometry for tracking the robot pose
self.odometry = wpimath.kinematics.DifferentialDriveOdometry(geo.Rotation2d.fromDegrees( -self.navx.getAngle() ))
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
# setup camera
# setup master and slave drive motors
self.drive_slave_left = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_LEFT_ID)
self.drive_master_left = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_LEFT_ID)
self.drive_master_left.follow(self.drive_slave_left)
self.drive_slave_right = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_RIGHT_ID)
self.drive_master_right = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_RIGHT_ID)
self.drive_master_right.follow(self.drive_slave_right)
# setup actuator motors
self.intake_motor = lazytalonsrx.LazyTalonSRX(self.INTAKE_ID)
self.low_tower_motor = lazytalonsrx.LazyTalonSRX(self.LOW_TOWER_ID)
self.high_tower_motor = lazytalonsrx.LazyTalonSRX(self.HIGH_TOWER_ID)
self.turret_motor = lazytalonsrx.LazyTalonSRX(self.TURRET_ID)
self.turret_motor.setEncoderConfig(lazytalonsrx.CTREMag, True)
self.flywheel_motor = rev.CANSparkMax(self.FLYWHEEL_MOTOR_ID,
rev.MotorType.kBrushless)
self.climb_winch_slave = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_SLAVE_ID)
self.climb_winch_master = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_MASTER_ID)
self.climb_winch_slave.follow(self.climb_winch_master)
self.slider_motor = lazytalonsrx.LazyTalonSRX(self.SLIDER_ID)
# setup imu
self.imu = lazypigeonimu.LazyPigeonIMU(self.intake_motor)
# setup proximity sensors
self.tower_sensors = [wpilib.DigitalInput(i) for i in range(0, 5)]
self.intake_sensors = [wpilib.DigitalInput(8), wpilib.DigitalInput(9)]
# setup joysticks
self.driver = wpilib.Joystick(0)
self.operator = wpilib.XboxController(1)
self.nt = NetworkTables.getTable("robot")
self.manual_indexer = True
self.chassis_slow = False
def robotInit(self):
try:
wpilib.CameraServer.launch()
except:
pass
self.stick = wpilib.Joystick(0)
self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless),
rev.CANSparkMax(2, rev.MotorType.kBrushless),
rev.CANSparkMax(3, rev.MotorType.kBrushless),
rev.CANSparkMax(1, rev.MotorType.kBrushless),
AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick)
self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0),
wpilib.DigitalInput(1), self.stick)
self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4),
wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5),
self.stick)
self.testvar = 0
self.ballintake = BallIntake(WPI_TalonSRX(4))
try:
self.ledController = LEDController.getInstance()
except:
pass
# Various one-time initializations happen here.
self.lift.initSensor()
self.base.navx.reset()
try:
NetworkTables.initialize()
except:
pass
try:
self.visiontable = NetworkTables.getTable("visiontable")
except:
pass
self.lights = False
self.op = False
self.testButton = JoystickButton(self.stick, 16)
self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick,
"init")
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 __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.rightB)
#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 robotInit(self): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = .0039 self.kI = 2e-6 self.kD = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.PIDTolerance = 1.0 self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .1 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0 wpilib.CameraServer.launch()
def robotInit(self):
# Create motors
self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
# You must call getPIDController() on an existing CANSparkMax or
# SparkMax object to fully use PID functionality
self.pidController = self.motor.getPIDController()
# Instantiate built-in encoder to display position
self.encoder = self.motor.getEncoder()
self.joystick = wpilib.Joystick(0)
# PID Coefficents and Controller Output Range
self.kP = 0.1
self.kI = 1e-4
self.kD = 0
self.kIz = 0
self.kFF = 0
self.kMinOutput = -1
self.kMaxOutput = 1
# Motor max RPM
self.maxRPM = 5700
# The restoreFactoryDefaults() method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.motor.restoreFactoryDefaults()
# Set PID Constants
self.pidController.setP(self.kP)
self.pidController.setI(self.kI)
self.pidController.setD(self.kD)
self.pidController.setIZone(self.kIz)
self.pidController.setFF(self.kFF)
self.pidController.setOutputRange(self.kMinOutput, self.kMaxOutput)
# Push PID Coefficients to SmartDashboard
wpilib.SmartDashboard.putNumber("P Gain", self.kP)
wpilib.SmartDashboard.putNumber("I Gain", self.kI)
wpilib.SmartDashboard.putNumber("D Gain", self.kD)
wpilib.SmartDashboard.putNumber("I Zone", self.kIz)
wpilib.SmartDashboard.putNumber("Feed Forward", self.kFF)
wpilib.SmartDashboard.putNumber("Min Output", self.kMinOutput)
wpilib.SmartDashboard.putNumber("Max Output", self.kMaxOutput)
def robotInit(self): self.kP = 0.03 self.kI = 0.00 self.kD = 0.00 self.kF = 0.00 self.exampleButton = 7 #Sets up your PID controller with set constants self.controller = PIDController(self.kP, self.kI, self.kD) #set the range of values the Input will give #spark max are 42counts per revolution self.controller.enableContinuousInput(0, 42) #you can find information about the motor stuff in the documentation self.speed = self.controller.calculate(rev.CANSparkMax(4,rev.MotorType.kBrushless).getEncoder())
def robotInit(self): self.green = 1 self.red = 1 self.blue = 1 self.yellow = 1 # these buttons are for the different stages, where the wheel will spin until it has the right color or the right number of rotations self.rotationsSpinButton = wpilib.DriverStation.getStickButton(0,0) #joystick channel and button number self.colorSpinButton = wpilib.DriverStation.getStickButton(0,1) self.ethansMotor = rev.CANSparkMax(4,rev.MotorType.kBrushless) self.ethansServo = wpilib.PWM(1) #this is the servo that flips up the wheel, it is connected to pwm channel 1 # .setPosition(0.0-1.0) means -90 degrees to 90 degrees self.spinning = False self.targetColor= green self.colorTime = False self.colorSensor = ??
def createObjects(self):
self.intake_motor = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.flashdrive_left_master = ctre.WPI_TalonSRX(1)
self.flashdrive_left_slave1 = ctre.WPI_VictorSPX(2)
self.flashdrive_left_slave2 = ctre.WPI_VictorSPX(3)
self.flashdrive_right_master = ctre.WPI_TalonSRX(4)
self.flashdrive_right_slave1 = ctre.WPI_VictorSPX(5)
self.flashdrive_right_slave2 = ctre.WPI_VictorSPX(6)
self.flashdrive_left_drive = wpilib.SpeedControllerGroup(
self.flashdrive_left_master, self.flashdrive_left_slave1,
self.flashdrive_left_slave2)
self.flashdrive_right_drive = wpilib.SpeedControllerGroup(
self.flashdrive_right_master, self.flashdrive_right_slave1,
self.flashdrive_right_slave2)
self.flashdrive_drivetrain = wpilib.drive.DifferentialDrive(
self.flashdrive_left_drive, self.flashdrive_right_drive)
self.joystick = wpilib.Joystick(0)
self.elevator_motor = ctre.WPI_TalonSRX(7)
def robotInit(self):
self.arm_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.stick = wpilib.Joystick(0)
# self.thing = SharpIR2Y0A21(0)
self.arm_motor.restoreFactoryDefaults()
self.arm_pidController = self.arm_motor.getPIDController()
self.arm_encoder = self.arm_motor.getEncoder()
self.kP = 0.1
self.kI = 0
self.kD = 0
self.kMaxOutput = 0.3
self.kMinOutput = -0.2
self.arm_pidController.setP(self.kP)
self.arm_pidController.setI(self.kI)
self.arm_pidController.setD(self.kD)
self.arm_pidController.setOutputRange(self.kMinOutput, self.kMaxOutput)
self.arm_encoder.setPosition(0.0)
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.leftSpark1 = rev.CANSparkMax(15, rev.MotorType.kBrushless) self.leftSpark2 = rev.CANSparkMax(14, rev.MotorType.kBrushless) self.leftSpark3 = rev.CANSparkMax(13, rev.MotorType.kBrushless) self.rightSpark1 = rev.CANSparkMax(20, rev.MotorType.kBrushless) self.rightSpark2 = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rightSpark3 = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.joystick1 = wpilib.Joystick(0) self.joystick2 = wpilib.Joystick(1) self.leftSparks = wpilib.SpeedControllerGroup(self.leftSpark1, self.leftSpark2, self.leftSpark3) self.rightSparks = wpilib.SpeedControllerGroup(self.rightSpark1, self.rightSpark2, self.rightSpark3)
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.imu = navx.AHRS.create_spi(update_rate_hz=50)
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.Solenoid(0)
self.intake_main_motor = ctre.WPI_TalonSRX(18)
self.indexer_motors = [
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
]
self.injector_motor = ctre.WPI_TalonSRX(14)
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(2, 3)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.codriver_gamepad = wpilib.XboxController(1)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
# how long before data times out
self.has_zeroed = False
def __init__(self):
self.revError = 30
self.turretTolerance = .5
self.turretMotorID = 15
self.turretMotor = rev.CANSparkMax(self.turretMotorID,
rev.MotorType.kBrushless)
self.turretEncoder = self.turretMotor.getEncoder()
self.turretEncoder.setPositionConversionFactor(12.414)
self.spinEncoder = self.spinMotor.getEncoder()
self.kP = 0.0085
self.kI = 0.0
self.kD = 0.0
self.turretTurnController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD)
self.turretTurnController.setSetpoint(0)
self.turretTurnController.setTolerance(0.1)
self.zeroTurret()
def robotInit(self):
# Create motor
self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.joystick = wpilib.Joystick(0)
# The restoreFactoryDefaults method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
self.motor.restoreFactoryDefaults()
# Parameters can be set by calling the appropriate set() method on the
# CANSparkMax object whose properties you want to change
#
# Set methods will return one of three CANError values which will let
# you know if the parameter was successfully set:
# CANError.kOk
# CANError.kError
# CANError.kTimeout
if self.motor.setIdleMode(rev.IdleMode.kCoast) is not rev.CANError.kOk:
wpilib.SmartDashboard.putString("Idle Mode", "Error")
# Similarly, parameters will have a get() method which allows you to
# retrieve their values from the controller
if self.motor.getIdleMode() is rev.IdleMode.kCoast:
wpilib.SmartDashboard.putString("Idle Mode", "Coast")
else:
wpilib.SmartDashboard.putString("Idle Mode", "Brake")
# Set ramp rate to 0
if self.motor.setOpenLoopRampRate(0) is not rev.CANError.kOk:
wpilib.SmartDashboard.putString("Ramp Rate", "Error")
# Read back ramp value
wpilib.SmartDashboard.putString("Ramp Rate",
str(self.motor.getOpenLoopRampRate()))
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Set up smart Dashboard
self.sd = NetworkTables.getTable("SmartDashboard")
# Set up motors
self.motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.encoder = rev._impl.CANEncoder(self.motor)
self.motor_PID = rev._impl.CANPIDController(self.motor)
# self.right_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
# self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor)
# Set up the joystick
self.stick = wpilib.Joystick(0)
# set up the timer
self.timer = wpilib.Timer()
# Set up the PID Gains as constants. These Values are Reccommended by Rev
self.kP = .1
self.kI = .0001
self.kD = 0
self.kIz = 0
self.kFF = 0
self.kMaxOutput = .5
self.kMinOutput = -.5
self.rotations = 0.0
self.maxRPM = 5700
# Put the PID gains into shuffleboard
self.sd.putNumber("P Gain", self.kP)
self.sd.putNumber("I Gain", self.kI)
self.sd.putNumber("D Gain", self.kD)
self.sd.putNumber("I Zone", self.kIz)
self.sd.putNumber("Feed Forward", self.kFF)
self.sd.putNumber("Max Output", self.kMaxOutput)
self.sd.putNumber("Min Output", self.kMinOutput)
self.sd.putNumber("Setpoint", self.rotations)
def __init__(self, intakeID, halfMoonID): self.intakeMotor = rev.CANSparkMax(intakeID, rev.MotorType.kBrushless) self.halfMoonMotor = rev.CANSparkMax(halfMoonID, rev.MotorType.kBrushless)
def __init__(self,port):
self.motor = rev.CANSparkMax(port, rev.MotorType.kBrushed)
def __init__(self, flyWheelID): self.flyWheelMotor = rev.CANSparkMax(flyWheelID, MotorType.kBrushless) self.flyWheelEncoder = self.flyWheelMotor.getEncoder()
def robotInit(self): #I got fed up with how long 'frontLeft, frontRight, etc.' looked #Drive motors self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.driveMotors = (self.flDriveMotor, self.frDriveMotor, self.rlDriveMotor, self.rrDriveMotor) #Turn motors self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless) self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.turnMotors = (self.flTurnMotor, self.frTurnMotor, self.rlTurnMotor, self.rrTurnMotor) #Turn encoders self.flTurnEncoder = self.flTurnMotor.getEncoder() self.frTurnEncoder = self.frTurnMotor.getEncoder() self.rlTurnEncoder = self.rlTurnMotor.getEncoder() self.rrTurnEncoder = self.rrTurnMotor.getEncoder() self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder, self.rlTurnEncoder, self.rrTurnEncoder) for encoder in self.turnEncoders: encoder.setPositionConversionFactor(20) #makes the encoder output in degrees '''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If we use the build-in, the following will be an issue and a potential solution. Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value. I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what the absolute encoder says it is. Something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) self.turnControllers[index].setReference(0, rev.controlType.kPosition) that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0. Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only able to reset the wheel positions by hand''' # PID coefficients self.kP = .0039 self.kI = 0 self.kD = 2.0e-6 self.PIDTolerance = 1.0 #PID controllers for the turn motors self.flTurnController = PIDController(self.kP, self.kI, self.kD) self.frTurnController = PIDController(self.kP, self.kI, self.kD) self.rlTurnController = PIDController(self.kP, self.kI, self.kD) self.rrTurnController = PIDController(self.kP, self.kI, self.kD) self.turnControllers = (self.flTurnController, self.frTurnController, self.rlTurnController, self.rrTurnController) for controller in self.turnControllers: controller.setTolerance(self.PIDTolerance) controller.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 33.75 self.robotWidth = 29.75
