def __init__(self): super().__init__() self.l_motor = SyncGroup(Talon, constants.motor_drive_l) self.r_motor = SyncGroup(Talon, constants.motor_drive_r) self.l_encoder = Encoder(*constants.encoder_drive_l) self.r_encoder = Encoder(*constants.encoder_drive_r) DISTANCE_PER_REV = 4 * math.pi TICKS_PER_REV = 128 REDUCTION = 30 / 36 self.l_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV) self.r_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV) self.gyro = Gyro(constants.gyro) self._gyro_p = 0.12 self._gyro_d = 0.005 self._prev_gyro_error = 0 quickdebug.add_printables(self, [ ('gyro angle', self.gyro.getAngle), ('left encoder', self.l_encoder.getDistance), ('right encoder', self.r_encoder.getDistance), 'left_pwm', 'right_pwm', 'encoder_goal' ]) quickdebug.add_tunables(self, ['_gyro_p', '_gyro_d'])
def __init__(self, Robot): #self.navx = Robot.navx self.speedLimit = 0.999 self._leftRamp = 0.0 self._rightRamp = 0.0 self._rampSpeed = 6.0 self._kOonBalanceAngleThresholdDegrees = 5.0 self._autoBalance = True self._quickstop_accumulator = 0.0 self._old_wheel = 0.0 self._driveReversed = True self._drivePool = DataPool("Drive") # setup drive train motors self.rightDrive = SyncGroup(RIGHT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.leftDrive = SyncGroup(LEFT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.rightDrive.setInverted(True) # setup drive train gear shifter self.shifter = Solenoid(DRIVE_SHIFTER_PORT) self.shifter.set(False) # setup drive train encoders self.leftEncoder = Encoder(LEFT_DRIVE_ENCODER_A, LEFT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RIGHT_DRIVE_ENCODER_A, RIGHT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.leftEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) self.leftEncoder.setReverseDirection(True) self.rightEncoder.setReverseDirection(False) self.rightEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK)
class Arm(Subsystem): """Raise and lower the robot's arm.""" def __init__(self): """Assign ports and save them for use in the move and stop methods.""" super().__init__() self.arm = ctre.WPI_TalonSRX(11) self.armencoder = Encoder(0, 1) self.armencoder.setDistancePerPulse(0.14) def move(self, value): """Move the arm according to the left and right Xbox controller triggers.""" if self.armencoder.getDistance() < 40: self.arm.set(value) if value > 0: direction = "up" elif value < 0: direction = "down" else: direction = "stopped" print("Arm moving", direction, "at", value) print("Arm angle is " + "%3f" % self.armencoder.getDistance()) def stop(self): """Stop the arm.""" self.arm.set(0.0)
def __init__(self, l_a=0, l_b=1, r_a=2, r_b=3): self.left = Encoder(l_a, l_b, reverseDirection=True) self.right = Encoder(r_a, r_b) self.left.setDistancePerPulse(self.dpp) self.right.setDistancePerPulse(self.dpp)
def __init__(self, robot): super().__init__("shooter") self.robot = robot self.counter = 0 # used for updating the log self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional # motor controllers self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.spark_hood = Talon(5) self.spark_feed = Talon(7) # encoders and PID controllers self.hood_encoder = Encoder( 4, 5) # generic encoder - we'll have to install one on the 775 motor self.hood_encoder.setDistancePerPulse(1 / 1024) self.hood_controller = wpilib.controller.PIDController(Kp=0.005, Ki=0, Kd=0.0) self.hood_setpoint = 5 self.hood_controller.setSetpoint(self.hood_setpoint) self.flywheel_encoder = self.sparkmax_flywheel.getEncoder( ) # built-in to the sparkmax/neo self.flywheel_controller = self.sparkmax_flywheel.getPIDController( ) # built-in PID controller in the sparkmax # limit switches, use is TBD self.limit_low = DigitalInput(6) self.limit_high = DigitalInput(7)
def __init__(self): self.robot = SpartanRobot.getRobotObject(self) self.setDefaultCommand(ArcadeDriveCommand()) #Power output to motors in range of -1 to 1 self.leftPower = 0 self.rightPower = 0 self.leftEncoder = Encoder(RobotMap.leftDriveEncoder1, RobotMap.leftDriveEncoder2, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RobotMap.rightDriveEncoder1, RobotMap.rightDriveEncoder2, True, Encoder.EncodingType.k4X) self.gyro = ADXRS450_Gyro() self.rightDriveMotor1 = VictorSP(RobotMap.rightDriveMotor1) self.rightDriveMotor2 = VictorSP(RobotMap.rightDriveMotor2) # self.rightDriveMotor3 = VictorSP(rightDriveMotor3) self.leftDriveMotor1 = VictorSP(RobotMap.leftDriveMotor1) self.leftDriveMotor2 = VictorSP(RobotMap.leftDriveMotor2) # self.leftDriveMotor3 = VictorSP(leftDriveMotor3) self.leftEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks) self.rightEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks) self.leftEncoder.setMaxPeriod(5) self.rightEncoder.setMaxPeriod(5) self.leftEncoder.setMinRate(0) self.rightEncoder.setMinRate(0) self.leftEncoder.setSamplesToAverage(1) self.rightEncoder.setSamplesToAverage(1) self.gyro.calibrate()
def __init__(self, robot): Subsystem.__init__(self, 'Turret') self.robot = robot self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X) motors = {} self.map = self.robot.botMap self.tmotors = motors for name in self.map.motorMap.motors: motors[name] = robot.Creator.createMotor( self.map.motorMap.motors[name]) for name in self.tmotors: self.tmotors[name].setInverted( self.map.motorMap.motors[name]['inverted']) self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast) self.kP = 0.05 self.kI = 0.000 self.kD = 0.002 self.turretPID = PID(self.kP, self.kI, self.kD) self.turretPID.limitVal(0.3) self.setPow = 0
class Encoder(Sensor): """ Sensor wrapper for a quadrature encoder. Has double attributes distance, rate (distance/second); boolean attributes stopped and direction. """ distance = rate = 0 stopped = direction = True def __init__(self, channel_a, channel_b, pulse_dist=1.0, reverse=False, modnum=1, cpr=128, enctype=CounterBase.k4x): """ Initializes the encoder with two channels, distance per pulse (default 1), no reversing, on module number 1, 128 CPR, and with 4x counting. """ super().__init__() self.e = WEncoder(modnum, channel_a, channel_b, reverse, enctype) self.cpr = cpr self.pulse_dist = pulse_dist def poll(self): dist = self.e.GetDistance() self.distance = dist self.rate = self.e.GetRate() self.stopped = self.e.GetStopped() self.direction = self.e.GetDirection()
def __init__(self): """Assign ports and save them for use in the move and stop methods.""" super().__init__() self.arm = ctre.WPI_TalonSRX(11) self.armencoder = Encoder(0, 1) self.armencoder.setDistancePerPulse(0.14)
def __init__(self, robot): super().__init__("Arm") self.robot = robot self.peakCurrentLimit = 30 self.PeaKDuration = 50 self.continuousLimit = 15 motor = {} for name in self.robot.RobotMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name]) self.motors = motor for name in self.motors: self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.motors[name].enableCurrentLimit(True) self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X) self.Zero = DigitalInput(6) self.kp = 0.00035 self.ki = 0.00000000001 self.kd = 0.0000001 self.ArmPID = PID(self.kp, self.ki, self.kd)
class Encoders: """ Encoder group class for drivetrain - We use 2 E4T encoders """ dpp = 6 * math.pi / 1440 # 6 in. wheels, 1440 ppr on e4ts def __init__(self, l_a=0, l_b=1, r_a=2, r_b=3): self.left = Encoder(l_a, l_b, reverseDirection=True) self.right = Encoder(r_a, r_b) self.left.setDistancePerPulse(self.dpp) self.right.setDistancePerPulse(self.dpp) def getDist(self): return (self.left.GetDistance + self.right.getDistance) / 2 def get_R_Dist(self): return self.right.GetDistance def get_L_Dist(self): return self.left.gGetDistance() def reset(self): self.right.reset() self.left.reset()
def __init__(self, robot): super().__init__("Drive") self.robot = robot self.peakCurrentLimit = 25 self.PeaKDuration = 50 self.continuousLimit = 15 self.map = robot.RobotMap # drive motor, sensors, and pistons motor = {} pistons = {} # create all drive motors, Sensors, and pistons for name in self.map.motorMap.motors: motor[name] = robot.Creator.createMotor(self.map.motorMap.motors[name]) for name in self.map.PneumaticMap.pistons: if name == 'Shifter': pistons[name] = robot.Creator.createPistons(self.map.PneumaticMap.pistons[name]) self.REnc = Encoder(0, 1, True, Encoder.EncodingType.k4X) self.LEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X) self.Gyro = ADXRS450_Gyro() # make motors, Sensors, and pistons local to subsystem self.Dmotor = motor self.Dpiston = pistons for name in self.Dmotor: self.Dmotor[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted']) # drive current limit self.Dmotor[name].configPeakCurrentLimit(self.peakCurrentLimit, 10) self.Dmotor[name].configPeakCurrentDuration(self.PeaKDuration, 10) self.Dmotor[name].configContinuousCurrentLimit(self.continuousLimit, 10) self.Dmotor[name].enableCurrentLimit(True) self.KtorqueMode = DoubleSolenoid.Value.kReverse self.KspeedMode = DoubleSolenoid.Value.kForward if wpilib.RobotBase.isSimulation(): self.kp = 0.001 self.ki = 0.00001 self.kd = 0.000000001 else: self.kp = 0.001 self.ki = 0.00001 self.kd = 0.000000001 self.DrivePID = PID(self.kp, self.ki, self.kd)
class Indexer(): def __init__(self, operator: OperatorControl): self.operator = operator self.motor = WPI_VictorSPX(7) self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.encoder.setIndexSource(2) self.limit = DigitalInput(4) self.dashboard = NetworkTables.getTable("SmartDashboard") self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1 self.targetValue = 10 self.realValue = 0 self.allowedError = 10 self.hole = 4 self.holeOffset = 36 - 15 self.maxspeed = .25 self.minspeed = .1 self.speedmult = 1/300 self.speed = .1 def update(self): self.realValue = self.encoder.get() offset = (self.targetValue - self.realValue) % self.totalValues - (self.totalValues / 2) self.speed = clamp(abs(offset) * self.speedmult, self.minspeed, self.maxspeed) self.dashboard.putString("Indexer", "{} offset".format(offset)) if (offset > self.allowedError): self.motor.set(ControlMode.PercentOutput, -self.speed) elif (offset < -self.allowedError): self.motor.set(ControlMode.PercentOutput, self.speed) else: self.motor.set(ControlMode.PercentOutput, 0) if (abs(offset) < 15): if (self.operator.getIndexUp()): self.hole = (self.hole + 1) % 5 elif (self.operator.getIndexDown()): self.hole = (self.hole + 4) % 5 self.setRotation(self.hole * 72 + self.holeOffset) def getRotation(self) -> float: return self.realValue / self.totalValues * 360 def setRotation(self, degrees): self.targetValue = clamp(degrees/360*self.totalValues, 0, self.totalValues)
def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag
def __init__(self, Robot): self._intake = Robot.intake self._drive = Robot.drive self._ramp = 0 self._rampSpeed = 6 self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX) self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX) self._LiftMotorLeft.setInverted(True) self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False, Encoder.EncodingType.k4X) self._liftEncoder.setDistancePerPulse(1) self._liftHallaffect = DigitalInput(HALL_DIO) self.zeroEncoder() self._liftPID = MiniPID(*LIFT_PID) self._liftPID.setOutputLimits(-0.45, 1) self.disableSpeedLimit = False
class Chassis(Subsystem): def __init__(self): super().__init__("Chassis") self.spark_L1 = Spark(robotmap.spark_L1) self.spark_L2 = Spark(robotmap.spark_L2) self.spark_R1 = Spark(robotmap.spark_R1) self.spark_R2 = Spark(robotmap.spark_R2) self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2) self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2) self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R) self.gyro = ADXRS450_Gyro(robotmap.gyro) self.encoder_L = Encoder(0, 1) self.encoder_R = Encoder(2, 3) self.dist_pulse_L = pi * 6 / 2048 self.dist_pulse_R = pi * 6 / 425 @classmethod def setDriveSpd(cls, spd_drive_new): robotmap.spd_chassis_drive = spd_drive_new @classmethod def setRotateSpd(cls, spd_rotate_new): robotmap.spd_chassis_rotate = spd_rotate_new def stop(self): self.drive.stopMotor() def curvatureDrive(self, spd_x, spd_z): self.drive.curvatureDrive(spd_x, spd_z, True) def joystickDrive(self): self.drive.curvatureDrive( -(oi.joystick.getRawAxis(1)) * robotmap.spd_chassis_drive, oi.joystick.getRawAxis(4) * robotmap.spd_chassis_rotate, True) def setupEncoder(self): self.encoder_L.setDistancePerPulse(self.dist_pulse_L) self.encoder_R.setDistancePerPulse(self.dist_pulse_R) self.encoder_L.reset() self.encoder_R.reset() def getGyroAngle(self): return self.gyro.getAngle() def resetGyro(self): self.gyro.reset() def gyroDrive(self, spd_temp, amt_turn=None): if spd_temp and amt_turn is not None: self.curvatureDrive(spd_temp, amt_turn) elif amt_turn is None: self.curvatureDrive(spd_temp, 0.0) else: raise ("GyroDrive() failed!")
def __init__(self): super().__init__() # Create the motor controllers and their respective speed controllers. self.leftMotors = SpeedControllerGroup( PWMSparkMax(constants.kLeftMotor1Port), PWMSparkMax(constants.kLeftMotor2Port), ) self.rightMotors = SpeedControllerGroup( PWMSparkMax(constants.kRightMotor1Port), PWMSparkMax(constants.kRightMotor2Port), ) # Create the differential drivetrain object, allowing for easy motor control. self.drive = DifferentialDrive(self.leftMotors, self.rightMotors) # Create the encoder objects. self.leftEncoder = Encoder( constants.kLeftEncoderPorts[0], constants.kLeftEncoderPorts[1], constants.kLeftEncoderReversed, ) self.rightEncoder = Encoder( constants.kRightEncoderPorts[0], constants.kRightEncoderPorts[1], constants.kRightEncoderReversed, ) # Configure the encoder so it knows how many encoder units are in one rotation. self.leftEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) self.rightEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) # Create the gyro, a sensor which can indicate the heading of the robot relative # to a customizable position. self.gyro = ADXRS450_Gyro() # Create the an object for our odometry, which will utilize sensor data to # keep a record of our position on the field. self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d()) # Reset the encoders upon the initilization of the robot. self.resetEncoders()
def __init__(self): super().__init__("Chassis") self.spark_L1 = Spark(robotmap.spark_L1) self.spark_L2 = Spark(robotmap.spark_L2) self.spark_R1 = Spark(robotmap.spark_R1) self.spark_R2 = Spark(robotmap.spark_R2) self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2) self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2) self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R) self.gyro = ADXRS450_Gyro(robotmap.gyro) self.encoder_L = Encoder(0, 1) self.encoder_R = Encoder(2, 3) self.dist_pulse_L = pi * 6 / 2048 self.dist_pulse_R = pi * 6 / 425
def __init__(self, operator: OperatorControl): self.operator = operator self.motor = WPI_VictorSPX(7) self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.encoder.setIndexSource(2) self.limit = DigitalInput(4) self.dashboard = NetworkTables.getTable("SmartDashboard") self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1 self.targetValue = 10 self.realValue = 0 self.allowedError = 10 self.hole = 4 self.holeOffset = 36 - 15 self.maxspeed = .25 self.minspeed = .1 self.speedmult = 1/300 self.speed = .1
def __init__(self, robot): Subsystem.__init__(self, 'Flywheel') self.CurPos = 0 self.PasPos = 0 self.robot = robot self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X) self.map = self.robot.botMap motor = {} piston = {} for name in self.robot.botMap.motorMap.motors: motor[name] = self.robot.Creator.createMotor( self.robot.botMap.motorMap.motors[name]) for name in self.robot.botMap.PneumaticMap.pistons: piston[name] = self.robot.Creator.createPistons( self.robot.botMap.PneumaticMap.pistons[name]) self.fmotor = motor self.fpiston = piston for name in self.fmotor: self.fmotor[name].setInverted( self.robot.botMap.motorMap.motors[name]['inverted']) self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast) if self.map.motorMap.motors[name]['CurLimit'] is True: self.fmotor[name].configStatorCurrentLimit( self.robot.Creator.createCurrentConfig( self.robot.botMap.currentConfig['Fly']), 40) self.kP = 0.008 self.kI = 0.00002 self.kD = 0.00018 self.kF = 0.0 # tune me when testing self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF) self.flywheelPID.MaxIn(680) self.flywheelPID.MaxOut(1) self.flywheelPID.limitVal(0.95) # Limit PID output power to 50% self.feetToRPS = 51.111
def __init__(self, channel_a, channel_b, pulse_dist=1.0, reverse=False, modnum=1, cpr=128, enctype=CounterBase.EncodingType.k4X): """ Initializes the encoder with two channels, distance per pulse (usu. feet, default 1), no reversing, on module number 1, 128 CPR, and with 4x counting. """ super().__init__() self.e = WEncoder(channel_a, channel_b, reverse, enctype) self.cpr = cpr self.e.setDistancePerPulse(pulse_dist)
def __init__(self, channel_a, channel_b, pulse_dist=1.0, reverse=False, modnum=1, cpr=128, enctype=CounterBase.k4x): """ Initializes the encoder with two channels, distance per pulse (default 1), no reversing, on module number 1, 128 CPR, and with 4x counting. """ super().__init__() self.e = WEncoder(modnum, channel_a, channel_b, reverse, enctype) self.cpr = cpr self.pulse_dist = pulse_dist
class Robot(TimedRobot): def robotInit(self): # Right Motors self.RightMotor1 = WPI_TalonFX(1) self.RightSensor1 = SensorCollection(self.RightMotor1) self.RightMotor2 = WPI_TalonFX(2) self.RightMotor3 = WPI_TalonFX(3) self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1, self.RightMotor2, self.RightMotor3) # Left Motors self.LeftMotor1 = WPI_TalonFX(4) self.LeftMotor2 = WPI_TalonFX(5) self.LeftMotor3 = WPI_TalonFX(6) self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1, self.LeftMotor2, self.LeftMotor3) # self.drive = DifferentialDrive(self.leftDriveMotors, # self.rightDriveMotors) self.testEncoder = Encoder(1, 2, 3, Encoder.EncodingType.k4X) self.dashboard = NetworkTables.getTable("SmartDashboard") def disabledInit(self): pass def autonomousInit(self): pass def teleopInit(self): pass def testInit(self): pass def robotPeriodic(self): self.dashboard.putNumber("Encoder", self.RightSensor1.getQuadratureVelocity()/2048*60) def disabledPeriodic(self): pass def autonomousPeriodic(self): speed = self.testEncoder.get() / 1028 self.leftDriveMotors.set(speed) self.rightDriveMotors.set(speed) def teleopPeriodic(self): pass def testPeriodic(self): pass
class Encoder(Sensor): """ Sensor wrapper for a quadrature encoder. Has double attributes distance, rate (distance/second); boolean attributes stopped and direction. """ distance = rate = 0 stopped = direction = True def __init__(self, channel_a, channel_b, pulse_dist=1.0, reverse=False, modnum=1, cpr=128, enctype=CounterBase.EncodingType.k4X): """ Initializes the encoder with two channels, distance per pulse (usu. feet, default 1), no reversing, on module number 1, 128 CPR, and with 4x counting. """ super().__init__() self.e = WEncoder(channel_a, channel_b, reverse, enctype) self.cpr = cpr self.e.setDistancePerPulse(pulse_dist) def poll(self): self.distance = self.e.getDistance() self.rate = self.e.getRate() self.stopped = self.e.getStopped() self.direction = self.e.getDirection()
def robotInit(self): # Right Motors self.RightMotor1 = WPI_TalonFX(1) self.RightMotor2 = WPI_TalonFX(2) self.RightMotor3 = WPI_TalonFX(3) self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1, self.RightMotor2, self.RightMotor3) # Left Motors self.LeftMotor1 = WPI_TalonFX(4) self.LeftMotor2 = WPI_TalonFX(5) self.LeftMotor3 = WPI_TalonFX(6) self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1, self.LeftMotor2, self.LeftMotor3) self.drive = DifferentialDrive(self.leftDriveMotors, self.rightDriveMotors) self.testEncoder = Encoder(1, 2, 3) self.dashboard = NetworkTables.getTable("SmartDashboard")
def __init__(self, robot): Subsystem.__init__(self, 'Drive') self.robot = robot motors = {} pistons = {} self.map = self.robot.botMap self.rEnc = Encoder(0, 1, False, Encoder.EncodingType.k4X) self.lEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X) self.Gyro = ADXRS450_Gyro() for name in self.map.motorMap.motors: motors[name] = robot.Creator.createMotor( self.map.motorMap.motors[name]) for name in self.robot.botMap.PneumaticMap.pistons: if name == 'Shifter': pistons[name] = self.robot.Creator.createPistons( self.robot.botMap.PneumaticMap.pistons[name]) self.dMotors = motors self.dPistons = pistons for name in self.dMotors: self.dMotors[name].setInverted( self.robot.botMap.motorMap.motors[name]['inverted']) self.dMotors[name].setNeutralMode(ctre.NeutralMode.Coast) if self.map.motorMap.motors[name]['CurLimit'] is True: self.dMotors[name].configStatorCurrentLimit( self.robot.Creator.createCurrentConfig( self.robot.botMap.currentConfig['Drive']), 40) self.kP = 0.0 self.kI = 0.0 self.kD = 0.0 self.DrivePID = PID(self.kP, self.kI, self.kD)
class Arm(Subsystem): """Raise and lower the robot's arm.""" def __init__(self): """Assign ports and save them for use in the move and stop methods.""" super().__init__() self.arm = ctre.WPI_TalonSRX(11) self.armencoder = Encoder(0, 1) self.armencoder.setDistancePerPulse(0.07) def move(self, value): """Move the arm according to the left and right Xbox controller triggers.""" if self.armencoder.getDistance() > -120: self.arm.set(value) else: self.arm.set(0.05) #print("Arm angle is " + "%3f" % self.armencoder.getDistance()) def stop(self): """Stop the arm.""" self.arm.set(0.0)
class Robot(TimedRobot): def robotInit(self): # Right Motors self.RightMotor1 = WPI_TalonFX(1) self.RightMotor2 = WPI_TalonFX(2) self.RightMotor3 = WPI_TalonFX(3) self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1, self.RightMotor2, self.RightMotor3) # Left Motors self.LeftMotor1 = WPI_TalonFX(4) self.LeftMotor2 = WPI_TalonFX(5) self.LeftMotor3 = WPI_TalonFX(6) self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1, self.LeftMotor2, self.LeftMotor3) self.drive = DifferentialDrive(self.leftDriveMotors, self.rightDriveMotors) self.testEncoder = Encoder(1, 2, 3) self.dashboard = NetworkTables.getTable("SmartDashboard") def disabledInit(self): pass def autonomousInit(self): pass def teleopInit(self): pass def testInit(self): pass def robotPeriodic(self): self.dashboard.putNumber("Encoder", self.testEncoder.getRate()) def disabledPeriodic(self): pass def autonomousPeriodic(self): self.drive.arcadeDrive(.5, 0) def teleopPeriodic(self): pass def testPeriodic(self): pass
def __init__(self): super().__init__() self._motor = SyncGroup(Talon, constants.motor_elevator) self._position_encoder = Encoder(*constants.encoder_elevator) self._photosensor = DigitalInput(constants.photosensor) self._stabilizer_piston = Solenoid(constants.solenoid_dropper) self._position_encoder.setDistancePerPulse( (PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION) # Trajectory controlling stuff self._follower = TrajectoryFollower() self.assure_tote = CircularBuffer(5) self.auto_stacking = True # Do the dew self._tote_count = 0 # Keep track of totes! self._has_bin = False # Do we have a bin on top? self._new_stack = True # starting a new stack? self._tote_first = False # Override bin first to grab totes before anything else self._should_drop = False # Are we currently trying to get a bin ? self._close_stabilizer = True # Opens the stabilizer manually self.force_stack = False # manually actuates the elevator down and up self._follower.set_goal(Setpoints.BIN) # Base state self._follower_thread = Thread(target=self.update_follower) self._follower_thread.start() self._auton = False quickdebug.add_tunables(Setpoints, ["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"]) quickdebug.add_printables( self, [('position', self._position_encoder.getDistance), ('photosensor', self._photosensor.get), "has_bin", "tote_count", "tote_first", "at_goal", "has_game_piece", "auto_stacking"])
class Turret(Subsystem): def __init__(self, robot): Subsystem.__init__(self, 'Turret') self.robot = robot self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X) motors = {} self.map = self.robot.botMap self.tmotors = motors for name in self.map.motorMap.motors: motors[name] = robot.Creator.createMotor( self.map.motorMap.motors[name]) for name in self.tmotors: self.tmotors[name].setInverted( self.map.motorMap.motors[name]['inverted']) self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast) self.kP = 0.05 self.kI = 0.000 self.kD = 0.002 self.turretPID = PID(self.kP, self.kI, self.kD) self.turretPID.limitVal(0.3) self.setPow = 0 def set(self, pw): self.tmotors['turret'].set(ctre.ControlMode.PercentOutput, pw) def setPower(self, pow): self.setPow = pow def getEnc(self): return self.tEncoder.get() def periodic(self): if self.robot.Limelight.isExisting(): self.set(0) # self.turretPID.outVal(self.robot.Limelight.getX())) else: self.set(self.setPow)
class Drive: def __init__(self, Robot): #self.navx = Robot.navx self.speedLimit = 0.999 self._leftRamp = 0.0 self._rightRamp = 0.0 self._rampSpeed = 6.0 self._kOonBalanceAngleThresholdDegrees = 5.0 self._autoBalance = True self._quickstop_accumulator = 0.0 self._old_wheel = 0.0 self._driveReversed = True self._drivePool = DataPool("Drive") # setup drive train motors self.rightDrive = SyncGroup(RIGHT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.leftDrive = SyncGroup(LEFT_DRIVE_MOTOR_IDS, WPI_VictorSPX) self.rightDrive.setInverted(True) # setup drive train gear shifter self.shifter = Solenoid(DRIVE_SHIFTER_PORT) self.shifter.set(False) # setup drive train encoders self.leftEncoder = Encoder(LEFT_DRIVE_ENCODER_A, LEFT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.rightEncoder = Encoder(RIGHT_DRIVE_ENCODER_A, RIGHT_DRIVE_ENCODER_B, False, Encoder.EncodingType.k4X) self.leftEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) self.leftEncoder.setReverseDirection(True) self.rightEncoder.setReverseDirection(False) self.rightEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK) def cheesyDrive(self, wheel, throttle, quickturn, altQuickturn, shift): throttle = Util.deadband(throttle) wheel = Util.deadband(wheel) if self._driveReversed: wheel *= -1 neg_inertia = wheel - self._old_wheel self._old_wheel = wheel wheel = Util.sinScale(wheel, 0.9, 1, 0.9) if wheel * neg_inertia > 0: neg_inertia_scalar = 2.5 else: if abs(wheel) > .65: neg_inertia_scalar = 6 else: neg_inertia_scalar = 4 neg_inertia_accumulator = neg_inertia * neg_inertia_scalar wheel += neg_inertia_accumulator if altQuickturn: if abs(throttle) < 0.2: alpha = .1 self._quickstop_accumulator = ( 1 - alpha) * self._quickstop_accumulator + alpha * self.limit( wheel, 1.0) * 5 over_power = -wheel * .75 angular_power = -wheel * 1 elif quickturn: if abs(throttle) < 0.2: alpha = .1 self._quickstop_accumulator = ( 1 - alpha) * self._quickstop_accumulator + alpha * self.limit( wheel, 1.0) * 5 over_power = 1 angular_power = -wheel * 1 else: over_power = 0 sensitivity = .9 angular_power = throttle * wheel * sensitivity - self._quickstop_accumulator self._quickstop_accumulator = Util.wrap_accumulator( self._quickstop_accumulator) if shift: if not self.shifter.get(): self.shifter.set(True) else: if self.shifter.get(): self.shifter.set(False) right_pwm = left_pwm = throttle left_pwm += angular_power right_pwm -= angular_power if left_pwm > 1: right_pwm -= over_power * (left_pwm - 1) left_pwm = 1 elif right_pwm > 1: left_pwm -= over_power * (right_pwm - 1) right_pwm = 1 elif left_pwm < -1: right_pwm += over_power * (-1 - left_pwm) left_pwm = -1 elif right_pwm < -1: left_pwm += over_power * (-1 - right_pwm) right_pwm = -1 if self._driveReversed: left_pwm *= -1 right_pwm *= -1 if self.shifter.get(): # if low gear #leftDrive.set(left_pwm) #rightDrive.set(right_pwm) self.moveRamped(left_pwm, right_pwm) else: self.moveRamped(left_pwm, right_pwm) def setGear(self, gear): if self.shifter.get() != gear: self.shifter.set(gear) def tankDrive(self, left, right): scaledBalance = self.autoBalance() left = self.limit(left + scaledBalance, self.speedLimit) right = self.limit(right + scaledBalance, self.speedLimit) self.leftDrive.set(left * LEFT_DRIFT_OFFSET) self.rightDrive.set(right * RIGHT_DRIFT_OFFSET) def limit(self, wheel, d): return Util.limit(wheel, d) def moveRamped(self, desiredLeft, desiredRight): self._leftRamp += (desiredLeft - self._leftRamp) / self._rampSpeed self._rightRamp += (desiredRight - self._rightRamp) / self._rampSpeed self.tankDrive(self._leftRamp, self._rightRamp) def autoShift(self, b): if self.shifter.get() != b: self.shifter.set(b) def periodic(self): self._drivePool.logDouble("gyro_angle", self.getRotation()) self._drivePool.logDouble("left_enc", self.rightEncoder.getDistance()) self._drivePool.logDouble("right_enc", self.leftEncoder.getDistance()) def setDrivetrainReversed(self, rev): self.driveReversed = rev def driveReversed(self): return self.driveReversed def getRotation(self): return self.navx.getAngle() def getLeftDistance(self): return self.leftEncoder.getDistance() * 2.54 * ROBOT_INVERTED def getRightDistance(self): return self.rightEncoder.getDistance() * 2.54 * ROBOT_INVERTED def resetDistance(self): self.leftEncoder.reset() self.rightEncoder.reset() def autoBalance(self): '''if self._autoBalance: pitchAngleDegrees = self.navx.getPitch() scaledPower = 1 + (0 - pitchAngleDegrees - self._kOonBalanceAngleThresholdDegrees) / self._kOonBalanceAngleThresholdDegrees if scaledPower > 2: scaledPower = 2 #return scaledPower;''' return 0 def setSpeedLimit(self, speedLimit): self.speedLimit = self.limit(speedLimit, DRIVE_SPEED_LIMIT)
class Lift: def __init__(self, Robot): self._intake = Robot.intake self._drive = Robot.drive self._ramp = 0 self._rampSpeed = 6 self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX) self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX) self._LiftMotorLeft.setInverted(True) self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False, Encoder.EncodingType.k4X) self._liftEncoder.setDistancePerPulse(1) self._liftHallaffect = DigitalInput(HALL_DIO) self.zeroEncoder() self._liftPID = MiniPID(*LIFT_PID) self._liftPID.setOutputLimits(-0.45, 1) self.disableSpeedLimit = False def setSpeed(self, speed): self._LiftMotorLeft.set(speed) self._LiftMotorRight.set(speed) def rampSpeed(self, speed): self._ramp += (speed - self._ramp) / self._rampSpeed if False and speed > 0: #is max limit hit self._ramp = 0 self._LiftMotorLeft.set(self._ramp) self._LiftMotorRight.set(self._ramp) def setLoc(self, target): target = abs(target) if target > 1: target = 1 elif target <= 0.1: target = 0 SmartDashboard.putNumber("loc dfliusafusd", target) self._liftPID.setSetpoint(target) def periodic(self): if self.isBottom(): #zero switch is active zero encoder self.zeroEncoder() else: if self._intake.getSpeed() >= 0: self._intake.rampSpeed(0.3) scaledLift = self.getEncoderVal() / LIFT_HEIGHT speed = self._liftPID.getOutput(scaledLift) if not self.disableSpeedLimit: speedLimit = pow(0.25, scaledLift) self._drive.setSpeedLimit(speedLimit) else: self._drive.setSpeedLimit(1) self.rampSpeed(speed) def getEncoderVal(self): return abs(self._liftEncoder.getDistance()) def zeroEncoder(self): self._liftEncoder.reset() def isBottom(self): return not self._liftHallaffect.get()
class DriveBase: left_motor_one = CANTalon left_motor_two = CANTalon right_motor_one = CANTalon right_motor_two = CANTalon left_encoder = Encoder right_encoder = Encoder navx = AHRS def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag def drive(self, left_power, right_power): self.base.tankDrive(left_power, right_power) def execute(self): if int(self.base.getNumMotors()) < 3: self.base.drive(0, 0) def get_drive_distance(self): return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance()) def rest_base(self): self.left_encoder.reset() self.right_encoder.reset() def pid_drive(self, speed, distance, to_angle=None): self.navx.isCalibrating() self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F) self.pid_d_controller.setOutputRange(speed - distance, speed + distance) if to_angle is None: set_angle = self.navx.getYaw() else: set_angle = to_angle self.pid_d_controller.setSetpoint(float(set_angle)) self.drive(speed + 0.03, speed) self.pid_d_controller.enable() self.current_flag = self.type_flag[0] self.auto_pid_out.current_flag = self.current_flag def pid_turn(self, angle): self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.turn_P, sensor_map.turn_I, sensor_map.turn_D, sensor_map.turn_F) self.pid_d_controller.setOutputRange(sensor_map.output_range_min, sensor_map.output_range_max) self.pid_d_controller.setSetpoint(float(angle)) self.pid_d_controller.enable() self.current_flag = self.type_flag[1] self.auto_pid_out.current_flag = self.current_flag def stop_pid(self): self.pid_d_controller.disable() self.pid_d_controller.reset()