class MyRobot(TimedRobot): # Defines the channels that are used on the inputs. In the simulator, they have to match up with the physics.py file # This is really useful when you have a variable used hundreds of times and you want to have it set so you can # change it all in one go. RLMotorChannel = 2 RRMotorChannel = 0 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 # RobotInit is where all of the things we are using is initialized. def robotInit(self): # Note the lack of the camera server initialization here. # Initializing drive motors RLMotor = Spark(self.RLMotorChannel) RRMotor = Spark(self.RRMotorChannel) FLMotor = Spark(self.FLMotorChannel) FRMotor = Spark(self.FRMotorChannel) # Grouping drive motors into left and right. self.Left = SpeedControllerGroup(RLMotor, FLMotor) self.Right = SpeedControllerGroup(RRMotor, FRMotor) # Initializing drive Joystick. self.DriveStick = Joystick(self.DriveStickChannel) # Initializing drive function. self.drive = DifferentialDrive(self.Left, self.Right) # Sets the right side motors to be inverted. self.drive.setRightSideInverted(rightSideInverted=True) # Turns the drive off after .1 seconds of inactivity. self.drive.setExpiration(0.1) # Components is a dictionary that contains any variables you want to put into it. All of the variables put into # components dictionary is given to the autonomous modes. self.components = {"drive": self.drive} # Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous # modes should inherit. self.automodes = autonomous.AutonomousModeSelector( "Sim_Autonomous", self.components) # Autonomous and teleop periodic both run the code on a fixed loop time. A part of TimedRobot. def autonomousPeriodic(self): # runs the autonomous modes when Autonomous is activated. self.automodes.run() def teleopPeriodic(self): # Turns on drive safety and checks to se if operator control and the robot is enabled. self.drive.setSafetyEnabled(True) if self.isOperatorControl() and self.isEnabled(): # Drives the robot with controller inputs. You can use buttons with self.DriveStick.getRawButton(ButtonNum). self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False)
class Driver(): def driveTrainInit(self, robot, controlScheme): self.controlScheme = controlScheme self.leftMotorGroup = wpilib.SpeedControllerGroup( ctre.WPI_TalonSRX(ports.talonPorts.get("leftChassisMotor"))) self.rightMotorGroup = wpilib.SpeedControllerGroup( ctre.WPI_TalonSRX(ports.talonPorts.get("rightChassisMotor"))) self.drive = DifferentialDrive(self.leftMotorGroup, self.rightMotorGroup) self.drive.setRightSideInverted(True) def driveRobotWithJoystick(self): speed = self.controlScheme.joystickDriveForward() * ( 1 - (0.95 * self.controlScheme.triggerSlowSpeed())) #speed = joystick.getY(GenericHID.Hand.kLeft)*(1-(0.75*joystick.getTriggerAxis(GenericHID.Hand.kRight))) rotation = self.alignWithVisionTargets( self.controlScheme.joystickTurn(), self.controlScheme.buttonVisionAlign()) * ( 1 - (0.95 * self.controlScheme.triggerSlowRotation())) #rotation = self.alignWithVisionTargets(joystick)*(1-(0.75*joystick.getTriggerAxis(GenericHID.Hand.kLeft))) self.drive.arcadeDrive(-speed, rotation, squareInputs=True) def alignWithVisionTargets(self, rotation, buttonVisionAlign): visionTable = NetworkTables.getTable('PiData') if (buttonVisionAlign): visionAlignment = visionTable.getNumber('yawToTarget', 0) if (visionAlignment < 0): rotation = -0.4 elif (visionAlignment > 0): rotation = 0.4 else: rotation = rotation return rotation