def robotInit(self):
"""Run when the robot turns on."""
field_styles = coloredlogs.DEFAULT_FIELD_STYLES
field_styles['filename'] = {'color': 'cyan'}
coloredlogs.install(
fmt=
"%(asctime)s[%(msecs)d] %(filename)s:%(lineno)d %(name)s %(levelname)s %(message)s",
datefmt="%m-%d %H:%M:%S",
field_styles=field_styles) # install to created handler
Command.getRobot = lambda x=0: self
# Update constants from json file on robot
# if self.isReal():
# Constants.updateConstants()
# Update constants for dashboard editing
Constants.initSmartDashboard()
# Initialize motor objects
drive.Drive().init()
backarm.BackArm().init()
intake.Intake().init()
# The PDP
# self.pdp = PDP(7)
# Set command group member variables
self.autonomous = autogroup.AutonomousCommandGroup()
self.disabled = disabledgroup.DisabledCommandGroup()
self.disabled.setRunWhenDisabled(True)
self.teleop = teleopgroup.TeleopCommandGroup()
self.test = testgroup.TestCommandGroup()
self.global_ = globalgroup.GlobalCommandGroup()
self.global_.setRunWhenDisabled(True)
# Start the camera sever
CameraServer.launch()
self.watchdog = watchdog.Watchdog(Constants.WATCHDOG_TIMEOUT,
self._watchdogTimeout)
self.globalInit()
LiveWindow.disableAllTelemetry()
def test(self):
while self.isTest() and self.isEnabled():
LiveWindow.run()
for component in self.components.values():
component.stop()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
def __init__(self, deviceNumber: int):
super().__init__(deviceNumber)
hal.report(hal.UsageReporting.kResourceType_CTRE_future3,
deviceNumber + 1)
self.description = "Victor SPX %s" % (deviceNumber, )
MotorSafety.__init__(self)
self.setExpiration(0.0)
self.setSafetyEnabled(False)
SendableBase.__init__(self)
LiveWindow.add(self)
self.setName("Victor SPX ", deviceNumber)
self.speed = 0.0
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
def test(self):
for component in self.components.values():
component.stop()
while self.isTest() and self.isEnabled():
LiveWindow.run()
self.update_networktables()
def testPeriodic(self):
LiveWindow.run()
self.updateDashboard()