def __init__(self): self.left_fly = CANTalon(motor_map.left_fly_motor) self.right_fly = CANTalon(motor_map.right_fly_motor) self.intake_main = CANTalon(motor_map.intake_main_motor) self.intake_mecanum = Talon(motor_map.intake_mecanum_motor) self.ball_limit = DigitalInput(sensor_map.ball_limit) self.intake_mecanum.setInverted(True) self.left_fly.reverseOutput(True) self.left_fly.enableBrakeMode(False) self.right_fly.enableBrakeMode(False) self.left_fly.setControlMode(CANTalon.ControlMode.Speed) self.right_fly.setControlMode(CANTalon.ControlMode.Speed) self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev) self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder=True, reverseDrive=False): # Initialise private motor controllers self._drive = CANTalon(driveCanTalonId) self.reverse_drive = reverseDrive self._steer = CANTalon(steerCanTalonId) self.absoluteEncoder = absoluteEncoder # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absoluteEncoder: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice( CANTalon.FeedbackDevice.AnalogEncoder) else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(RobotMap.steering_p, 0.0, 0.0) self._steer.setPosition(0.0) # Private members to store the setpoints self._speed = 0.0 self._direction = 0.0 self._opposite_direction = math.pi
def __init__(self, drive, steer, absolute=True, reverse_drive=False, reverse_steer=False, zero_reading=0, drive_encoder=False, reverse_drive_encoder=False): # Initialise private motor controllers self._drive = CANTalon(drive) self.reverse_drive = reverse_drive self._steer = CANTalon(steer) self.drive_encoder = drive_encoder self._distance_offset = 0 # Offset the drive distance counts # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absolute: self.counts_per_radian = 1024.0 / (2.0 * math.pi) self._steer.setFeedbackDevice( CANTalon.FeedbackDevice.AnalogEncoder) self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.reverseSensor(reverse_steer) self._steer.reverseOutput(not reverse_steer) # Read the current encoder position self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs self._offset = zero_reading - 256.0 if reverse_steer: self._offset = -self._offset else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 / (2.0 * math.pi)) self._offset = self.counts_per_radian * 2.0 * math.pi / 4.0 self._steer.setPosition(0.0) if self.drive_encoder: self.drive_counts_per_rev = 80 * 6.67 self.drive_counts_per_metre = (self.drive_counts_per_rev / (math.pi * 0.1016)) self.drive_max_speed = 570 self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.changeDriveControlMode(CANTalon.ControlMode.Speed) self._drive.reverseSensor(reverse_drive_encoder) else: self.drive_counts_per_rev = 0.0 self.drive_max_speed = 1.0 self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus) self._drive.setVoltageRampRate(150.0)
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): super().__init__() self.robot = robot if RobotMap.motor_controllers == "talonsrx": self._motors = [ CANTalon(RobotMap.motor_a_talon_id), CANTalon(RobotMap.motor_b_talon_id), CANTalon(RobotMap.motor_c_talon_id), CANTalon(RobotMap.motor_d_talon_id) ] for motor in self._motors: motor.changeControlMode(RobotMap.drive_motor_mode) elif RobotMap.motor_controllers == "victor": self._motors = [ Victor(RobotMap.motor_a_pwm_id), Victor(RobotMap.motor_b_pwm_id), Victor(RobotMap.motor_c_pwm_id), Victor(RobotMap.motor_d_pwm_id) ]
def __init__(self, channel): self.t = CANTalon(channel) self.channel = channel
from grt.mechanism.drivecontroller import ArcadeDriveController from grt.sensors.encoder import Encoder from grt.mechanism.mechcontroller import MechController from grt.sensors.navx import NavX from grt.macro.straight_macro import StraightMacro from grt.mechanism.pickup import Pickup from grt.mechanism.manual_shooter import ManualShooter from queue import Queue #Compressor initialization c = Compressor() c.start() #Manual pickup Talons and Objects pickup_achange_motor1 = CANTalon(45) pickup_achange_motor2 = CANTalon(46) pickup_achange_motor1.changeControlMode(CANTalon.ControlMode.Follower) pickup_achange_motor1.set(47) pickup_achange_motor1.reverseOutput(True) pickup_roller_motor = CANTalon(8) pickup = Pickup(pickup_achange_motor1, pickup_achange_motor2, pickup_roller_motor) #Manual shooter Talons and Objects flywheel_motor = CANTalon(44) shooter_act = Solenoid(1) turntable_motor = CANTalon(12)
from grt.sensors.attack_joystick import Attack3Joystick from grt.sensors.xbox_joystick import XboxJoystick #from grt.sensors.gyro import Gyro from grt.core import SensorPoller from grt.mechanism.drivetrain import DriveTrain from grt.mechanism.drivecontroller import ArcadeDriveController from grt.mechanism.motorset import Motorset from grt.sensors.ticker import Ticker from grt.sensors.encoder import Encoder from grt.sensors.talon import Talon from grt.mechanism.mechcontroller import MechController #DT Talons and Objects dt_right = CANTalon(1) dt_r2 = CANTalon(2) dt_r3 = CANTalon(3) dt_r4 = CANTalon(4) dt_left = CANTalon(7) dt_l2 = CANTalon(8) dt_l3 = CANTalon(9) dt_l4 = CANTalon(10) dt_r2.changeControlMode(CANTalon.ControlMode.Follower) dt_r3.changeControlMode(CANTalon.ControlMode.Follower) dt_r4.changeControlMode(CANTalon.ControlMode.Follower) dt_l2.changeControlMode(CANTalon.ControlMode.Follower) dt_l3.changeControlMode(CANTalon.ControlMode.Follower) dt_l4.changeControlMode(CANTalon.ControlMode.Follower)
def __init__(self, robot): self.robot = robot self.motor_a = CANTalon(RobotMap.drive_motors_motor_a_id) self.motor_b = CANTalon(RobotMap.drive_motors_motor_b_id)
def __init__(self): self.chopper_motor = CANTalon(motor_map.chopper_motor) self.chopper_motor.setSafetyEnabled(False) self.chopper_motor.enableLimitSwitch(True, True)