def robotInit(self):
global stick, drive, gyro, highGear
stick = wpilib.Joystick(1)
drive = wpilib.RobotDrive((wpilib.CANTalon(0)), (wpilib.CANTalon(2)),
(wpilib.CANTalon(1)), (wpilib.CANTalon(3)))
drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft, True)
drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft, True)
gyro = wpilib.Gyro(0)
highGear = wpilib.DoubleSolenoid(0, 1)
def __init__(self, robot):
super().__init__()
self.robot = robot
self.front_left_motor = wpilib.Talon(1)
self.back_left_motor = wpilib.Talon(2)
self.front_right_motor = wpilib.Talon(3)
self.back_right_motor = wpilib.Talon(4)
self.drive = wpilib.RobotDrive(self.front_left_motor,
self.back_left_motor,
self.front_right_motor,
self.back_right_motor)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
# Encoders may measure differently in the real world and in
# simulation. In this example the robot moves 0.042 barleycorns
# per tick in the real world, but the simulated encoders
# simulate 360 tick encoders. This if statement allows for the
# real robot to handle this difference in devices.
if robot.isReal():
self.left_encoder.setDistancePerPulse(0.042)
self.right_encoder.setDistancePerPulse(0.042)
else:
# Circumference in ft = 4in/12(in/ft)*PI
self.left_encoder.setDistancePerPulse(
(4.0 / 12.0 * math.pi) / 360.0)
self.right_encoder.setDistancePerPulse(
(4.0 / 12.0 * math.pi) / 360.0)
self.rangefinder = wpilib.AnalogInput(6)
self.gyro = wpilib.Gyro(1)
wpilib.LiveWindow.addActuator("Drive Train", "Front_Left Motor",
self.front_left_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Back Left Motor",
self.back_left_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Front Right Motor",
self.front_right_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Back Right Motor",
self.back_right_motor)
wpilib.LiveWindow.addSensor("Drive Train", "Left Encoder",
self.left_encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Encoder",
self.right_encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Rangefinder",
self.rangefinder)
wpilib.LiveWindow.addSensor("Drive Train", "Gyro", self.gyro)
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(1)
self.rstick = wpilib.Joystick(2)
self.lr_motor = wpilib.Jaguar(1)
self.rr_motor = wpilib.Jaguar(2)
self.lf_motor = wpilib.Jaguar(3)
self.rf_motor = wpilib.Jaguar(4)
self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor,
self.lf_motor, self.rf_motor)
# Position gets automatically updated as robot moves
self.gyro = wpilib.Gyro(1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.stick = wpilib.Joystick(1)
self.drive = wpilib.RobotDrive(0, 1, 2, 3)
self.gyro = wpilib.Gyro(0)
self.lift1 = wpilib.Jaguar(4)
self.lift2 = wpilib.Jaguar(5)
#self.backclaw = wpilib.Jaguar(7)
self.encoder = wpilib.Encoder(1, 2)
self.encoder.setDistancePerPulse(0.333)
self.compressor = wpilib.Compressor()
self.squeeze = wpilib.DoubleSolenoid(0, 1)
self.RobotDrive.setInvertedMotor(2, True)
self.RobotDrive.setInvertedMotor(3, True)
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(1)
self.rstick = wpilib.Joystick(2)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.Gyro(1)
self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
def robotInit(self):
frontLeft = wpilib.Victor(0)
frontRight = wpilib.Victor(1)
rearLeft = wpilib.Victor(2)
rearRight = wpilib.Victor(3)
self.robot_drive = wpilib.RobotDrive(frontLeftMotor=frontLeft,
frontRightMotor=frontRight,
rearLeftMotor=rearLeft,
rearRightMotor=rearRight)
self.joystick = wpilib.Joystick(0)
self.gyro = wpilib.Gyro(0)
#calibrate the gyro
self.gyro_drift = 0.0
wpilib.Timer.delay(2.0)
last_angle = self.gyro.getAngle()
wpilib.Timer.delay(10.0)
self.gyro_drift = (self.gyro.getAngle() - last_angle) / 10.0
self.timer = wpilib.Timer()
self.dashboard = dashboard.Dashboard()
def send_thread():
while True:
self.dashboard.send_udp(dashboard.encode_gyro(
self.get_angle()))
wpilib.Timer.delay(0.05)
def recv_thread():
while True:
self.dashboard.get_msg()
t_send = threading.Thread(target=send_thread)
t_send.daemon = True
t_send.start()
t_recv = threading.Thread(target=recv_thread)
t_recv.daemon = True
def __init__(self):
super().__init__()
print("Matt the fantastic ultimate wonderful humble person")
wpilib.SmartDashboard.init()
#self.digitalInput=wpilib.DigitalInput(4)
self.CANJaguar = wpilib.CANJaguar(1)
self.gyro = wpilib.Gyro(1)
self.joystick = wpilib.Joystick(1)
self.joystick2 = wpilib.Joystick(2)
self.jaguar = wpilib.Jaguar(1)
self.accelerometer = wpilib.ADXL345_I2C(1,
wpilib.ADXL345_I2C.kRange_2G)
self.solenoid = wpilib.Solenoid(7)
self.solenoid2 = wpilib.Solenoid(8)
self.p = 1
self.i = 0
self.d = 0
wpilib.SmartDashboard.PutBoolean('Soleinoid 1', False)
wpilib.SmartDashboard.PutBoolean('Soleinoid 2', False)
#self.pid = wpilib.PIDController(self.p, self.i, self.d, self.gyro, self.jaguar)
self.sensor = wpilib.AnalogChannel(5)
self.ballthere = False
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(1)
self.rstick = wpilib.Joystick(2)
self.lr_motor = wpilib.Jaguar(1)
self.rr_motor = wpilib.Jaguar(2)
self.lf_motor = wpilib.Jaguar(3)
self.rf_motor = wpilib.Jaguar(4)
self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor,
self.lf_motor, self.rf_motor)
# The output function of a mecanum drive robot is always
# +1 for all output wheels. However, traditionally wired
# robots will be -1 on the left, 1 on the right.
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontLeft, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
# Position gets automatically updated as robot moves
self.gyro = wpilib.Gyro(1)
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)
def __init__(self, robot):
self.robot = robot
# Configure drive motors
self.frontLeftCIM = wpilib.Victor(1)
self.frontRightCIM = wpilib.Victor(2)
self.backLeftCIM = wpilib.Victor(3)
self.backRightCIM = wpilib.Victor(4)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Left CIM",
self.frontLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Right CIM",
self.frontRightCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Left CIM",
self.backLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Right CIM",
self.backRightCIM)
# Configure the RobotDrive to reflect the fact that all our motors are
# wired backwards and our drivers sensitivity preferences.
self.drive = wpilib.RobotDrive(self.frontLeftCIM, self.frontRightCIM,
self.backLeftCIM, self.backRightCIM)
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(.1)
self.drive.setSensitivity(.5)
self.drive.setMaxOutput(1.0)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft,
True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight,
True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft,
True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight,
True)
# Configure encoders
self.rightEncoder = wpilib.Encoder(
1,
2,
reverseDirection=True,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(
3,
4,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.rightEncoder.setPIDSourceParameter(
wpilib.Encoder.PIDSourceParameter.kDistance)
self.leftEncoder.setPIDSourceParameter(
wpilib.Encoder.PIDSourceParameter.kDistance)
if robot.isReal():
# Converts to feet
self.rightEncoder.setDistancePerPulse(0.0785398)
self.leftEncoder.setDistancePerPulse(0.0785398)
else:
# Convert to feet 4in diameter wheels with 360 tick simulated encoders.
self.rightEncoder.setDistancePerPulse((4 * math.pi) / (360 * 12))
self.leftEncoder.setDistancePerPulse((4 * math.pi) / (360 * 12))
wpilib.LiveWindow.addSensor("DriveTrain", "Right Encoder",
self.rightEncoder)
wpilib.LiveWindow.addSensor("DriveTrain", "Left Encoder",
self.leftEncoder)
# Configure gyro
# -> the original pacgoat example is at channel 2, but that was before WPILib
# moved to zero-based indexing. You need to change the gyro channel in
# /usr/share/frcsim/models/PacGoat/robots/PacGoat.SDF, from 2 to 0.
self.gyro = wpilib.Gyro(0)
if robot.isReal():
# TODO: Handle more gracefully
self.gyro.setSensitivity(0.007)
wpilib.LiveWindow.addSensor("DriveTrain", "Gyro", self.gyro)
super().__init__()
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# hello
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.sweeper_relay = wpilib.Relay(0)
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.can_motor = wpilib.CANTalon(15)
self.sensor = Sensor(self.tote_motor, self.can_motor)
self.tote_forklift = ToteForklift(self.tote_motor, self.sensor, 2)
self.can_forklift = CanForklift(self.can_motor, self.sensor, 3)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.next_pos = 1
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift,
self.drive)
self.components = {
'tote_forklift': self.tote_forklift,
'can_forklift': self.can_forklift,
'drive': self.drive,
'align': self.align,
'sensors': self.sensor
}
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.components)
# #Defining Buttons##
self.canUp = Button(self.joystick1, 3)
self.canDown = Button(self.joystick1, 2)
self.canTop = Button(self.joystick1, 6)
self.canBottom = Button(self.joystick1, 7)
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.ui_joystick_can_up = Button(self.ui_joystick, 5)
self.ui_joystick_can_down = Button(self.ui_joystick, 3)
self.reverseDirection = Button(self.joystick1, 1)
#self.alignTrigger = Button(self.joystick2, 1)
self.toteTo = 0
self.oldTote = 0
self.canTo = 0
self.oldCan = 0
self.reverseRobot = False
self.oldReverseRobot = False
self.autoLift = False
self.sd.putNumber('liftTo', 0)
self.sd.putNumber('binTo', 0)
self.sd.putBoolean('autoLift', False)
self.sd.putBoolean('reverseRobot', False)
def __init__(self):
''' initialize things'''
super().__init__()
print("Electrical test program -- don't use for competition!")
#################################################################
# 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.lr_motor = wpilib.Jaguar(2)
self.rr_motor = wpilib.Jaguar(3)
self.rf_motor = wpilib.Jaguar(4)
self.winch_motor = wpilib.CANJaguar(5)
self.intake_motor = wpilib.Jaguar(6)
# Catapult gearbox control
self.gearbox_in_solenoid = wpilib.Solenoid(1)
self.gearbox_out_solenoid = wpilib.Solenoid(2)
# Arm up/down control
self.vent_bottom_solenoid = wpilib.Solenoid(3)
self.fill_bottom_solenoid = wpilib.Solenoid(4)
self.fill_top_solenoid = wpilib.Solenoid(5)
self.vent_top_solenoid = wpilib.Solenoid(6)
self.solenoid6 = wpilib.Solenoid(7)
self.ball_nudge_solenoid = wpilib.Solenoid(8)
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.arm_angle_sensor = wpilib.AnalogChannel(4)
self.ball_sensor = wpilib.AnalogChannel(6)
self.accelerometer = wpilib.ADXL345_I2C(1,
wpilib.ADXL345_I2C.kRange_2G)
self.compressor = wpilib.Compressor(1, 1)
self.compressor.Start()