Exemple #1
0
 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)
     self.logger.add("VPR", lambda: self.motor_rb.getMotorOutputPercent())
     self.logger.add("VRL", lambda: self.motor_lb.getMotorOutputPercent())
 def create_data_logger_instance(self):
     """Create the DataLogger class instance for logging the data."""
     self.__data_log_path = os.path.join(
         PATH_LOG,
         time.strftime("%Y-%m-%d_%H%M%S") + "_log")
     if not os.path.exists(self.__data_log_path):
         os.makedirs(self.__data_log_path)
     self.data_logger = DataLogger('data_log.csv', self.__data_log_path,
                                   'plot.png')
Exemple #3
0
    def __init__(self):
        self.board = ArduinoBoard("/dev/ttyACM0", 230400, timeout=5)
        self.data_analyser = DataLogger(self.board.sample_no,
                                        self.board.sample_freq)
        self.f, self.x = self.data_analyser.set_sample_freq(
            self.board.sample_freq)

        self.mode = None
        self.xscale = 1
        self.yscale = 1

        # pyqtgraph stuff
        pg.setConfigOptions(antialias=True)
        self.traces = dict()
        self.app = QtGui.QApplication(sys.argv)
        self.win = KeyPressWindow(title='Spectrum Analyzer')
        self.win.setWindowTitle('Spectrum Analyzer')

        self.win.sigKeyPress.connect(self.keyPressed)

        self.waveform = self.win.addPlot(title='WAVEFORM',
                                         row=1,
                                         col=1,
                                         labels={'bottom': "Time (s)"})
        self.spectrum = self.win.addPlot(title='SPECTRUM',
                                         row=1,
                                         col=2,
                                         labels={'bottom': "Frequency (Hz)"})
        self.specgram = self.win.addPlot(title='SPECTROGRAM',
                                         row=2,
                                         col=1,
                                         colspan=2,
                                         labels={'bottom': "Frequency (Hz)"})

        self.img = pg.ImageItem()
        self.specgram.addItem(self.img)

        # bipolar colormap
        pos = np.array([0., 1., 0.5, 0.25, 0.75])
        color = np.array(
            [[0, 255, 255, 255], [255, 255, 0, 255], [0, 0, 0, 255],
             (0, 0, 255, 255), (255, 0, 0, 255)],
            dtype=np.ubyte)
        cmap = pg.ColorMap(pos, color)
        lut = cmap.getLookupTable(0.0, 1.0, 256)

        self.img.setLookupTable(lut)
        self.img.setLevels([20 * np.log10(10), 20 * np.log10(10000)])

        # waveform and spectrum x points
        self.scale_plots()

        # tell Arduino to start sending data
        self.board.send_command("Send Data")
class EvalNode:
    def __init__(self):
        self.logged_data = OrderedDict({'time_stamp': 0})
        #self.create_data_logger_instance()
        self.t1 = Worker(self, 'time_synched_cb')
        self.__message = ''

    def create_data_logger_instance(self):
        """Create the DataLogger class instance for logging the data."""
        self.__data_log_path = os.path.join(
            PATH_LOG,
            time.strftime("%Y-%m-%d_%H%M%S") + "_log")
        if not os.path.exists(self.__data_log_path):
            os.makedirs(self.__data_log_path)
        self.data_logger = DataLogger('data_log.csv', self.__data_log_path,
                                      'plot.png')

    def trigger_test(self, cmd):
        if cmd == 'start':
            if self.t1.is_alive():
                self.__message = "{} is already running.".format(self.t1)
            else:
                self.t1.start()
                self.__message = "{} is started".format(self.t1)
        elif cmd == 'stop':
            if not self.t1.is_alive():
                self.__message = "{} is already stopped".format(self.t1)
            else:
                self.t1.stop()
                self.data_logger.save_data_to_csv()
                self.__message = "{} is stopped".format(self.t1)
        print(self.__message)
        return self.__message

    def log_to_datafile_and_dcrt(self):
        """Return dcrt debug message and adds data to data list."""
        now = self.timestamp()
        self.logged_data['time_stamp'] = '%d' % (now)
        print(self.logged_data)
        message = self.data_logger.create_dcrt_log_message(
            now, self.logged_data)
        self.data_logger.add_data_to_data_list(self.logged_data)
        return message

    def time_synched_cb(self):
        """Call back function for SCP Receive thread."""
        message_dat = self.log_to_datafile_and_dcrt()
        #print(message_dat)
        time.sleep(0.005)

    def timestamp(self):
        """Return the current clock time."""
        return time.time()
Exemple #5
0
 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)
Exemple #6
0
 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))
Exemple #7
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("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
Exemple #8
0
def train(problem,              # Problem object, describing the task.
          initial_policy,       # Initial policy.
          goal_state,           # Goal state (s^g).
          full_start_dist,      # Full start state distribution (rho_0).
          N_new=200, N_old=100, # Num new and old start states to sample.
          R_min=0.5, R_max=1.0, # Reward bounds defining what a "good" start state is.
          num_iters=100,        # Number of iterations of training to run.
          num_ppo_iters=20,     # Number of iterations of PPO training to run per train step.
          curriculum_strategy=random, 
          train_algo=ppo, 
          start_distribution=uniform,
          debug=False):

    data_logger = DataLogger(col_names=['overall_iter', 'ppo_iter', 
                                        'overall_perf', 'overall_area', 'overall_reward',
                                        'ppo_perf', 'ppo_lens', 'ppo_rews'],
                             filepath=os.path.join(DATA_DIR, 'data_%s.csv' % args.type),
                             data_dir=DATA_DIR)

    print(locals(), flush=True);
    if curriculum_strategy in [random]:
        return train_pointwise(**locals());
    elif curriculum_strategy in [backward_reachable]:
        return train_gridwise(**locals());
    elif curriculum_strategy is None:
        return train_ppo_only(**locals());
    else:
        raise ValueError("You passed in an unknown curriculum strategy!");
Exemple #9
0
    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.motor_lb.setInverted(False)
        self.motor_rb.setInverted(False)
        self.motor_rf.setInverted(False)
        self.motor_lf.setInverted(False)
        self.left_offset = 0
        self.right_offset = 0

        self.timer = wpilib.Timer()
        self.timer.start()
        self.computed_velocity = 0

        self.logger_enabled = False
        #self.logger = None
        self.logger = DataLogger('drivetrain.csv')
        self.init_logger(self.logger)
Exemple #10
0
    def __init__(self, config):
        self._config = config
        self._time = 0
        if config['gui'] == 'pygame':
            if config['display'] == 'fullscreen':
                self._display = pygame.display.set_mode(
                    config['display_size'], pygame.FULLSCREEN)
            else:
                self._display = pygame.display.set_mode(config['display_size'])

        # Mutation rates that probably ought to be zero.
        self._mutation_of_mean_mutation_rate = 0.000
        self._mutation_of_sigma_mutation_rate = 0.000
        self._reproduction_mutation_rate = 0.000

        self._init_agents()
        self._init_landscape_locks()

        # render_data stores RGB, Energy and first 3 Keys for rendering.
        if config['world_d'] == 1:
            self._render_data = np.zeros(
                (config['map_size'][0], config['num_1d_history'], 7))
            self._bitmap = np.zeros(
                (config['map_size'][0], config['num_1d_history'], 3),
                dtype=np.uint8)
        else:
            self._render_data = np.zeros((config['map_size'] + (7, )))
            self._bitmap = np.zeros((config['map_size'] + (3, )),
                                    dtype=np.uint8)

        self._data = {}  # For plotting, etc.
        self._data_history_len = self._render_data.shape[1]
        self._repo_energy_stats = []
        # Track mean min, max; sigma min, max
        self._locks_metrics = np.zeros((4, config['num_locks']))
        self._locks_metrics[0] = np.inf
        self._locks_metrics[1] = -np.inf
        self._locks_metrics[2] = np.inf
        self._locks_metrics[3] = -np.inf

        self._data_logger = None
        if config['log_data'] is not None:
            self._data_logger = DataLogger(['sigma'], config['log_data'], 1000)
Exemple #11
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()
Exemple #12
0
 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))
Exemple #13
0
    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()
Exemple #14
0
 def __init__(self, service, measurement):
     self.config = measurement["configuration"]
     self.service = service
     self.measurement = measurement
     self.collections_created = False
     self.ms = MSInstance(service, measurement)
     self.dl = DataLogger(service, measurement)
     self.mids = {} # {subject1: {metric1:mid, metric2:mid}, subj2: {...}}
     # {mid: [{"ts": ts, "value": val}, {"ts": ts, "value": val}]}
     self.mid_to_data = {}
     self.mid_to_et = {}
     self.unis = UNISInstance(service)
     self.num_collected = 0
Exemple #15
0
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()
Exemple #16
0
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()
Exemple #17
0
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()
Exemple #18
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()
        default='roborio-3223-frc.local',
        help='specify ip address of robot')
    return parser

parser = setup_options_parser()
args = parser.parse_args()

NetworkTable.setIPAddress(args.robot)
NetworkTable.setClientMode()
NetworkTable.initialize()

file_name = os.path.join(args.output_dir, "structure.jpg")
tmp_name = os.path.join(args.output_dir, "structure.tmp.jpg")
vision = Vision()
vision.mode = 5
vision.setup_mode_listener()

logger = DataLogger(args.log_dir)

now = time.time()

with vision:
    while True:
        vision.get_depths()
        vision.idepth_stats()
        vision.set_display()
        cv2.imwrite(tmp_name, vision.display)
        os.rename(tmp_name, file_name)
        logger.log_data(vision.depth, vision.ir)
        time.sleep(0.05)
Exemple #20
0
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
Exemple #21
0
 def initialize(self):
     self.drivetrain.zeroEncoders()
     self.drivetrain.zeroNavx()
     self.i = 0
     self.logger = DataLogger("ss_trajectory.csv")
     self.drivetrain.init_logger(self.logger)
Exemple #22
0
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()
from data_logger import DataLogger
import time

if __name__ == "__main__":
    log_variables = [
        "air_temperature", "humidity", "co2", "o2", "water_temperature", "ph",
        "ec", "heater_is_on", "humidifier_is_on"
    ]
    data_logger = DataLogger(log_variables)

    while True:
        data_logger.run()
        time.sleep(1)
Exemple #24
0
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)
Exemple #25
0
class Collector:
    """Collects reported measurements and aggregates them for
    sending to MS at appropriate intervals.

    Also does a bunch of other stuff which should probably be handled by separate classes.
    Creates all the metadata objects, and the measurement object in UNIS for all data inserted.
    Depends directly on the MS and UNIS... output could be far more modular.
    """

    def __init__(self, service, measurement):
        self.config = measurement["configuration"]
        self.service = service
        self.measurement = measurement
        self.collections_created = False
        self.ms = MSInstance(service, measurement)
        self.dl = DataLogger(service, measurement)
        self.mids = {} # {subject1: {metric1:mid, metric2:mid}, subj2: {...}}
        # {mid: [{"ts": ts, "value": val}, {"ts": ts, "value": val}]}
        self.mid_to_data = {}
        self.mid_to_et = {}
        self.unis = UNISInstance(service)
        self.num_collected = 0

    def insert(self, data, ts):
        '''
        Called (by probe_runner) to insert new data into this collector object.
        '''
        mids = self.mids
        for subject, met_val in data.iteritems():
            if "ts" in met_val:
                ts = met_val["ts"]
                del met_val["ts"]
            for metric, value in met_val.iteritems():
                if metric not in self.measurement["eventTypes"]:
                    self._add_et(metric)
                if not metric in mids.get(subject, {}):
                    r = self.unis.find_or_create_metadata(subject,
                                                          metric,
                                                          self.measurement)
                    mids.setdefault(subject, {})[metric] = r["id"]
                    self.mid_to_data[r["id"]] = []
                self.mid_to_et[mids[subject][metric]] = metric
                self._insert_datum(mids[subject][metric], ts, value)

        self.num_collected += 1
        if self.num_collected >= self.config["reporting_params"]:
            ret = self.report()
            if ret:
                self.num_collected = 0

    def _insert_datum(self, mid, ts, val):
        item = dict({"ts": ts * 10e5,
                     "value":val})
        self.mid_to_data[mid].append(item)

    def _add_et(self, metric):
        self.measurement["eventTypes"].append(metric)
        r = self.unis.put("/measurements/" +
                          self.measurement["id"],
                          data=self.measurement)
        if r:
            self.measurement = r

    def report(self):
        '''
        Send all data collected so far, then clear stored data.
        '''
        post_data = []
        for mid, data in self.mid_to_data.iteritems():
            if len(data):
                post_data.append({"mid":mid, "data":data})
        ms_ret = self.ms.post_data(post_data)
        dl_ret = self.dl.write_data(post_data, self.mid_to_et)
        if not ms_ret and not dl_ret and self.num_collected < self.config["reporting_tolerance"] * self.config["reporting_params"]:
            return None
        self._clear_data()
        return True

    def _clear_data(self):
        for mid in self.mid_to_data:
            self.mid_to_data[mid]=[]
Exemple #26
0
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

Please contact the author(s) of this library if you have any questions.
Authors: David Fridovich-Keil   ( [email protected] )
"""

################################################################################
#
# Node to wrap the DataLogger class.
#
################################################################################

from data_logger import DataLogger

import rospy
import sys
import numpy as np

if __name__ == "__main__":
    rospy.init_node("data_logger")
    logger = DataLogger()
    if not logger.Initialize():
        print("Failed to initialize the data_logger node.")
        sys.exit(1)

    rospy.on_shutdown(logger.Save)
    rospy.spin()
Exemple #27
0
import time
import datetime
import json
import argparse

from data_logger import DataLogger, DataType

# Store the log file to "./logs/"
try:
    os.makedirs("logs")
except os.error as err:
    # It's OK if the log directory exists. This is to be compatible with Python 2.7
    if err.errno != errno.EEXIST:
        raise err

data_logger = DataLogger("logs", max_file_size=4000, max_file_count=10)


def _parse_configs(config_file_path):
    """
    Parse the configuration file.

    :param config_file_path: The path to the configuration file (including the filename)
    :type: str
    :return: The configuration data
    :rtype: dict
    """
    with open(config_file_path, 'r') as config_file:
        configs = json.load(config_file)
    return configs
Exemple #28
0
def hw_monitor(device, args):
    collect_sensor_data = init_sensors()

    sensors_spec = dict(CPU='it8686-isa-0a40.temp3',
                        GPU='amdgpu-pci-4200.edge',
                        Chipset='it8686-isa-0a40.temp2',
                        System='it8792-isa-0a60.temp3')

    fan_spec = dict(CPU='it8686-isa-0a40.fan1',
                    GPU='amdgpu-pci-4200.fan1',
                    Rear='it8792-isa-0a60.fan3',
                    SYS1='it8686-isa-0a40.fan2')

    sensors_data_logger = DataLogger(collect_sensor_data,
                                     max_entries=(device.width / 2) -
                                     10).start()
    loadavg_data_logger = DataLogger(psutil.getloadavg,
                                     max_entries=device.width - 2).start()
    network_data_logger = DataLogger(
        lambda: psutil.net_io_counters(pernic=True)[args.network],
        max_entries=device.width / 2 - 15).start()

    def shutdownHandler(signal, frame):
        loadavg_data_logger.stop()
        sensors_data_logger.stop()
        network_data_logger.stop()
        exit(0)

    signal.signal(signal.SIGINT, shutdownHandler)
    signal.signal(signal.SIGTERM, shutdownHandler)

    virtual = viewport(device,
                       width=device.width,
                       height=1024,
                       mode='RGBA',
                       dither=True)
    with canvas(virtual) as draw:
        y_offset = render_logo(draw, 0, device.width, args.title)

        hotspots = [
            snapshot(device.width, 9, cpu_percent.render, interval=0.5),
            snapshot(device.width,
                     cpu_barchart.height + 4,
                     cpu_barchart.render,
                     interval=0.5),
            snapshot(device.width,
                     cpu_stats.height + 11,
                     cpu_stats.render,
                     interval=2),
            snapshot(device.width, uptime.height, uptime.render, interval=10),
            snapshot(device.width, 10, system_load.render, interval=1.0),
            snapshot(device.width,
                     loadavg_chart.height,
                     loadavg_chart.using(loadavg_data_logger),
                     interval=1.0),
            snapshot(device.width, 10, memory.render, interval=5.0),
            snapshot(device.width, 28, disk.directory('/'), interval=5.0),
            snapshot(device.width,
                     network.height,
                     network.using(args.network, network_data_logger),
                     interval=2.0),
            snapshot(device.width,
                     64,
                     sensors_chart.using(sensors_data_logger, sensors_spec,
                                         fan_spec),
                     interval=1.0),
            snapshot(device.width,
                     64 * 6,
                     device_list.init('http://192.168.1.254'),
                     interval=30)
        ]

        for hotspot in hotspots:
            virtual.add_hotspot(hotspot, (0, y_offset))
            y_offset += hotspot.height

        render_logo(draw, y_offset, device.width, args.title)

    # time.sleep(5.0)
    # for y in pause_every(64, 96, infinite(y_offset)):
    for y in stepper(64, y_offset, 512):
        with framerate_regulator():
            virtual.set_position((0, y))
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()
Exemple #30
0
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()
Exemple #31
0
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()
Exemple #32
0
class Loki():
    def __init__(self, config):
        self._config = config
        self._time = 0
        if config['gui'] == 'pygame':
            if config['display'] == 'fullscreen':
                self._display = pygame.display.set_mode(
                    config['display_size'], pygame.FULLSCREEN)
            else:
                self._display = pygame.display.set_mode(config['display_size'])

        # Mutation rates that probably ought to be zero.
        self._mutation_of_mean_mutation_rate = 0.000
        self._mutation_of_sigma_mutation_rate = 0.000
        self._reproduction_mutation_rate = 0.000

        self._init_agents()
        self._init_landscape_locks()

        # render_data stores RGB, Energy and first 3 Keys for rendering.
        if config['world_d'] == 1:
            self._render_data = np.zeros(
                (config['map_size'][0], config['num_1d_history'], 7))
            self._bitmap = np.zeros(
                (config['map_size'][0], config['num_1d_history'], 3),
                dtype=np.uint8)
        else:
            self._render_data = np.zeros((config['map_size'] + (7, )))
            self._bitmap = np.zeros((config['map_size'] + (3, )),
                                    dtype=np.uint8)

        self._data = {}  # For plotting, etc.
        self._data_history_len = self._render_data.shape[1]
        self._repo_energy_stats = []
        # Track mean min, max; sigma min, max
        self._locks_metrics = np.zeros((4, config['num_locks']))
        self._locks_metrics[0] = np.inf
        self._locks_metrics[1] = -np.inf
        self._locks_metrics[2] = np.inf
        self._locks_metrics[3] = -np.inf

        self._data_logger = None
        if config['log_data'] is not None:
            self._data_logger = DataLogger(['sigma'], config['log_data'], 1000)

    def _init_agents(self):
        """Creates dict with all agents' key and state data."""

        self._agent_data = dict(keys=np.zeros(
            (self._config['num_agents'], self._config['num_locks'],
             int(Key._num))),
                                state=np.zeros((self._config['num_agents'],
                                                int(State._num))))
        keys = self._agent_data['keys']
        keys[:, :, Key.mean] = np.random.uniform(size=keys.shape[0:2])
        keys[:, :, Key.sigma] = np.ones(keys.shape[0:2]) * 0.1

        # Same mutation rate for all (although could try random intial mutation
        # rates, e.g. np.random.uniform(size=keys.shape[0:2]) * 0.02
        keys[:, :, Key.mean_mut] = 0.01
        keys[:, :, Key.sigma_mut] = 0.0001

        state = self._agent_data['state']
        state[:, State.repo_threshold] = 5.

        state[:, State.repo_threshold_mut] = np.random.uniform(
            size=state.shape[0]) * self._reproduction_mutation_rate
        state[:, State._colour_start:State._colour_end] = np.random.uniform(
            size=(state.shape[0], 3))

    def _init_landscape_locks(self):
        # --- A few different ways of setting up the initial locks
        # Randomised
        if (self._config['landscape'] == 'wobbly'
                or self._config['landscape'] == 'rough'):
            self._locks = np.random.uniform(0,
                                            1,
                                            size=(self._config['num_agents'],
                                                  self._config['num_locks']))
            if self._config['landscape'] == 'wobbly':
                window_len = int(self._config['map_size'][0] / 8)
                left_off = math.ceil((window_len - 1) / 2)
                right_off = math.ceil((window_len - 2) / 2)
                w = np.ones(window_len, 'd')
                for i in range(self._config['num_locks']):
                    s = np.r_[self._locks[:, i][window_len - 1:0:-1],
                              self._locks[:, i],
                              self._locks[:, i][-2:-window_len - 1:-1]]
                    self._locks[:, i] = np.convolve(
                        w / w.sum(), s, mode='valid')[left_off:-right_off]

        elif self._config['landscape'] == 'gradient':
            self._locks = np.ones(
                (self._config['num_agents'], self._config['num_locks']))
            self._locks[:, 0] = np.linspace(0., 1., self._locks.shape[0])
            self._locks[:, 1] = np.linspace(0., 1., self._locks.shape[0])
            self._locks[:, 2] = np.linspace(0., 1., self._locks.shape[0])

        elif (self._config['landscape'] == 'level'
              or self._config['landscape'] == 'variable'):
            self._locks = np.ones(
                (self._config['num_agents'], self._config['num_locks']))
            self._locks *= 0.5

            if self._config['landscape'] == 'variable':
                self._locks[:int(self._locks.shape[0] / 2),
                            0] = np.linspace(0., 1.,
                                             int(self._locks.shape[0] / 2))
                self._locks[:int(self._locks.shape[0] / 2),
                            1] = np.linspace(0., 1.,
                                             int(self._locks.shape[0] / 2))
                self._locks[:int(self._locks.shape[0] / 2),
                            2] = np.linspace(0., 1.,
                                             int(self._locks.shape[0] / 2))

        elif self._config['landscape'] == 'black-white':
            self._locks = np.ones(
                (self._config['num_agents'], self._config['num_locks']))
            half = int(self._locks.shape[0] / 2)
            self._locks[0:half, :] = 0
            self._locks[half:, :] = 1
        else:
            raise ValueError('Unkown landscape config {}'.format(
                self._config['landscape']))

    def set_config(self, key, value):
        if key not in self._config:
            raise ValueError('{} not in config'.format(key))
        self._config[key] = value

    def set_render_lock(self, render_lock):
        self._config['show_lock'] = render_lock

    def set_render_colour(self, render_colour):
        self._config['render_colour'] = render_colour

    def set_render_texture(self, render_texture):
        self._config['render_texture'] = render_texture

    def render(self):
        self._render_data = np.roll(self._render_data, 1, axis=1)

        row_offset = 0
        if self._config['world_d'] == 1:  #  and show_lock:
            row_offset = math.ceil(self._config['num_1d_history'] * 0.02)

        self._agents_to_render_data(row=row_offset)
        self._render_data_to_bitmap(
            render_colour=self._config['render_colour'],
            render_texture=self._config['render_texture'])

        if self._config['world_d'] == 1:
            if self._config['show_lock']:
                self._bitmap[:, 0:row_offset, :] = np.expand_dims(
                    (self._locks[:, 0:3] * 255).astype(np.uint8), axis=1)
            else:
                self._bitmap[:, 0:row_offset, :] = 0

        if (self._config['gui'] == 'pygame' or self._config['save_frames']
                or self._config['gui'] == 'yield_frame'):

            image = self._bitmap_to_image(self._config['display_size'])

            if self._config['gui'] == 'pygame':
                self._display_image(np.array(image), self._display)
            if self._config['save_frames']:
                Image.fromarray(self._bitmap.swapaxes(0, 1)).resize(
                    self._config['display_size']).save(
                        'output_v/loki_frame_t{:09d}.png'.format(self._time))
            if self._config['gui'] == 'yield_frame':
                # import pdb; pdb.set_trace()
                image = image.rotate(-90)
                imgByteArr = io.BytesIO()
                image.save(imgByteArr, format='PNG')
                return imgByteArr.getvalue()

    def step_frame(self):
        self.step()
        return self.render()

    def step(self):
        self._change_locks()
        self._extract_energy()
        self._replication(self._config['map_size'])
        self._gather_data()
        if self._data_logger:
            self._data_logger.add_data(
                [self._data['sigma_history'][-1].mean()])
        self._time += 1

    def _gather_data(self):
        if 'energy_history' not in self._data:
            self._data['energy_history'] = []
        self._data['energy_history'].append(
            self._agent_data['state'][:, State.energy])
        if 'mean_history' not in self._data:
            self._data['mean_history'] = []
        self._data['mean_history'].append(
            self._agent_data['keys'][:, :, Key.mean].mean(axis=0))
        if 'sigma_history' not in self._data:
            self._data['sigma_history'] = []
        self._data['sigma_history'].append(
            self._agent_data['keys'][:, :, Key.sigma].mean(axis=0))
        if 'repo_energy_stats' not in self._data:
            self._data['repo_energy_stats'] = []
        self._data['repo_energy_stats'].append(
            np.array(self._repo_energy_stats).mean())
        self._repo_energy_stats = []
        for _, data_list in self._data.items():
            if len(data_list) > self._data_history_len:
                data_list.pop(0)

    def plot_data(self):
        plt.clf()
        plot_h = 4
        plot_w = 2
        plot_i = 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.plot(np.array(self._data['mean_history'])[:])
        ax.set_title('Mean history')
        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.plot(np.array(self._data['sigma_history'])[:])
        ax.set_title('Sigma history')

        if self._config['num_locks'] == 1:
            plt.tight_layout()
            plt.draw()
            plt.pause(0.0001)
            return

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.plot(self._locks)
        ax.set_title('Lock values')

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.plot(self._extracted_energy)
        ax.set_title('Extracted energy')

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.hist(self._agent_data['state'][:, State.energy], 20)
        ax.set_title('Histogram of agent energies')

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        ax.plot(self._agent_data['keys'][:, :, Key.mean_mut])
        ax.set_title('Key mutability')

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        # Get min and max for lock means.
        m0_min = self._agent_data['keys'][:, :, Key.mean].min(axis=0)
        m0_max = self._agent_data['keys'][:, :, Key.mean].max(axis=0)
        smaller = m0_min < self._locks_metrics[0]
        self._locks_metrics[0][smaller] = m0_min[smaller]
        larger = m0_max > self._locks_metrics[1]
        self._locks_metrics[1][larger] = m0_max[larger]
        ax.scatter(self._agent_data['keys'][:, :, Key.mean][:, 0],
                   self._agent_data['keys'][:, :, Key.mean][:, 1])
        ax.set_xlim(self._locks_metrics[0][0], self._locks_metrics[1][0])
        ax.set_ylim(self._locks_metrics[0][1], self._locks_metrics[1][1])
        ax.set_title('Mean')

        plot_i += 1
        ax = plt.subplot(plot_h, plot_w, plot_i)
        # Get min and max for lock sigmas.
        m0_min = self._agent_data['keys'][:, :, Key.sigma].min(axis=0)
        m0_max = self._agent_data['keys'][:, :, Key.sigma].max(axis=0)
        smaller = m0_min < self._locks_metrics[2]
        self._locks_metrics[2][smaller] = m0_min[smaller]
        larger = m0_max > self._locks_metrics[3]
        self._locks_metrics[3][larger] = m0_max[larger]
        ax.scatter(self._agent_data['keys'][:, :, Key.sigma][:, 0],
                   self._agent_data['keys'][:, :, Key.sigma][:, 1])
        ax.set_xlim(self._locks_metrics[2][0], self._locks_metrics[3][0])
        ax.set_ylim(self._locks_metrics[2][1], self._locks_metrics[3][1])
        ax.set_title('Sigma')

        plt.tight_layout()
        plt.draw()
        plt.pause(0.0001)
        self._locks_metrics *= 0.9

    def _extract_energy(self):
        # env is list of locks
        dist_squared = np.square(self._agent_data['keys'][:, :, Key.mean] -
                                 self._locks)
        sigmas = self._agent_data['keys'][:, :, Key.sigma]
        self._agent_data['keys'][:, :,
                                 Key.energy] = (np.exp(-dist_squared /
                                                       (2 * sigmas * sigmas)) /
                                                (sigmas * sqrt_2_pi))
        if self._config['extraction_method'] == 'max':
            self._extracted_energy = self._agent_data[
                'keys'][:, :, Key.energy].max(
                    axis=1) * self._config['extraction_rate']
        elif self._config['extraction_method'] == 'mean':
            self._extracted_energy = self._agent_data[
                'keys'][:, :, Key.energy].mean(
                    axis=1) * self._config['extraction_rate']
        self._agent_data['state'][:, State.energy] += self._extracted_energy
        # print('Max energy', self._agent_data['state'][:,State.energy].max())

    def _agents_to_render_data(self, row=0):
        # Updata RGB matrix
        new_render_data = np.concatenate(
            (self._agent_data['state']
             [:, [State.red, State.green, State.blue, State.energy]],
             self._agent_data['keys'][:, 0:3, Key.mean]),
            axis=1)
        if new_render_data.shape[0] == self._render_data.shape[0]:
            # new_self._render_data is 1D, i.e. a row
            self._render_data[:, row] = new_render_data
        else:
            # new_render_data is 2D, i.e. data for the whole render_data map.
            self._render_data[:] = new_render_data.reshape(
                self._render_data.shape)

    def _render_data_to_bitmap(self,
                               render_colour='rgb',
                               render_texture='flat'):
        energy = self._render_data[:, :, 3]
        if render_texture == 'energy_up':
            normed_energy = (energy - np.min(energy)) / (np.ptp(energy) +
                                                         0.0001)
        elif render_texture == 'energy_down':
            normed_energy = 1. - (energy - np.min(energy)) / (np.ptp(energy) +
                                                              0.0001)
        else:
            normed_energy = np.ones_like(energy)
        if (normed_energy > 1.0).any():
            print('WARNING normed_energy max {}'.format(normed_energy.max()))
        if (normed_energy < 0.0).any():
            print('WARNING normed_energy min {}'.format(normed_energy.min()))

        # RGB
        colour = self._render_data[:, :, 0:3]
        if render_colour == 'irgb':
            colour = (1 - colour)
        elif render_colour == 'none':
            colour = np.ones_like(colour)
        elif render_colour == 'keys':
            colour = self._render_data[:, :, 4:7]
        if (colour > 1.0).any():
            print('WARNING colour max {}'.format(colour.max()))
        if (colour < 0.0).any():
            print('WARNING colour min {}'.format(colour.min()))

        self._bitmap[:] = (colour * normed_energy[:, :, np.newaxis] *
                           255).astype(np.uint8)

    def _bitmap_to_image(self, display_size):
        return Image.fromarray(self._bitmap).resize(
            (display_size[1], display_size[0]))

    def _display_image(self, image, display):
        surfarray.blit_array(display, image)
        pygame.display.flip()

    def _replication(self, map_size):
        agent_indices, agent_neighbours = init_indices(map_size)
        indices = list(range(len(agent_indices)))
        random.shuffle(indices)
        for index in indices:
            self._replicate_stochastic(agent_indices[index],
                                       agent_neighbours[index])

    def _replicate_stochastic(self, agent_index, neighbours):
        energy = self._agent_data['state'][agent_index, State.energy]
        reproduction_threshold = self._agent_data['state'][
            agent_index, State.repo_threshold]
        if energy >= reproduction_threshold:
            choose = random.sample(range(len(neighbours)), 1)[0]
            neighbour_idx = neighbours[choose]

            # No regard for neighour's energy
            # if np.random.uniform() < energy / 10:
            #   self._make_offspring(self._agent_data, agent_index, neighbour_idx)

            # No chance if zero energy, 50:50 if matched, and 100% if double.
            opponent_energy = self._agent_data['state'][neighbour_idx,
                                                        State.energy]
            chance = energy / (opponent_energy + 0.00001)
            if chance > 2:
                chance = 2
            if np.random.uniform() < chance / 2:
                self._repo_energy_stats.append(energy)
                self._make_offspring(agent_index, neighbour_idx)

    def _make_offspring(self, agent_index, target_index):
        self._agent_data['state'][agent_index, State.energy] /= 2
        self._agent_data['keys'][target_index, :] = self._agent_data['keys'][
            agent_index, :]
        self._agent_data['state'][target_index, :] = self._agent_data['state'][
            agent_index, :]
        self._mutate_agent(target_index)

    def _mutate_agent(self, target_index):
        # Now locks are expected to be 0 <= x <= 1
        lower = 0.0
        higher = 1.0

        check_array(self._agent_data['keys'][target_index, :, Key.mean])
        mutate_array(self._agent_data['keys'][target_index, :, Key.mean],
                     self._agent_data['keys'][target_index, :, Key.mean_mut],
                     lower=lower,
                     higher=higher,
                     reflect=True,
                     dist='cauchy')
        check_array(self._agent_data['keys'][target_index, :, Key.mean])

        mutate_array(self._agent_data['keys'][target_index, :, Key.sigma],
                     self._agent_data['keys'][target_index, :, Key.sigma_mut],
                     lower=0.1,
                     dist='cauchy')
        # Turn off mutation of mutation rates
        mutate_array(self._agent_data['keys'][target_index, :, Key.mean_mut],
                     self._mutation_of_mean_mutation_rate,
                     lower=0.0,
                     higher=1.0,
                     reflect=True,
                     dist='cauchy')
        mutate_array(self._agent_data['keys'][target_index, :, Key.sigma_mut],
                     self._mutation_of_sigma_mutation_rate,
                     lower=0.0,
                     higher=1.0,
                     reflect=True,
                     dist='cauchy')
        self._agent_data['state'][
            target_index, State.repo_threshold] = mutate_value(
                self._agent_data['state'][target_index, State.repo_threshold],
                self._agent_data['state'][target_index,
                                          State.repo_threshold_mut],
                lower=0.0,
                dist='cauchy')
        mutate_array(
            self._agent_data['state'][target_index,
                                      State._colour_start:State._colour_end],
            0.01,
            lower=0.0,
            higher=1.0,
            reflect=True)

    def _change_locks(self, force=False):
        changed = False
        lock_mutability = np.ones(
            self._locks.shape[1]) * self._config['lock_mutation_level']

        intensity = 0.1
        roughness = 4
        if force:
            intesity = 1.0
            roughness = 4
        window_len = int(self._config['map_size'][0] / roughness)
        left_off = math.ceil((window_len - 1) / 2)
        right_off = math.ceil((window_len - 2) / 2)
        w = np.ones(window_len, 'd')

        for i in range(self._locks.shape[1]):
            if np.random.uniform() < lock_mutability[i] or force:
                # self._locks[0] = np.random.uniform(-1,1) * 5
                # self._locks[:,i] += np.random.uniform(-0.1, 0.1,
                #         size=(self._locks.shape[0],))

                # Slowly evolving lock with -n < 0.1
                change = np.random.normal(
                    size=(self._locks.shape[0], )) * intensity
                s = np.r_[change[window_len - 1:0:-1], change,
                          change[-2:-window_len - 1:-1]]
                change = np.convolve(w / w.sum(), s,
                                     mode='valid')[left_off:-right_off]
                self._locks[:, i] += change
                # self._locks[0] += np.random.standard_cauchy() * 5

                self._locks[:, i] = np.clip(self._locks[:, i], 0., 1.)
                changed = True
def main():
    # display detected goal and distance from it
    # in a live window
    parser = setup_options_parser()
    args = parser.parse_args()
    replaying = args.replay_dir is not None
    recording = args.record
    pclviewer = None
    if replaying:
        replayer = Replayer(args.replay_dir)
        mode = "stopped"
        top = 120
        left = 160
        cv2.namedWindow("View")
        cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
        cv2.createTrackbar("area_threshold", "View", 10, 500,
                        lambda *args: None)
        cv2.createTrackbar("frame", "View", 0, len(replayer.frame_names), lambda *args: None)
        with Vision(use_sensor=False) as vision:
            while True:
                vision.mode = cv2.getTrackbarPos("mode", "View")
                vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
                _frame_i = cv2.getTrackbarPos("frame", "View")
                if 0 <= _frame_i < len(replayer.frame_names):
                    frame_i = _frame_i
                vision.get_recorded_depths(replayer, frame_i)
                vision.idepth_stats()
                vision.set_display()
                if mode == "stopped" and vision.mode == 4:
                    cv2.rectangle(
                        vision.display, (left, top), (left + 10, top+10), 
                        (255, 0, 0))

                if mode != "stopped" and pclviewer is not None:
                    pclviewer.updateCloud(vision.xyz)


                cv2.imshow("View", vision.display)
                wait_delay = 50
                if mode == "fw" and frame_i < len(replayer.frame_names) - 1:
                    cv2.setTrackbarPos("frame", "View", frame_i+1)
                    wait_delay = replayer.offset_milis(frame_i)
                elif mode == "bw" and 0 < frame_i:
                    cv2.setTrackbarPos("frame", "View", frame_i-1)
                    wait_delay = replayer.offset_milis(frame_i-1)
                x = cv2.waitKey(wait_delay)
                if x % 128 == 27:
                    break
                elif ord('0') <= x <= ord('7'):
                    cv2.setTrackbarPos("mode", "View", x - ord('0'))
                elif ord('`') == x:
                    cv2.setTrackbarPos("mode", "View", 0)
                elif ord('s') == x:
                    mode = "stopped"
                elif ord('f') == x:
                    mode = 'fw'
                elif ord('b') == x:
                    mode = 'bw'
                elif ord('p') == x:
                    print(replayer.file_name(frame_i))
                elif ord('i') == x:
                    cv2.imwrite("plop.jpg", vision.display);
                elif ord('z') == x and libpclproc is not None:
                    if pclviewer is None:
                        pclviewer = libpclproc.process(vision.xyz)
                    else:
                        pclviewer.close()
                        pclviewer = None
                if pclviewer is not None and not pclviewer.wasStopped():
                    pclviewer.spin()

                if mode == "stopped" and vision.mode == 4:
                    if x == 65361:
                        # left arrow key
                        left -= 2
                    elif x == 65362:
                        # up arrow key
                        top -= 2
                    elif x == 65363:
                        # right arrow key
                        left += 2
                    elif x == 65364:
                        # down arrow key
                        top += 2
                    elif x == ord('p'):
                        print('x: ', vision.xyz[0, top:top+10, left:left+10])
                        print('y: ', vision.xyz[1, top:top+10, left:left+10])
                        print('z: ', vision.xyz[2, top:top+10, left:left+10])
            cv2.destroyWindow("View")
    else:
        logger = DataLogger("logs")
        if recording:
            logger.begin_logging()
        cv2.namedWindow("View")
        cv2.createTrackbar("mode", "View", 0, 7, lambda *args: None)
        '''
        cv2.createTrackbar("area_threshold", "View", 10, 500,
                        lambda *args: None)
        '''
        cv2.createTrackbar("angle", "View", 0, 90,
                        lambda *args: None)
        cv2.createTrackbar("velocity", "View", 1000, 10000,
                        lambda *args: None)
        with Vision() as vision:
            while True:
                vision.mode = cv2.getTrackbarPos("mode", "View")
                #vision.area_threshold = cv2.getTrackbarPos("area_threshold", "View")
                vision.angle = cv2.getTrackbarPos("angle", "View")
                vision.get_depths()
                vision.idepth_stats()
                vision.set_display()
                logger.log_data(vision.depth, vision.ir)
                cv2.imshow("View", vision.display)
                x = cv2.waitKey(50)
                if x % 128 == 27:
                    break
                elif ord('0') <= x <= ord('7'):
                    cv2.setTrackbarPos("mode", "View", x - ord('0'))
                elif ord('`') == x:
                    cv2.setTrackbarPos("mode", "View", 0)
            cv2.destroyWindow("View")
Exemple #34
0
LogManager.setupLogging("climate-station")
log = logging.getLogger('climate-station')
log.info('Hello world.')

sensor = Sensor(i2c(1, Sensor.BASE_ADDRESS))

poller = DevicePoller(1)
poller.add_device("sensor", sensor)

display = LedDisplayController(i2c(1, LedDisplayController.BASE_ADDRESS))
display.turnOnOscillator()
display.turnOnDisplay()
display.setDimming(0)

presenter = Presenter(2, poller, display)

dataLogger = DataLogger(60, poller)

try:
    with IdleLoop() as idle:
        idle.setTicksPerSecond(1)
        idle.register(dataLogger)
        idle.register(presenter)
        idle.run()

except:
    e = sys.exc_info()[0]
    log.exception(e)
    print('Caught exception: {0}'.format(e))