class MyRobot(magicbot.MagicRobot): # high level components cargo_mechanism: CargoMechanism hatch_mechanism: HatchMechanism drivetrain_mechanism: DrivetrainMechanism pressure_sensor: PressureSensor # low level components cargo_rotator: CargoRotator cargo_locking_servo: CargoServo hatch_grabber: HatchGrabber hatch_rack: HatchRack drivetrain: Drivetrain shifters: Shifters def createObjects(self): # CARGO MECHANISM self.cargo_mechanism_motor_rotator = ctre.WPI_TalonSRX( robotmap.Ports.Cargo.rotator) self.cargo_mechanism_servo_lock = wpilib.Servo( robotmap.Ports.Cargo.locking_servo) utils.configure_motor( self.cargo_mechanism_motor_rotator, ctre.NeutralMode.Brake, robotmap.MotorConfiguration.Cargo.peak_current, robotmap.MotorConfiguration.Cargo.peak_current_duration) self.cargo_mechanism_motor_rotator.setInverted( robotmap.MotorConfiguration.Cargo.inverted) # HATCH MECHANISM self.hatch_grab_actuator = wpilib.DoubleSolenoid( robotmap.Ports.Hatch.Grabber.pcm, robotmap.Ports.Hatch.Grabber.front, robotmap.Ports.Hatch.Grabber.back) self.hatch_rack_actuator_left = wpilib.DoubleSolenoid( robotmap.Ports.Hatch.Extension.pcm, robotmap.Ports.Hatch.Extension.left_front, robotmap.Ports.Hatch.Extension.left_back) self.hatch_rack_actuator_right = wpilib.DoubleSolenoid( robotmap.Ports.Hatch.Extension.pcm, robotmap.Ports.Hatch.Extension.right_front, robotmap.Ports.Hatch.Extension.right_back) # DRIVETRAIN SYSTEM # TODO fix motor port naming. self.drivetrain_right_front = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.right_back) self.drivetrain_right_back = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.right_bottom) self.drivetrain_right_top = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.right_top) self.drivetrain_left_front = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.left_front) self.drivetrain_left_back = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.left_bottom) self.drivetrain_left_top = ctre.WPI_TalonSRX( robotmap.Ports.Drivetrain.Motors.left_top) utils.configure_drivetrain_motors(self.drivetrain_left_back, self.drivetrain_left_front, self.drivetrain_left_top, self.drivetrain_right_back, self.drivetrain_right_front, self.drivetrain_right_top) self.drivetrain_right_motors = wpilib.SpeedControllerGroup( self.drivetrain_right_back, self.drivetrain_right_front, self.drivetrain_right_top) self.drivetrain_left_motors = wpilib.SpeedControllerGroup( self.drivetrain_left_back, self.drivetrain_left_front, self.drivetrain_left_top) self.differential_drive = wpilib.drive.DifferentialDrive( self.drivetrain_left_motors, self.drivetrain_right_motors) self.left_shifter_actuator = wpilib.DoubleSolenoid( robotmap.Ports.Drivetrain.Shifters.pcm, robotmap.Ports.Drivetrain.Shifters.left_front, robotmap.Ports.Drivetrain.Shifters.left_back) self.right_shifter_actuator = wpilib.DoubleSolenoid( robotmap.Ports.Drivetrain.Shifters.pcm, robotmap.Ports.Drivetrain.Shifters.right_front, robotmap.Ports.Drivetrain.Shifters.right_back) self.pressure_sensor_input = wpilib.AnalogInput( robotmap.Ports.PressureSensor.port) # MISC self.oi = OI() # run camera streaming program wpilib.CameraServer.launch("vision.py:main") self.current_camera = 0 self.camera_table = networktables.NetworkTables.getTable( "/CameraPublisher") # this is important for this year... # self.use_teleop_in_autonomous = True self.navx = navx.AHRS.create_spi() def teleopInit(self): self.drivetrain_mechanism.reset() self.cargo_mechanism.reset() self.hatch_mechanism.reset() def teleopPeriodic(self): try: # DRIVETRAIN if self.drivetrain.drive_mode == DriveModes.ARCADEDRIVE: self.drivetrain.arcade_drive( self.oi.drivetrain_curve( self.oi.driver.getY(self.oi.driver.Hand.kLeft)), -self.oi.driver.getX(self.oi.driver.Hand.kRight)) elif self.drivetrain.drive_mode == DriveModes.TANKDRIVE: self.drivetrain.tank_drive( self.oi.drivetrain_curve( self.oi.driver.getY(self.oi.driver.Hand.kLeft)), self.oi.drivetrain_curve( self.oi.driver.getY(self.oi.driver.Hand.kRight))) if self.navx.isConnected() and self.oi.check_drivetrain_straight( self.oi.driver.getX(self.oi.driver.Hand.kRight), self.oi.driver.getY(self.oi.driver.Hand.kLeft)): self.drivetrain.drive_straight( self.oi.drivetrain_curve( self.oi.driver.getY(self.oi.driver.Hand.kLeft))) elif self.drivetrain.drive_mode == DriveModes.DRIVETOANGLE: self.drivetrain.drive_mode = DriveModes.ARCADEDRIVE if self.oi.get_drivetrain_shift(): self.drivetrain_mechanism.toggle_shift() # if self.oi.get_drive_mode_switch(): # self.drivetrain.toggle_mode() # HATCHES if self.oi.get_hatch_grabber(): self.hatch_mechanism.toggle_grab() if self.oi.get_hatch_rack(): self.hatch_mechanism.toggle_extended() # CARGO cargo_power = self.oi.process_deadzone( self.oi.sysop.getY(self.oi.sysop.Hand.kLeft), robotmap.Tuning.CargoMechanism.deadzone) if cargo_power > 0: self.cargo_mechanism.raise_lift(cargo_power) if cargo_power < 0: self.cargo_mechanism.lower_lift(-cargo_power) if self.oi.get_cargo_lock(): self.cargo_mechanism.toggle_lock() # SENSORS if self.oi.get_camera_switch(): self.current_camera = 0 if self.current_camera == 1 else 1 if self.oi.get_calibrate_pressure(): self.pressure_sensor.calibrate_pressure() # SMARTDASHBOARD dash.putNumber("Calibrated Pressure: ", self.pressure_sensor.get_pressure()) dash.putString("Grabber: ", self.hatch_grabber.state) dash.putString("Rack: ", self.hatch_rack.state) dash.putString("Drive Mode: ", self.drivetrain.drive_mode) self.camera_table.putString("Selected Camera", f"{self.current_camera}") except: self.onException()