def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# make this true to ignore joystick errors
self.debug = False
self.enabled_time = 0
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.navigation = Navigation(self)
self.pneumatics = Pneumatics(self)
self.peripherals = Peripherals(self)
#wpilib.SmartDashboard.putData(self.drivetrain)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
wpilib.SmartDashboard.putData(Scheduler.getInstance())
# instantiate the command used for the autonomous period
self.autonomousCommand = None
def __init__(self):
self.drive = new Nomad()
self.oi = new OI()
self.ds = DriverStation.getInstance()
self.infont = NetworkTables.getTable('info')
self.sensors = new Sensors()
def robotInit(self):
# Instances of classes
# Instantiate Subsystems
self.drivetrain = Drivetrain(self)
self.hp_intake = Hp_Intake()
self.ramp = Ramp()
self.shifters = Shifters()
# instantiate Encoders for drivetrain?
#self.encoders = Encoders(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
# Instantiate Xbox
self.xbox = wpilib.Joystick(2)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
self.loops = 0
# untested vision
#XXX might crash sim
wpilib.CameraServer.launch("vision.py:main")
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
# Define Robot getter
Command.getRobot = lambda: self
# Create subsystems
self.liftElevator = LiftElevator()
self.drivetrain = DriveTrain()
self.intakeOutput = IntakeOutput()
self.compressor = Compressor()
self.compressor.start()
# Shuffleboard options
# Autonomous commands
# self.autonomousCommand = DelayedDriveToBaseLine()
self.autonomousCommand = DriveToBaseLine()
"""
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
"""
self.oi = OI()
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
self.blocker = Blocker(self)
self.climbmotors = Climbmotors(self)
self.climbpistons = Climbpistons(self)
self.agitator = Agitator(self)
wpilib.SmartDashboard.putStringArray(
"Auto List",
["Far Left", "Far Right", "Center", "Center Low Goal", "Default"])
"""self.autoChooser.addOption("Far Left", AutoFarLeft)
self.autoChooser.addOption("Far Right", AutoFarRight)
self.autoChooser.addOption("Center", AutoShoot)
self.autoChooser.addOption("Center Low Goal", AutoCenterLowGoal)
self.autoChooser.setDefaultOption("Default", AutoBackupShoot)
wpilib.SmartDashboard.putData('Select Autonomous...', self.autoChooser)"""
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def createObjects(self):
self.front_left_motor = WPI_TalonSRX(robotmap.front_left_drive)
self.front_right_motor = WPI_TalonSRX(robotmap.front_right_drive)
self.back_left_motor = WPI_TalonSRX(robotmap.back_left_drive)
self.back_right_motor = WPI_TalonSRX(robotmap.back_right_drive)
motor_configuration.bulk_configure(self.front_left_motor, self.front_right_motor, self.back_left_motor, self.back_right_motor)
self.left_motors = wpilib.SpeedControllerGroup(self.front_left_motor, self.back_left_motor)
self.right_motors = wpilib.SpeedControllerGroup(self.front_right_motor, self.back_right_motor)
self.front_left_motor.setInverted(True)
self.front_right_motor.setInverted(True)
self.drivetrain_object = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors)
self.oi = OI()
# self.arduino_serial_connection = serial.Serial(
# port=robotmap.com_port,
# baudrate=9600,
# parity=serial.PARITY_ODD,
# stopbits=serial.STOPBITS_TWO,
# bytesize=serial.SEVENBITS
# )
wpilib.CameraServer.launch('vision.py:main')
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
# Define Robot getter
Command.getRobot = lambda: self
# Shuffleboard options
self.driveSwitcher = wpilib.SendableChooser()
self.driveSwitcher.setDefaultOption("WPI Mecanum",
self.DriveSwitcher.WPI_MECANUM)
self.driveSwitcher.addOption("Field Oriented",
self.DriveSwitcher.FIELD_ORIENTED)
self.driveSwitcher.addOption("Morpheus", self.DriveSwitcher.MORPHEUS)
self.driveSwitcher.addOption("No Joystick", self.DriveSwitcher.NO_JOY)
# Create subsystems
self.mastyBoi = MastBoi()
self.suplex = Flipper()
self.spootnikDrives = SpootnikDrives()
self.hatchEffector = HatchEffector()
self.cargoEffector = CargoEffector()
# Autonomous commands
self.autonomousCommand = DriveToBaseLine()
"""
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
"""
self.oi = OI()
def robotInit(self):
super().__init__()
# Instances of classes
# Instantiate Subsystems
#XXX DEBUGGING
self.drivetrain = Drivetrain(self)
self.shooter = Shooter(self)
self.carrier = Carrier(self)
self.feeder = Feeder(self)
self.intake = Intake(self)
#self.winch = Winch(self)
#self.climber = Climber(self)
#self.climber_big = Climber_Big(self)
#self.climber_little = Climber_Little(self)
# Instantiate Joysticks
self.left_joy = wpilib.Joystick(1)
self.right_joy = wpilib.Joystick(2)
# Instantiate Xbox
self.xbox = wpilib.Joystick(3)
# Instantiate OI; must be AFTER joysticks are inited
self.oi = OI(self)
self.timer = wpilib.Timer()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.oi = OI(self)
self.chassis = Chassis(self)
self.logger = logging.getLogger("robotpy")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.oi = OI(self)
self.drivetrain = Drivetrain(self)
self.oi.setup_button_bindings()
wpilib.CameraServer.launch()
def robotInit(self):
Command.getRobot = lambda x=0: self
self.timer = wpilib.Timer()
self.oneShot = False
self.sampSubsystem = SampSubsystem()
self.testSubsystem = TestSubsystem()
self.autonomousCommand = SampCommand()
self.oi = OI()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Create operator interface
self.oi = OI(self)
# Create subsystems
self.example_subsystem = ExampleSubsystem(self)
#Create the command used for the autonomous period
self.autonomous_command = ExampleCommand(self)
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.puncher = Puncher()
self.claw = Claw()
self.arm = Arm()
self.intake = Intake()
self.elevator = Elevator()
self.hatch = Hatch()
self.intake_winch = IntakeWinch()
self.oi = OI(self)
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
self.oi = OI()
self.controller = self.oi.controller
self.driveTrain = DriveTrain()
self.colorSpinner = ColorSpinner()
self.sushiWheel = SushiRotator()
Command.getRobot = lambda x=0: self
Command.getOi = lambda x=0: self.oi
Command.getSushiWheel = lambda x=0: self.sushiWheel
self.timer = wpilib.Timer()
self.oneShot = False
#self.autonomousCommand = autonomous.AutonomousCommand()
wpilib.CameraServer.launch("vision.py:main")
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.rear_puncher = RearPuncher()
self.arm = Arm()
self.intake = Intake()
self.elevator = Elevator()
self.hatch = Hatch()
self.intake_winch = IntakeWinch()
self.lift = Lift()
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.running = {}
self.omni_driving = True
self.omni_drive = omni_drive
self.drive_motors = DriveMotors(self)
self.oi = OI(self)
self.chassis = Chassis(self)
self.auto_tasks = move_forward_auto # [[list, of, tasks, to_go, through, sequentially], [and, this, list, will, run, in, parallel]
self.current_auto_tasks = []
self.vision_array = Array("d", [0.0, 0.0, 0.0, 0.0])
self.vision_terminate_event = Event()
self.vision = Vision(self.vision_array, self.vision_terminate_event)
def robotInit(self):
self.activated = False
"""Robot-wide initialization code should go here."""
#SwiftCameraServer().launch('camera1.py:main')
#SwiftCameraServer().launch('camera2.py:main')
wpilib.CameraServer.launch()
self.configuration = constants.COMP_BOT
self.arm = Arm(self)
self.drivetrain = DriveTrain(self)
self.pneumatic = Pneumatic(self)
self.compressor = wpilib.Compressor()
self.camera_servo_1 = CameraServo(self, constants.CAMERA_1_PWM,
constants.CAMERA_1_DEFAULT)
self.camera_servo_2 = CameraServo(self, constants.CAMERA_2_PWM,
constants.CAMERA_2_DEFAULT)
self.oi = OI(self)
def robotInit(self):
Command.robot = self
# Init networktables
NetworkTables.initialize()
self.sd = NetworkTables.getTable('SmartDashboard')
# Init subsystems
self.drivetrain = Drivetrain()
self.Arm = Arm()
# Init oi
self.oi = OI(self)
# Init commands
self.auto = Auto()
def robotInit(self):
"""Initialises 'bot w/ all subsystems (winch needs testing) and joysticks"""
#Subsystem initialisation. Woo.
self.drivetrain = Drivetrain(self)
self.lift = Lift(self)
self.claw = Claw(self)
self.mast = Mast(self)
self.winch = Winch(self)
self.oi = OI(self)
#These are self-describing autonomouses. Waaaaaait... Autono-mouse?
self.ThreeToteAutonomousCommand = ThreeToteAutonomous(self)
self.CanAutonomousCommand = CanAutonomous(self)
self.CanNToteAutoCommand = PlayMacro(self, "macro_1.csv")
self.DriveAutonomousCommand = PlayMacro(self, "macro.csv")
self.ToteAutonomousCommand = ToteAutonomous(self)
self.PlayMacroCommand = PlayMacro(self, "autonomous.csv")
def robotInit(self):
"""This function is run when the robot is first started up and should be
used for any initialization code."""
self.drivetrain = DriveTrain(self)
self.elevator = Elevator(self)
self.wrist = Wrist(self)
self.claw = Claw()
self.oi = OI(self)
# instantiate the command used for the autonomous period
self.autonomousCommand = Autonomous(self)
# Show what command your subsystem is running on the SmartDashboard
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.elevator)
wpilib.SmartDashboard.putData(self.wrist)
wpilib.SmartDashboard.putData(self.claw)
def robotInit(self):
super().__init__(self)
# init robot subs and commands
self.Creator = Creator() # program to create robot parts for subs
self.botMap = RobotMap(self)
self.oi = OI(self)
self.teleop = TeleOp(self)
self.Drive = Drive(self)
self.Flywheel = Flywheel(self)
self.Limelight = Limelight(self)
self.Intake = Intake(self)
self.Conveyor = Conveyor(self)
self.Turret = Turret(self)
self.s = Scheduler
def robotInit(self):
"""Initialise the robot."""
#Initialise the subsystems and the button mapping:
self.drivetrain = Drivetrain(self)
self.ears = Ears(self)
self.hat = Hat(self)
self.tilt = Tilt(self)
self.oi = OI(self)
#Initialise the macro shenanigans (should probably clean this up once I get the dashboard sorted)
self.macroTimeout = self.oi.smart_dashboard.getInt("Macro", 15)
self.macroName = self.oi.smart_dashboard.getString("Macro Name", "macro.csv")
Settings.str_macro_name = self.macroName
Settings.num_macro_timeout = self.macroTimeout
macro_string = str(Settings.num_macro_timeout)
print("Robot initialized with a macro timeout of " + macro_string)
self.simpleAutonCommand = PlayMacro(self, "macro.csv")
self.shooterAutonCommand = PlayMacro(self, "macro_launch.csv")
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
self.blocker = Blocker(self)
self.climbmotors = Climbmotors(self)
self.climbpistons = Climbpistons(self)
self.agitator = Agitator(self)
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.setDefaultOption("Drive", DriveAutonomous(self))
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
"""Robot-wide initialization code should go here"""
super().__init__()
if self.isReal(): # use the real drive train
self.drivetrain = DriveTrain(self)
else: # use the simulated drive train
#self.drivetrain = DriveTrainSim(self)
self.drivetrain = DriveTrainSim(self)
self.shooter = Shooter(self)
self.vision = Vision()
# oi MUST be created after all other subsystems since it uses them
self.oi = OI(self)
self.enabled_time = 0 # something is especially weird with the sim about this needing to be initialized in robotInit
self.autonomousCommand = None # initialize the placeholder command for autonomous
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
self.claw = clawsubsystem.ClawSubsystem(self)
self.climb = climbsubsystem.ClimbSubsystem(self)
self.lift = liftsubsystem.LiftSubsystem(self)
self.drive = drivesubsystem.DriveSubsystem(self)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
self.teleDrive = TeleopDriveLowCommand(self)
SmartDashboard.putNumber("creep_mult", 0.3)
def robotInit(self):
"""
Init Robot
"""
# Robot Components
# Constructor params are PWM Ports on the RIO
self.drivetrain = drive.Drivetrain(self, 1,2,3,4)
self.intake = intake.Intake(0, self)
self.popper = popper.Popper(0,0)
self.front_lift = lift.Lift(0,1,2)
self.rear_lift = lift.Lift(0,5,4)
self.imu = imu.IMU(2)
self.encoders = encoders.Encoders()
CameraServer.launch("subsystems/camera.py:main")
self.oi = OI(self)
#self.drivecommand = DriveCommandGroup()
self.timer = Timer()
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot",
DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
