def __init__ (self): ''' Constructor. ''' super().__init__() print("Team 1418 robot code for 2014") ################################################################# # THIS CODE IS SHARED BETWEEN THE MAIN ROBOT AND THE ELECTRICAL # # TEST CODE. WHEN CHANGING IT, CHANGE BOTH PLACES! # ################################################################# wpilib.SmartDashboard.init() # Joysticks self.joystick1 = wpilib.Joystick(1) self.joystick2 = wpilib.Joystick(2) # Motors self.lf_motor = wpilib.Jaguar(1) self.lf_motor.label = 'lf_motor' self.lr_motor = wpilib.Jaguar(2) self.lr_motor.label = 'lr_motor' self.rr_motor = wpilib.Jaguar(3) self.rr_motor.label = 'rr_motor' self.rf_motor = wpilib.Jaguar(4) self.rf_motor.label = 'rf_motor' self.winch_motor = wpilib.CANJaguar(5) self.winch_motor.label = 'winch' self.intake_motor = wpilib.Jaguar(6) self.intake_motor.label = 'intake' # Catapult gearbox control self.gearbox_solenoid=wpilib.DoubleSolenoid(2, 1) self.gearbox_solenoid.label = 'gearbox' # Arm up/down control self.vent_bottom_solenoid = wpilib.Solenoid(3) self.vent_bottom_solenoid.label = 'vent bottom' self.fill_bottom_solenoid = wpilib.Solenoid(4) self.fill_bottom_solenoid.label = 'fill bottom' self.fill_top_solenoid = wpilib.Solenoid(5) self.fill_top_solenoid.label = 'fill top' self.vent_top_solenoid = wpilib.Solenoid(6) self.vent_top_solenoid.label = 'vent top' self.pass_solenoid = wpilib.Solenoid(7) self.pass_solenoid.label = 'pass' self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor, self.lf_motor, self.rf_motor) self.robot_drive.SetSafetyEnabled(False) self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kFrontLeftMotor, True) self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kRearLeftMotor, True) # Sensors self.gyro = wpilib.Gyro(1) self.ultrasonic_sensor = wpilib.AnalogChannel(3) self.ultrasonic_sensor.label = 'Ultrasonic' self.arm_angle_sensor = wpilib.AnalogChannel(4) self.arm_angle_sensor.label = 'Arm angle' self.ball_sensor = wpilib.AnalogChannel(6) self.ball_sensor.label = 'Ball sensor' self.accelerometer = wpilib.ADXL345_I2C(1, wpilib.ADXL345_I2C.kRange_2G) self.compressor = wpilib.Compressor(1,1) ################################################################# # END SHARED CODE # ################################################################# # # Initialize robot components here # self.drive = drive.Drive(self.robot_drive, self.ultrasonic_sensor,self.gyro) self.initSmartDashboard() self.pushTimer=wpilib.Timer() self.catapultTimer=wpilib.Timer() self.catapult=catapult.Catapult(self.winch_motor,self.gearbox_solenoid,self.pass_solenoid,self.arm_angle_sensor,self.ball_sensor,self.catapultTimer) self.intakeTimer=wpilib.Timer() self.intake=intake.Intake(self.vent_top_solenoid,self.fill_top_solenoid,self.fill_bottom_solenoid,self.vent_bottom_solenoid,self.intake_motor,self.intakeTimer) self.pulldowntoggle=False self.components = { 'drive': self.drive, 'catapult': self.catapult, 'intake': self.intake } self.control_loop_wait_time = 0.025 self.autonomous = AutonomousModeManager(self.components)