def __init__(self, robot):
#Motor Setup
self.RakeMotor = ctre.WPI_TalonSRX(7)
self.ConveyorMotor1 = ctre.WPI_TalonSRX(1)
self.ConveyorMotor2 = ctre.WPI_TalonSRX(6)
self.Flywheel = ctre.WPI_TalonSRX(10)
#Timer Setup
self.ShootTimer = wpilib.Timer()
self.PrintTimer = wpilib.Timer()
self.PrintTimer.start()
self.RakeTimer = wpilib.Timer()
self.IndexTimer = wpilib.Timer()
#Encoder Setup
#Fix these
#self.Conveyor1Encoder = wpilib.Encoder(6, 7, False, wpilib.Encoder.EncodingType.k1X)
#self.Conveyor2Encoder = wpilib.Encoder(8, 9, False, wpilib.Encoder.EncodingType.k1X)
#self.FlywheelEncoder = wpilib.Encoder(10, 11, False, wpilib.Encoder.EncodingType.k1X)
#self.ConveyorMotor1.get
#Misc Setup:
self.IndexSensor1 = wpilib.DigitalInput(5)
self.IndexSensor2 = wpilib.DigitalInput(6)
self.IndexSensor3 = wpilib.DigitalInput(7)
#self.BumpSwitch = wpilib.DigitalInput(1)
self.RakeRelease1 = wpilib.PWM(0)
self.RakeRelease2 = wpilib.PWM(1)
self.RakeDropping = True
self.AutoShoot = False
self.AutoShootLow = False
self.TimerBegin = False
self.TimerRunning = False
self.IndexRunning = False
self.WaitTime = .5
self.ConveyorIndex = 1536 #1.5 rotations * 1024 encoder counts, Placeholder
#---------------------------------------------------------------------------------
#PID loop for velocity
#keeps the flywheel going at a constant RPM at all times
#Corrects for errors
self.Flywheel.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.Flywheel.config_kF(0, 0.01, 0)
self.Flywheel.config_kP(0, 0.01, 0)
self.Flywheel.config_kI(0, 0.00001, 0)
self.Flywheel.config_kD(0, 0.0, 0)
self.fleshWound = wpilib.Timer()
self.fleshWound.start()
self.Errorzone = 0
def __init__(self):
super().__init__('Elevator')
self.sensor = wpilib.DigitalInput(9) # temp num, true is on
self.motor = ctre.WPI_TalonSRX(3)
self.other_motor = ctre.WPI_TalonSRX(2)
self.right_motor = ctre.WPI_TalonSRX(14)
self.other_right_motor = ctre.WPI_TalonSRX(15)
self.zeroed = False
self.motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.motor.configOpenLoopRamp(0.1, 0)
self.elevator_table = networktables.NetworkTables.getTable('/Elevator')
self.motor.setSensorPhase(True)
self.initialize_motionMagic()
self.fan = wpilib.PWM(1)
self.fan2 = wpilib.PWM(0)
self.timer = wpilib.Timer()
self.timer.start()
self.logger = None
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 = ??
class HatchGrab(Subsystem):
def __init__(self):
print('HatchGrab: init called')
super().__init__('HatchGrab')
self.logPrefix = "HatchGrab: "
self.hatchGrabSolenoid = wpilib.Solenoid(1,robotmap.hatchgrab.solenoid)
self.servo1 = wpilib.PWM(robotmap.cargograb)
self.servo2 = wpilib.PWM(robotmap.cargograb
def HatchOpen(self):
self.hatchGrabSolenoid.set(1)
def retractRamp(self):
self.hatchGrabSolenoid.set(2)
def initDefaultCommand(self):
self.setDefaultCommand(CargoTeleopDefault)
print("{}Default command set to CargoGrab".format(self.logPrefix))
def __init__(self):
self.yaw = sd.getEntry('yaw').getDouble(0)
self.pitch = sd.getEntry('pitch').getDouble(0)
self.flywheelPort = 0
#determine from Chazzy-poo
self.hoodReduction = 12
self.turretMotorID = 11
self.spinMotorID = 12
self.spinMotor2ID = 13
self.engageMotorID = 14
self.engageMotor = rev.CANSparkMax(self.engageMotorID,rev.MotorType.kBrushless)
self.turretMotor = rev.CANSparkMax(self.turretMotorID,rev.MotorType.kBrushless)
self.spinMotor = rev.CANSparkMax(self.spinMotorID,rev.MotorType.kBrushless)
self.spinMotor2 = rev.CANSparkMax(self.spinMotor2ID,rev.MotorType.kBrushless)
self.kP = 0.0
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.setBounds(0,360)
self.turretTurnController.setTolerance(0.1)
self.sP = 0.0
self.sI = 0.0
self.sD = 0.0
self.cruisingVelocity = 5500 #in rpm
self.minVelocity = 1000
self.spinController = rev._impl.CANPIDController(self.spinMotor)
self.spinController.setP(self.sP,self.flywheelPort)
self.spinController.setI(self.sI,self.flywheelPort)
self.spinController.setD(self.sD,self.flywheelPort)
self.spinController.setSmartMotionMaxVelocity(self.cruisingVelocity,self.flywheelPort)
self.spinController.setSmartMotionMinOutputVelocity(self.minVelocity, self.flywheelPort)
#don't know whether we are using two motors, likely depends on an epic battle between Liam and Chaz
#Chaz won (we are just using one)
'''
self.spinController2 = rev._impl.CANPIDController(self.spinMotor2)
self.spinController2.setP(self.sP,self.flywheelPort)
self.spinController2.setI(self.sI,self.flywheelPort)
self.spinController2.setD(self.sD,self.flywheelPort)
self.spinController2.setSmartMotionMaxVelocity(self.cruisingVelocity,self.flywheelPort)
self.spinController2.setSmartMotionMinOutputVelocity(self.minVelocity, self.flywheelPort)
'''
self.stallLimit = 78
self.freeLimit = 22
self.limitRPM = 2200
self.spinMotor.setSmartCurrentLimit(self.stallLimit,self.freeLimit,self.limitRPM)
self.hoodServo = wpilib.PWM(0)
self.hoodServo2 = wpilib.PWM(1)
#not a definite value, just guesstimation
self.defaultVelocity = 4500
self.startAngle = 30 #need to determine physically what the angle is initially
self.idealAngle = 35
def robotInit(self):
# devices
self.controller = wpilib.XboxController(0)
self.buttonBoard = wpilib.Joystick(1)
ahrs = navx.AHRS.create_spi()
self.pdp = wpilib.PowerDistributionPanel(50)
self.ledStrip = wpilib.PWM(0)
self.ledInput = -0.99
self.superDrive = drivetrain.initDrivetrain()
self.pathFollower = sea.PathFollower(self.superDrive, ahrs)
# for autonomous mode
self.autoScheduler = autoScheduler.AutoScheduler()
self.autoScheduler.idleFunction = self.autoIdle
self.autoScheduler.updateCallback = self.updateScheduler
# controls the state of the robot
self.controlModeMachine = sea.StateMachine()
self.manualState = sea.State(self.driving)
self.autoState = sea.State(self.autoScheduler.runSchedule)
self.testState = sea.State(self.testing)
# for shifting gear box
self.compressor = wpilib.Compressor(0)
self.piston1 = wpilib.DoubleSolenoid(0, 1)
self.piston2 = wpilib.DoubleSolenoid(2, 3)
# drive gears
self.superDrive.gear = None
self.driveGear = drivetrain.mediumVoltageGear
self.driveMode = "velocity"
self.driveSpeed = "medium"
self.driveGears = \
{"voltage" : \
{"slow" : drivetrain.slowVoltageGear,
"medium" : drivetrain.mediumVoltageGear,
"fast" : drivetrain.fastVoltageGear},
"velocity" : \
{"slow" : drivetrain.slowVelocityGear,
"medium" : drivetrain.mediumVelocityGear,
"fast" : drivetrain.fastVelocityGear},
"position" : \
{"slow" : drivetrain.slowPositionGear,
"medium" : drivetrain.mediumPositionGear,
"fast" : drivetrain.fastPositionGear}
}
# testing
self.testSettings = { \
"wheelNum" : 0,
"motorNum" : 0,
"speed" : 0
}
# for dashboard motor data
self.motorData = [dict() for _ in range(len(self.superDrive.motors))]
# sets initial values so the dashboard doesn't break when it tries to get
# them before the values are updated in self.updateMotorData
for motor in range(len(self.superDrive.motors)):
initAmps = round(self.superDrive.motors[motor].getOutputCurrent(), 2)
initTemp = round(self.superDrive.motors[motor].getMotorTemperature(), 2)
self.motorData[motor]["amps"] = initAmps
self.motorData[motor]["temp"] = initTemp
self.motorData[motor]["maxAmp"] = initAmps
self.motorData[motor]["maxTemp"] = initTemp
self.app = None
sea.startDashboard(self, dashboard.CompetitionDashboard)
def create_pwm(channel):
return wpilib.PWM(channel)