def test_profiler1(): """ forward velocity, trapezoid, no overshoot """ DT = 0.02 profiler = TrapezoidalProfile(cruise_v=10, a=20, target_pos=50, tolerance=.5, current_target_v=0) t = 0 pos = 0 if log_trajectory: logger = DataLogger("test_profiler1.csv") logger.log_while_disabled = True logger.do_print = True logger.add('t', lambda: t) logger.add('pos', lambda: pos) logger.add('v', lambda: profiler.current_target_v) logger.add('a', lambda: profiler.current_a) while not profiler.isFinished(pos): if log_trajectory: logger.log() profiler.calculate_new_velocity(pos, DT) pos += profiler.current_target_v * DT t += DT if t > 10: if log_trajectory: logger.close() assert False, "sim loop timed out" if t < 0.501: assert profiler.current_a == pytest.approx(20, 0.01), "t = %f" % (t, ) if 0.501 < t < 5: assert profiler.current_target_v == pytest.approx( 10., 0.01), "t = %f" % (t, ) assert profiler.current_a == 0, "t = %f" % (t, ) if 5 < t < 5.5 - 0.001: assert profiler.current_a == pytest.approx(-20., 0.01), "t = %f" % (t, ) if t == pytest.approx(5.50, 0.001): assert profiler.current_a == pytest.approx(0., 0.01), "t = %f" % (t, ) assert profiler.current_target_v == pytest.approx( 0., 0.01), "t = %f" % (t, ) if log_trajectory: logger.log() logger.close() assert t == pytest.approx(5.52, 0.01) assert profiler.current_a == 0
def test_StateSpaceDriveCommand(Notifier): global log_trajectory left_drive = DriveSide( Ks=1.293985, Kv=0.014172 * 63. / 0.3048, Ka=0.005938 * 63. / 0.3048) right_drive = DriveSide( Ks=1.320812, Kv=0.013736 * 63. / 0.3048, Ka=0.005938 * 63. / 0.3048) robot = Rockslide() robot.robotInit() robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock() robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock() robot.drivetrain.getVoltage = MagicMock(return_value=10) command = StateSpaceDriveCommand("straight3m.tra") command.initialize() dt = robot.getPeriod() t = 0 if log_trajectory: logger = DataLogger("test_StateSpaceDriveCommand.csv") logger.log_while_disabled = True logger.do_print = False logger.add('t', lambda: t) logger.add('pos_l_m', lambda: left_drive.pos_m) logger.add('pos_r_m', lambda: right_drive.pos_m) logger.add('m_pos_l_m', lambda: command.y[0,0]) logger.add('m_pos_r_m', lambda: command.y[1,0]) logger.add('vel_l_mps', lambda: left_drive.v_mps) logger.add('vel_r_mps', lambda: right_drive.v_mps) logger.add('target_pos_l_m', lambda: command.r[0,0]) logger.add('target_pos_r_m', lambda: command.r[2,0]) logger.add('target_vel_l_mps', lambda: command.r[1,0]) logger.add('target_vel_r_mps', lambda: command.r[3,0]) logger.add('voltage', lambda: command.drivetrain.getVoltage()) logger.add('vpl', lambda: command.drivetrain.motor_lb.get()) logger.add('vpr', lambda: command.drivetrain.motor_rb.get()) while not command.isFinished(): logger.log() getLeftEncoder.return_value = left_drive.pos_m * 630 / 0.3048 getRightEncoder.return_value = right_drive.pos_m * 630 / 0.3048 command.execute() V = command.drivetrain.getVoltage() vpl = command.drivetrain.motor_lb.get() vpr = command.drivetrain.motor_rb.get() left_drive.update(V * vpl, dt) right_drive.update(V * vpr, dt) t += dt assert t < 10 command.end()
def test_profiler4(): """ reverse velocity, triangle, no overshoot """ DT = 0.02 profiler = TrapezoidalProfile(cruise_v=10, a=20, target_pos=-4, tolerance=.5, current_target_v=0) t = 0 pos = 0 if log_trajectory: logger = DataLogger("test_profiler4.csv") logger.log_while_disabled = True logger.add('t', lambda: t) logger.add('pos', lambda: pos) logger.add('v', lambda: profiler.current_target_v) logger.add('a', lambda: profiler.current_a) while not profiler.isFinished(pos): if log_trajectory: logger.log() profiler.calculate_new_velocity(pos, DT) pos += profiler.current_target_v * DT t += DT if t > 10: if log_trajectory: logger.close() assert False, "sim loop timed out" if t < 0.4599: assert profiler.current_a == pytest.approx(-20, 0.01), "t = %f" % (t, ) if t == pytest.approx(0.46, 0.01): assert profiler.current_target_v == pytest.approx(-9.2, 0.01) assert profiler.current_a == pytest.approx(-20, 0.01), "t = %f" % (t, ) if 0.4601 < t < 0.92: assert profiler.current_a == pytest.approx(20, 0.01), "t = %f" % (t, ) if log_trajectory: logger.log() logger.close() assert t == pytest.approx(0.94, 0.01) assert profiler.current_a == 0
def test_ProfiledForward2(Notifier, sim_hooks): global log_trajectory robot = Rockslide() robot.robotInit() DT = robot.getPeriod() robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock() robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock() getLeftEncoder.return_value = 0 getRightEncoder.return_value = 0 command = ProfiledForward(10) command.initialize() t = 0 pos_ft = 0 if log_trajectory: logger = DataLogger("test_profiled_forward2.csv") logger.log_while_disabled = True logger.do_print = True logger.add('t', lambda: t) logger.add('pos', lambda: pos_ft) logger.add('target_pos', lambda: command.dist_ft) logger.add('v', lambda: command.profiler_l.current_target_v) logger.add('max_v', lambda: command.max_v_encps) logger.add('a', lambda: command.profiler_l.current_a) logger.add('max_a', lambda: command.max_acceleration) logger.add('voltage', lambda: command.drivetrain.getVoltage()) logger.add('vpl', lambda: command.drivetrain.motor_lb.get()) logger.add('adist', lambda: command.profiler_l.adist) logger.add('err', lambda: command.profiler_l.err) while t < 10: if log_trajectory: logger.log() getLeftEncoder.return_value = pos_ft * command.drivetrain.ratio getRightEncoder.return_value = -pos_ft * command.drivetrain.ratio command.execute() v = command.profiler_l.current_target_v pos_ft += v * DT t += DT sim_hooks.time = t if command.isFinished(): break command.end() if log_trajectory: logger.log() logger.close()
class StateSpaceDriveCommand(_CsvTrajectoryCommand, StateSpaceDriveController): def __init__(self, fnom): _CsvTrajectoryCommand.__init__(self, fnom) StateSpaceDriveController.__init__(self, Command.getRobot().drivetrain) self.u_min = -8 self.u_max = 8 def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.i = 0 self.logger = DataLogger("ss_trajectory.csv") self.drivetrain.init_logger(self.logger) def execute(self): (dt_s, xl_m, xr_m, vl_mps, vr_mps, al_mps2, ar_mps2, heading_rad) = self.get_trajectory_point_m(self.i) self.update(xl_m, xr_m, vl_mps, vr_mps) self.logger.log() self.i += 1 def end(self): self.drivetrain.off() self.logger.flush()
class ElevatorTest2(Command): def __init__(self): super().__init__("laksdjfkl") self.elevator = self.getRobot().elevator self.requires(self.elevator) self.timer = wpilib.Timer() self.done = False self.state = 1 self.voltage = 0.75 def initialize(self): self.logger = DataLogger('elevator-majig.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add( "enc_pos1", lambda: self.elevator.motor.getSelectedSensorVelocity(0)) self.logger.add("current_motor1", lambda: self.elevator.motor.getOutputCurrent()) self.logger.add("current_follow1", lambda: self.elevator.other_motor.getOutputCurrent()) self.logger.add("voltage_motor1", lambda: self.elevator.motor.getMotorOutputVoltage()) self.logger.add( "voltage_follow1", lambda: self.elevator.other_motor.getMotorOutputVoltage()) self.logger.add("bvoltage_motor1", lambda: self.elevator.motor.getBusVoltage()) self.logger.add("bvoltage_follow1", lambda: self.elevator.other_motor.getBusVoltage()) self.logger.add( "enc_pos2", lambda: self.elevator.other_right_motor. getSelectedSensorVelocity(0)) self.logger.add("current_motor2", lambda: self.elevator.right_motor.getOutputCurrent()) self.logger.add( "current_follow2", lambda: self.elevator.other_right_motor.getOutputCurrent()) self.logger.add( "voltage_motor2", lambda: self.elevator.right_motor.getMotorOutputVoltage()) self.logger.add( "voltage_follow2", lambda: self.elevator.other_right_motor.getMotorOutputVoltage()) self.logger.add("bvoltage_motor2", lambda: self.elevator.right_motor.getBusVoltage()) self.logger.add( "bvoltage_follow2", lambda: self.elevator.other_right_motor.getBusVoltage()) self.timer.start() def execute(self): e = self.elevator.getEncoderPosition() if self.state == 1: self.elevator.ascend(self.voltage) if e > 28000: self.state = 2 self.elevator.descend(self.voltage) if self.state == 2: self.elevator.descend(self.voltage) if e < 1000 and self.state == 2: self.state = 1 self.voltage += 0.25 elif self.voltage > 1: self.voltage = 0 self.done = True self.logger.log() def isFinished(self): return self.done def end(self): self.elevator.hover() self.logger.close()
class Drivetrain(Subsystem): '''''' #: encoder/ft ratio ratio = 886.27 def __init__(self): super().__init__('Drivetrain') #The set motor controllers for this years robot and how motors are coded self.motor_rb = ctre.WPI_TalonSRX(1) self.motor_rf = ctre.WPI_VictorSPX(17) self.motor_lb = ctre.WPI_TalonSRX(13) self.motor_lf = ctre.WPI_VictorSPX(12) self.motor_rf.follow(self.motor_rb) self.motor_lf.follow(self.motor_lb) self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf] self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb) self.navx = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(16) self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0) self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.navx_table = networktables.NetworkTables.getTable('/Sensor/Navx') self.leftEncoder_table = networktables.NetworkTables.getTable("/Encoder/Left") self.rightEncoder_table = networktables.NetworkTables.getTable("/Encoder/Right") self.leftError = networktables.NetworkTables.getTable("/TalonL/Error") self.rightError = networktables.NetworkTables.getTable("/TalonR/Error") self.motor_lb.setSensorPhase(True) self.motor_rb.setSensorPhase(True) self.timer = wpilib.Timer() self.timer.start() self.mode = "" self.computed_velocity = 0 self.logger = None def dumb_turn(self): self.drive.arcadeDrive(0, 0.4, False) def execute_turn(self, angle): position = angle / 60. self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position) self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position) self.drive.feed() def initDefaultCommand(self): self.setDefaultCommand(Drive()) def init_logger(self): self.logger = DataLogger('drivetrain.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("heading", lambda: self.navx.getAngle()) self.logger.add("enc_pos_l", lambda: self.motor_lb.getSelectedSensorPosition(0)) self.logger.add("enc_pos_r", lambda: self.motor_rb.getSelectedSensorPosition(0)) self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0)) self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0)) self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0)) self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0)) self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0)) self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0)) self.logger.add("computed_velocity", lambda: self.computed_velocity) self.logger.add("mode", lambda: self.mode) self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage()) self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent()) self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent()) self.logger.add("current_rf", lambda: self.pdp.getCurrent(0)) self.logger.add("current_rb", lambda: self.pdp.getCurrent(1)) self.logger.add("current_lf", lambda: self.pdp.getCurrent(15)) self.logger.add("current_lb", lambda: self.pdp.getCurrent(13)) def zeroEncoders(self): self.motor_rb.setSelectedSensorPosition(0, 0, 0) self.motor_lb.setSelectedSensorPosition(0, 0, 0) def zeroNavx(self): self.navx.reset() def initialize_driveTurnlike(self): #The PID values with the motors self.zeroEncoders() self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0) self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0) self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0) self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0) self.motor_rb.configNominalOutputForward(.1, 0) self.motor_lb.configNominalOutputForward(.1, 0) self.motor_rb.configNominalOutputReverse(-0.1, 0) self.motor_lb.configNominalOutputReverse(-0.1, 0) self.motor_rb.configPeakOutputForward(0.4, 0) self.motor_lb.configPeakOutputForward(0.4, 0) self.motor_rb.configPeakOutputReverse(-0.4, 0) self.motor_lb.configPeakOutputReverse(-0.4, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) # self.motor_rb.config_kF(0, 0, 0) # self.motor_lb.config_kF(0, 0, 0) self.motor_rb.config_kP(0, 0.18, 0) self.motor_lb.config_kP(0, 0.18, 0) self.motor_rb.config_kI(0, 0, 0) self.motor_lb.config_kI(0, 0, 0) self.motor_rb.config_kD(0, 8, 0) self.motor_lb.config_kD(0, 8, 0) def uninitialize_driveTurnlike(self): #The PID values with the motors self.motor_rb.configNominalOutputForward(0, 0) self.motor_lb.configNominalOutputForward(0, 0) self.motor_rb.configNominalOutputReverse(0, 0) self.motor_lb.configNominalOutputReverse(0, 0) self.motor_rb.configPeakOutputForward(1, 0) self.motor_lb.configPeakOutputForward(1, 0) self.motor_rb.configPeakOutputReverse(-1, 0) self.motor_lb.configPeakOutputReverse(-1, 0) def initilize_driveForward(self): self.mode = "Forward" #The PID values with the motors for drive forward self.zeroEncoders() self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0) self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0) self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0) self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0) self.motor_rb.configNominalOutputForward(0, 0) self.motor_lb.configNominalOutputForward(0, 0) self.motor_rb.configNominalOutputReverse(0, 0) self.motor_lb.configNominalOutputReverse(0, 0) self.motor_rb.configPeakOutputForward(1, 0) self.motor_lb.configPeakOutputForward(1, 0) self.motor_rb.configPeakOutputReverse(-1, 0) self.motor_lb.configPeakOutputReverse(-1, 0) self.motor_lb.setIntegralAccumulator(0, 0, 0) self.motor_rb.setIntegralAccumulator(0, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.motor_rb.config_kF(0, 0, 0) self.motor_lb.config_kF(0, 0, 0) self.motor_rb.config_kP(0, 0.18, 0) self.motor_lb.config_kP(0, 0.18, 0) self.motor_rb.config_kI(0, 0.001, 0) self.motor_lb.config_kI(0, 0.001, 0) self.motor_rb.config_kD(0, 2, 0) self.motor_lb.config_kD(0, 2, 0) def initialize_velocity_closedloop(self): self.motor_rb.configNominalOutputForward(0, 0) self.motor_lb.configNominalOutputForward(0, 0) self.motor_rb.configNominalOutputReverse(0, 0) self.motor_lb.configNominalOutputReverse(0, 0) self.motor_rb.configPeakOutputForward(1, 0) self.motor_lb.configPeakOutputForward(1, 0) self.motor_rb.configPeakOutputReverse(-1, 0) self.motor_lb.configPeakOutputReverse(-1, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) # tested for counterclockwise turn self.motor_rb.config_kF(0, 1.88, 0) self.motor_rb.config_kP(0, 4.18, 0) self.motor_rb.config_kI(0, 0.01, 0) self.motor_rb.config_kD(0, 450, 0) self.motor_lb.config_kF(0, 0.88, 0) self.motor_lb.config_kP(0, 3.18, 0) self.motor_lb.config_kI(0, 0.01, 0) self.motor_lb.config_kD(0, 450, 0) def getAngle(self): return self.navx.getAngle() def set_turn_velocity(self, v_degps): velocity_ratio = 1.6 self.computed_velocity = v_encp100ms = velocity_ratio * v_degps #self.computed_velocity = v_encp100ms = 32 self.motor_rb.set(ctre.ControlMode.Velocity, v_encp100ms) self.motor_lb.set(ctre.ControlMode.Velocity, v_encp100ms) def execute_driveforward(self, positionL, positionR): self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionR) self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionL) self.drive.feed() def isFinished_driveforward(self, target): sensorPL = self.motor_lb.getSelectedSensorPosition(0) a1 = (target-0.2)*self.ratio a2 = (target+0.2)*self.ratio if a1 < sensorPL < a2: return True def end_driveforward(self): self.motor_rb.set(0) self.motor_lb.set(0) self.mode = "" off = end_driveforward def getEncoderVelocity(self, fps): return fps*self.ratio/10 def getEncoderAccel(self, fps2): return fps2*self.ratio/10 def periodic(self): #Variables for the Navx angle = self.navx.getAngle() self.navx_table.putNumber('Angle', angle) #Variables used for the dashboard sensorPL = self.motor_lb.getSelectedSensorPosition(0) self.leftEncoder_table.putNumber("Position", sensorPL) sensorPR = self.motor_rb.getSelectedSensorPosition(0) self.rightEncoder_table.putNumber("Position", sensorPR) sensorVL = self.motor_lb.getSelectedSensorVelocity(0) self.leftEncoder_table.putNumber("Velocity", sensorVL) sensorVR = self.motor_rb.getSelectedSensorVelocity(0) self.rightEncoder_table.putNumber("Velocity", sensorVR) voltageL = self.motor_lb.getMotorOutputPercent() voltageR = self.motor_rb.getMotorOutputPercent() self.leftError.putNumber("Voltage", voltageL) self.rightError.putNumber("Voltage", voltageR) if self.logger is not None: self.logger.log()
class Intake(Subsystem): def __init__(self): super().__init__('Intake') self.intake_motor_closeOpen = wpilib.VictorSP(8) self.intake_motor_rightWheel = wpilib.VictorSP(9) self.intake_motor_leftWheel = wpilib.VictorSP(7) self.limit_switch = wpilib.DigitalOutput(0) self.pdp = wpilib.PowerDistributionPanel(16) self.intake_table = networktables.NetworkTables.getTable('/Intake') self.timer = wpilib.Timer() self.timer.start() self.logger = None #self.init_logger() def initDefaultCommand(self): self.setDefaultCommand(grabber.Grabber()) def closeGrabber(self, x): self.motor_closeOpen_set(x) def init_logger(self): self.logger = DataLogger('intake.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("voltagep_r", lambda: self.intake_motor_rightWheel.get()) self.logger.add("voltagep_m", lambda: self.intake_motor_closeOpen.get()) self.logger.add("voltagep_l", lambda: self.intake_motor_leftWheel.get()) self.logger.add("voltage", lambda: self.pdp.getVoltage()) self.logger.add("current_r", lambda: self.pdp.getCurrent(0)) self.logger.add("current_m", lambda: self.pdp.getCurrent(1)) self.logger.add("current_l", lambda: self.pdp.getCurrent(15)) def openGrabber(self): ''' if self.limit_switch == True: self.grabberOff() else:''' self.motor_closeOpen_set(-0.5) def open2Grabber(self): self.intake_motor_closeOpen.set(-0.4) self.cubeOut() def grabberOff(self): self.motor_closeOpen_set(0) def cubeOut(self): self.intake_motor_rightWheel.set(0.5) self.intake_motor_leftWheel.set(-0.5) def cubeIn(self): self.intake_motor_rightWheel.set(-1) self.intake_motor_leftWheel.set(1) def intakeWheelsOff(self): self.intake_motor_rightWheel.set(0) self.intake_motor_leftWheel.set(0) def motor_closeOpen_set(self, voltage_percent): self.intake_motor_closeOpen.set(voltage_percent) def motor_rightWheel_set(self, voltage_percent): self.intake_motor_rightWheel.set(voltage_percent) def motor_leftWheel_set(self, voltage_percent): self.intake_motor_leftWheel.set(voltage_percent) def periodic(self): self.intake_table.putBoolean("LimitSwitch", self.limit_switch.get()) if self.logger is not None: self.logger.log()
class CsvTrajectoryCommand(_CsvTrajectoryCommand): def __init__(self, fnom): super().__init__(fnom) self.ctrl_heading = PIDController( Kp=0, Ki=0, Kd=0, Kf=0, source=self.drivetrain.getAngle, output=self.correct_heading, period=self.period, ) self.ctrl_heading.setInputRange(-180, 180) self.ctrl_heading.setOutputRange(-0.5, 0.5) self.ctrl_heading.setContinuous(True) self.max_velocity_fps = 11 self.max_velocity_encps = self.drivetrain.fps_to_encp100ms(self.max_velocity_fps) self.ctrl_l = DriveController( Kp=0, Kd=0, Ks=1.293985, Kv=0.014172, Ka=0.005938, get_voltage=self.drivetrain.getVoltage, source=self.drivetrain.getLeftEncoderVelocity, output=self.drivetrain.setLeftMotor, period=self.period, ) self.ctrl_l.setInputRange(-self.max_velocity_encps, self.max_velocity_encps) self.ctrl_r = DriveController( Kp=0, Kd=0, Ks=1.320812, Kv=0.013736, Ka=0.005938, get_voltage=self.drivetrain.getVoltage, source=self.drivetrain.getRightEncoderVelocity, output=self.drivetrain.setRightMotor, period=self.period, ) self.ctrl_r.setInputRange(-self.max_velocity_encps, self.max_velocity_encps) def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.ctrl_l.enable() self.ctrl_r.enable() self.ctrl_heading.enable() self.logger = DataLogger("csv_trajectory1.csv") self.drivetrain.init_logger(self.logger) self.logger.add("profile_vel_r", lambda: self.target_v_r) self.logger.add("profile_vel_l", lambda: self.target_v_l) self.logger.add("pos_ft_l", lambda: self.pos_ft_l) self.logger.add("i", lambda: self.i) self.timer.start() self.i = 0 #print ('pdf init') def execute(self): self.pos_ft_l = self.drivetrain.getLeftEncoder() / self.drivetrain.ratio self.pos_ft_r = self.drivetrain.getRightEncoder() / self.drivetrain.ratio (_, _, _, self.target_v_l, self.target_v_r, self.target_a_l, self.target_a_r, self.target_heading) = self.get_trajectory_point_enc(self.i) self.ctrl_l.setSetpoint(self.target_v_l) self.ctrl_l.setAccelerationSetpoint(self.target_a_l) self.ctrl_r.setSetpoint(self.target_v_r) self.ctrl_r.setAccelerationSetpoint(self.target_a_r) self.ctrl_heading.setSetpoint(self.target_heading) self.drivetrain.feed() self.logger.log() self.i += 1 def end(self): self.ctrl_l.disable() self.ctrl_r.disable() self.ctrl_heading.disable() self.drivetrain.off() self.logger.flush() #print ('pdf end') def correct_heading(self, correction): pass
class ProfiledForward(wpilib.command.Command): def __init__(self, distance_ft): super().__init__("ProfiledForward") self.drivetrain = self.getRobot().drivetrain self.requires(self.drivetrain) self.dist_ft = distance_ft self.dist_enc = distance_ft * self.drivetrain.ratio self.timer = Timer() self.period = self.getRobot().getPeriod() self.ctrl_heading = PIDController( Kp=0, Ki=0, Kd=0, Kf=0, source=self.drivetrain.getAngle, output=self.correct_heading, period=self.period, ) self.ctrl_heading.setInputRange(-180, 180) self.ctrl_heading.setOutputRange(-0.5, 0.5) self.max_velocity_fps = 3 # ft/sec self.max_v_encps = self.drivetrain.fps_to_encp100ms( self.max_velocity_fps) self.max_acceleration = 3 # ft/sec^2 self.profiler_l = TrapezoidalProfile( cruise_v=self.max_velocity_fps, a=self.max_acceleration, target_pos=self.dist_ft, tolerance=(3 / 12.), # 3 inches ) self.profiler_r = TrapezoidalProfile( cruise_v=self.max_velocity_fps, a=self.max_acceleration, target_pos=-self.dist_ft, tolerance=(3 / 12.), # 3 inches ) self.ctrl_l = DriveController( Kp=0, Kd=0, Ks=1.293985, Kv=0.014172, Ka=0.005938, get_voltage=self.drivetrain.getVoltage, source=self.drivetrain.getLeftEncoderVelocity, output=self.drivetrain.setLeftMotor, period=self.period, ) self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps) self.ctrl_r = DriveController( Kp=0, Kd=0, Ks=1.320812, Kv=0.013736, Ka=0.005938, get_voltage=self.drivetrain.getVoltage, source=self.drivetrain.getRightEncoderVelocity, output=self.drivetrain.setRightMotor, period=self.period, ) self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps) self.target_v_l = 0 self.target_v_r = 0 self.target_a_l = 0 self.target_a_r = 0 self.pos_ft_l = 0 self.pos_ft_r = 0 def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.ctrl_l.enable() self.ctrl_r.enable() self.ctrl_heading.enable() self.logger = DataLogger("profiled_forward.csv") self.drivetrain.init_logger(self.logger) self.logger.add("profile_vel_r", lambda: self.target_v_r) self.logger.add("profile_vel_l", lambda: self.target_v_l) self.logger.add("pos_ft_l", lambda: self.pos_ft_l) self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos) self.logger.add("adist_l", lambda: self.profiler_l.adist) self.logger.add("err_l", lambda: self.profiler_l.err) self.logger.add("choice_l", lambda: self.profiler_l.choice) self.logger.add("adist_r", lambda: self.profiler_l.adist) self.logger.add("err_r", lambda: self.profiler_l.err) self.logger.add("choice_r", lambda: self.profiler_l.choice) self.timer.start() #print ('pdf init') def execute(self): self.pos_ft_l = self.drivetrain.getLeftEncoder( ) / self.drivetrain.ratio self.pos_ft_r = self.drivetrain.getRightEncoder( ) / self.drivetrain.ratio #print ('pdf exec ', self.timer.get()) self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period) self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period) self.target_v_l = self.drivetrain.fps_to_encp100ms( self.profiler_l.current_target_v) self.target_v_r = self.drivetrain.fps_to_encp100ms( self.profiler_r.current_target_v) self.target_a_l = self.drivetrain.fps2_to_encpsp100ms( self.profiler_l.current_a) self.target_a_r = self.drivetrain.fps2_to_encpsp100ms( self.profiler_r.current_a) self.ctrl_l.setSetpoint(self.target_v_l) self.ctrl_l.setAccelerationSetpoint(self.target_a_l) self.ctrl_r.setSetpoint(self.target_v_r) self.ctrl_r.setAccelerationSetpoint(self.target_a_r) #self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward()) #self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward()) self.logger.log() self.drivetrain.feed() def isFinished(self): return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3 and self.profiler_l.current_target_v == 0 and self.profiler_l.current_a == 0 and self.profiler_r.current_target_v == 0 and self.profiler_r.current_a == 0) def end(self): self.ctrl_l.disable() self.ctrl_r.disable() self.ctrl_heading.disable() self.drivetrain.off() self.logger.flush() #print ('pdf end') def correct_heading(self, correction): self.profiler_l.setCruiseVelocityScale(1 + correction) self.profiler_r.setCruiseVelocityScale(1 - correction)
class Elevator(Subsystem): #: encoder ticks per revolution (need ticks per ft) ratio = 4096 max_pos = 3 min_pos = 0 def __init__(self): super().__init__('Elevator') self.sensor = wpilib.DigitalInput(9) # temp num, true is on self.motor = ctre.WPI_TalonSRX(3) self.other_motor = ctre.WPI_TalonSRX(2) self.other_motor.follow(self.motor) self.zeroed = False self.motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, 0) self.elevator_table = networktables.NetworkTables.getTable('/Elevator') self.motor.setSensorPhase(True) self.initialize_motionMagic() self.timer = wpilib.Timer() self.timer.start() self.logger = None self.init_logger() def init_logger(self): self.logger = DataLogger('elevator.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("enc_pos", lambda: self.motor.getSelectedSensorPosition(0)) self.logger.add("voltagep_motor", lambda: self.motor.getMotorOutputPercent()) self.logger.add("voltagep_othermotor", lambda: self.other_motor.getMotorOutputPercent()) self.logger.add("voltage", lambda: self.motor.getBusVoltage()) self.logger.add("current_motor", lambda: self.motor.getOutputCurrent()) self.logger.add("current_othermotor", lambda: self.other_motor.getOutputCurrent()) self.logger.add("zeroed", lambda: 1 if self.zeroed else 0) def initialize_motionMagic(self): self.motor.configMotionAcceleration(int(self.ftToEncoder_accel(1)), 0) self.motor.configMotionCruiseVelocity(int(self.ftToEncoder_vel(1)), 0) self.motor.configNominalOutputForward(0, 0) self.motor.configNominalOutputReverse(0, 0) self.motor.configPeakOutputForward(0.8, 0) self.motor.configPeakOutputReverse(-0.8, 0) self.motor.selectProfileSlot(0, 0) self.motor.config_kF(0, 0, 0) self.motor.config_kP(0, 0.018, 0) self.motor.config_kI(0, 0, 0) self.motor.config_kD(0, 0, 0) def set_position(self, position): if (position > self.max_pos): position = self.max_pos if (position < self.min_pos): position = self.min_pos self.motor.set(ctre.ControlMode.MotionMagic, position * self.ratio) def initDefaultCommand(self): self.setDefaultCommand(ElevatorTest()) def ftToEncoder_accel(self, ftPerSec_sq): return ftPerSec_sq * self.ratio / 10 def ftToEncoder_vel(self, ftPerSec): return ftPerSec * self.ratio / 10 def hover(self): self.motor.set(0.1) pass def descend(self, voltage): pass def ascend(self, voltage): self.motor.set(voltage) def test_drive_x(self, x): self.motor.set(x) def test_drive_positive(self): self.motor.set(0.5) def test_drive_negative(self): self.motor.set(-0.1) def off(self): self.motor.stopMotor() def zeroEncoder(self): self.zeroed = True self.motor.setSelectedSensorPosition(0, 0, 0) def getSensor(self): return self.sensor.get() def periodic(self): position = self.motor.getSelectedSensorPosition(0) self.elevator_table.putNumber("Position", position) self.elevator_table.putNumber("Motor Current", self.motor.getOutputCurrent()) self.elevator_table.putNumber("Other Motor Current", self.other_motor.getOutputCurrent()) if self.logger is not None: self.logger.log()
class Drivetrain(Subsystem): '''''' #: encoder/ft ratio ratio = 630 def __init__(self): super().__init__('Drivetrain') #The set motor controllers for this years robot and how motors are coded self.motor_rb = ctre.WPI_TalonSRX(1) self.motor_rf = ctre.WPI_VictorSPX(17) self.motor_lb = ctre.WPI_TalonSRX(13) self.motor_lf = ctre.WPI_VictorSPX(12) self.motor_rf.follow(self.motor_rb) self.motor_lf.follow(self.motor_lb) self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf] self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb) self.navx = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(16) self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0) self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.table = networktables.NetworkTables.getTable("/Drivetrain") self.sd_table = networktables.NetworkTables.getTable("/SmartDashboard") self.motor_lb.setSensorPhase(False) self.motor_rb.setSensorPhase(False) self.left_offset = 0 self.right_offset = 0 self.timer = wpilib.Timer() self.timer.start() self.computed_velocity = 0 self.logger = None #self.init_logger() def drive_forward(self, motorF): #self.drive.arcadeDrive(-motorF, 0, squaredInputs= False) self.motor_lb.set(motorF) self.motor_rb.set(-motorF * 1.0) self.drive.feed() def custom_move(self, rvolt, lvolt): self.motor_lb.set(lvolt) self.morot_rb.set(-rvolt) self.drive.feed() def turn_right(self, voltage): self.motor_lb.set(voltage) self.motor_rb.set(voltage) self.drive.feed() def turn_left(self, voltage): self.motor_lb.set(-voltage) self.motor_rb.set(-voltage) self.drive.feed() def wheels_stopped(self): return abs(self.motor_lb.getSelectedSensorVelocity(0)) <= 30 and abs( self.motor_rb.getSelectedSensorVelocity(0)) <= 30 def voltage_ramp_on(self): self.motor_lb.configOpenLoopRamp(0.2, 0) self.motor_rb.configOpenLoopRamp(0.2, 0) def voltage_ramp_off(self): self.motor_lb.configOpenLoopRamp(0, 0) self.motor_rb.configOpenLoopRamp(0, 0) def execute_turn(self, angle): position = angle / 60. self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position) self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position) self.drive.feed() def initDefaultCommand(self): self.setDefaultCommand(Drive()) def init_logger(self): self.logger = DataLogger('drivetrain.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("heading", lambda: self.navx.getAngle()) self.logger.add("enc_pos_l", lambda: self.getLeftEncoder()) self.logger.add("enc_pos_r", lambda: self.getRightEncoder()) self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0)) self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0)) #self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0)) #self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0)) #self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0)) #self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0)) self.logger.add("computed_velocity", lambda: self.computed_velocity) self.logger.add("mode", lambda: self.running_command_name()) self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage()) #self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent()) #self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent()) self.logger.add("current_rf", lambda: self.pdp.getCurrent(0)) self.logger.add("current_rb", lambda: self.pdp.getCurrent(1)) self.logger.add("current_lf", lambda: self.pdp.getCurrent(15)) self.logger.add("current_lb", lambda: self.pdp.getCurrent(13)) self.logger.add("enc_offset_l", lambda: self.left_offset) self.logger.add("enc_offset_r", lambda: self.right_offset) def running_command_name(self): command = self.getCurrentCommand() if command is not None: return command.getName() def zeroEncoders(self): self.right_offset = self.motor_rb.getSelectedSensorPosition(0) self.left_offset = self.motor_lb.getSelectedSensorPosition(0) def getLeftEncoder(self): return self.motor_lb.getSelectedSensorPosition(0) - self.left_offset def getRightEncoder(self): return self.motor_rb.getSelectedSensorPosition(0) - self.right_offset def zeroNavx(self): self.navx.reset() def initialize_driveTurnlike(self): #The PID values with the motors self.zeroEncoders() self.motor_rb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(1.25)), 0) self.motor_lb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(1.25)), 0) self.motor_rb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(2.5)), 0) self.motor_lb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(2.5)), 0) self.motor_rb.configNominalOutputForward(.1, 0) self.motor_lb.configNominalOutputForward(.1, 0) self.motor_rb.configNominalOutputReverse(-0.1, 0) self.motor_lb.configNominalOutputReverse(-0.1, 0) self.motor_rb.configPeakOutputForward(0.4, 0) self.motor_lb.configPeakOutputForward(0.4, 0) self.motor_rb.configPeakOutputReverse(-0.4, 0) self.motor_lb.configPeakOutputReverse(-0.4, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) # self.motor_rb.config_kF(0, 0, 0) # self.motor_lb.config_kF(0, 0, 0) self.motor_rb.config_kP(0, 0.18, 0) self.motor_lb.config_kP(0, 0.18, 0) self.motor_rb.config_kI(0, 0, 0) self.motor_lb.config_kI(0, 0, 0) self.motor_rb.config_kD(0, 8, 0) self.motor_lb.config_kD(0, 8, 0) def uninitialize_driveTurnlike(self): #The PID values with the motors self.motor_rb.configNominalOutputForward(0, 0) self.motor_lb.configNominalOutputForward(0, 0) self.motor_rb.configNominalOutputReverse(0, 0) self.motor_lb.configNominalOutputReverse(0, 0) self.motor_rb.configPeakOutputForward(1, 0) self.motor_lb.configPeakOutputForward(1, 0) self.motor_rb.configPeakOutputReverse(-1, 0) self.motor_lb.configPeakOutputReverse(-1, 0) def initialize_driveForward(self): self.zeroEncoders() self.config_parameters(p=0.18, i=0.001, d=2, f=0) self.config_motionmagic(v_fps=3.5, a_fps2=5.0) def initialize_velocity_closedloop(self): self.motor_rb.configNominalOutputForward(0, 0) self.motor_lb.configNominalOutputForward(0, 0) self.motor_rb.configNominalOutputReverse(0, 0) self.motor_lb.configNominalOutputReverse(0, 0) self.motor_rb.configPeakOutputForward(1, 0) self.motor_lb.configPeakOutputForward(1, 0) self.motor_rb.configPeakOutputReverse(-1, 0) self.motor_lb.configPeakOutputReverse(-1, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) # tested for counterclockwise turn self.motor_rb.config_kF(0, 1.88, 0) self.motor_rb.config_kP(0, 2.18, 0) self.motor_rb.config_kI(0, 0.00, 0) self.motor_rb.config_kD(0, 450, 0) self.motor_lb.config_kF(0, 1.88, 0) self.motor_lb.config_kP(0, 2.18, 0) self.motor_lb.config_kI(0, 0.00, 0) self.motor_lb.config_kD(0, 450, 0) def config_parameters(self, p, i, f, d, nominal_forward = 0, nominal_reverse = 0, peak_forward = 1, peak_reverse = -1): self.motor_rb.configNominalOutputForward(nominal_forward, 0) self.motor_lb.configNominalOutputForward(nominal_forward, 0) self.motor_rb.configNominalOutputReverse(nominal_reverse, 0) self.motor_lb.configNominalOutputReverse(nominal_reverse, 0) self.motor_rb.configPeakOutputForward(peak_forward, 0) self.motor_lb.configPeakOutputForward(peak_forward, 0) self.motor_rb.configPeakOutputReverse(peak_reverse, 0) self.motor_lb.configPeakOutputReverse(peak_reverse, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) # tested for counterclockwise turn self.motor_rb.config_kF(0, f, 0) self.motor_rb.config_kP(0, p, 0) self.motor_rb.config_kI(0, i, 0) self.motor_rb.config_kD(0, d, 0) self.motor_lb.config_kF(0, f, 0) self.motor_lb.config_kP(0, p, 0) self.motor_lb.config_kI(0, i, 0) self.motor_lb.config_kD(0, d, 0) def config_motionmagic(self, v_fps, a_fps2): self.motor_rb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(v_fps)), 0) self.motor_lb.configMotionAcceleration(int(self.fps2_to_encpsp100ms(v_fps)), 0) self.motor_rb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(a_fps2)), 0) self.motor_lb.configMotionCruiseVelocity(int(self.fps_to_encp100ms(a_fps2)), 0) def getAngle(self): return self.navx.getAngle() def set_turn_velocity(self, v_degps): print(v_degps) velocity_ratio = 1.6 self.computed_velocity = v_encp100ms = velocity_ratio * v_degps #self.computed_velocity = v_encp100ms = 32 self.motor_rb.set(ctre.ControlMode.Velocity, v_encp100ms) self.motor_lb.set(ctre.ControlMode.Velocity, v_encp100ms) self.drive.feed() def execute_driveforward(self, positionL, positionR): self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionR + self.right_offset) self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionL + self.left_offset) self.drive.feed() def isFinished_driveforward(self, target): sensorPL = self.motor_lb.getSelectedSensorPosition(0) a1 = (target-0.2)*self.ratio a2 = (target+0.2)*self.ratio if a1 < sensorPL < a2: return True def end_driveforward(self): self.motor_rb.set(0) self.motor_lb.set(0) off = end_driveforward def fps_to_encp100ms(self, fps): return fps*self.ratio/10 def fps2_to_encpsp100ms(self, fps2): return fps2*self.ratio/10 def periodic(self): #Variables for the Navx autoMode = self.sd_table.getString("autonomousMode", None) self.sd_table.putString("robotAutoMode", autoMode) switch_attempt = self.sd_table.getBoolean("switchAttempt", None) self.sd_table.putBoolean("robotSwitchAttempt", switch_attempt) scale_attempt = self.sd_table.getBoolean("scaleAttempt", None) self.sd_table.putBoolean("robotScaleAttempt", scale_attempt) angle = self.navx.getAngle() self.table.putNumber('Angle', angle) #Variables used for the dashboard sensorPL = self.getLeftEncoder() self.table.putNumber("Left/Position", sensorPL) sensorPR = self.getRightEncoder() self.table.putNumber("Right/Position", sensorPR) sensorVL = self.motor_lb.getSelectedSensorVelocity(0) self.table.putNumber("Left/Velocity", sensorVL) sensorVR = self.motor_rb.getSelectedSensorVelocity(0) self.table.putNumber("Right/Velocity", sensorVR) #voltageL = self.motor_lb.getMotorOutputPercent() #voltageR = self.motor_rb.getMotorOutputPercent() #self.table.putNumber("Left/Voltage", voltageL) #self.table.putNumber("Right/Voltage", voltageR) if self.logger is not None: self.logger.log()
N_max = int(sys.argv[1]) if len(sys.argv) > 2: name = sys.argv[2] else: name = 'prime_numbers_1.csv' else: print("Using default value N_max = 100") print("Optional command line argument to change this") N_max = 100 name = 'prime_numbers_1.csv' # setup other default parameters var_names = ['i', 'pn'] path = os.getcwd() + '/' logger = DataLogger(path, name, var_names, log_interval=99) def is_prime(n): for ni in range(2, n): if (n % ni) == 0: return False return True index = 0 for i in range(N_max): if is_prime(i): print(index, i) logger.log(i=index, pn=i) index += 1 logger.flush_cache()