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')
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()
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 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 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
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!");
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)
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 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()
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 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 __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
class StateSpaceDriveCommand(_CsvTrajectoryCommand, StateSpaceDriveController):
def __init__(self, fnom):
_CsvTrajectoryCommand.__init__(self, fnom)
StateSpaceDriveController.__init__(self, Command.getRobot().drivetrain)
self.u_min = -8
self.u_max = 8
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.i = 0
self.logger = DataLogger("ss_trajectory.csv")
self.drivetrain.init_logger(self.logger)
def execute(self):
(dt_s, xl_m, xr_m, vl_mps, vr_mps, al_mps2, ar_mps2, heading_rad) = self.get_trajectory_point_m(self.i)
self.update(xl_m, xr_m, vl_mps, vr_mps)
self.logger.log()
self.i += 1
def end(self):
self.drivetrain.off()
self.logger.flush()
class Drivetrain(Subsystem):
''''''
#: encoder/ft ratio
ratio = 886.27
def __init__(self):
super().__init__('Drivetrain')
#The set motor controllers for this years robot and how motors are coded
self.motor_rb = ctre.WPI_TalonSRX(1)
self.motor_rf = ctre.WPI_VictorSPX(17)
self.motor_lb = ctre.WPI_TalonSRX(13)
self.motor_lf = ctre.WPI_VictorSPX(12)
self.motor_rf.follow(self.motor_rb)
self.motor_lf.follow(self.motor_lb)
self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf]
self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb)
self.navx = navx.AHRS.create_spi()
self.pdp = wpilib.PowerDistributionPanel(16)
self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake)
self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.navx_table = networktables.NetworkTables.getTable('/Sensor/Navx')
self.leftEncoder_table = networktables.NetworkTables.getTable("/Encoder/Left")
self.rightEncoder_table = networktables.NetworkTables.getTable("/Encoder/Right")
self.leftError = networktables.NetworkTables.getTable("/TalonL/Error")
self.rightError = networktables.NetworkTables.getTable("/TalonR/Error")
self.motor_lb.setSensorPhase(True)
self.motor_rb.setSensorPhase(True)
self.timer = wpilib.Timer()
self.timer.start()
self.mode = ""
self.computed_velocity = 0
self.logger = None
def dumb_turn(self):
self.drive.arcadeDrive(0, 0.4, False)
def execute_turn(self, angle):
position = angle / 60.
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * position)
self.drive.feed()
def initDefaultCommand(self):
self.setDefaultCommand(Drive())
def init_logger(self):
self.logger = DataLogger('drivetrain.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("heading", lambda: self.navx.getAngle())
self.logger.add("enc_pos_l", lambda: self.motor_lb.getSelectedSensorPosition(0))
self.logger.add("enc_pos_r", lambda: self.motor_rb.getSelectedSensorPosition(0))
self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0))
self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0))
self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0))
self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0))
self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0))
self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0))
self.logger.add("computed_velocity", lambda: self.computed_velocity)
self.logger.add("mode", lambda: self.mode)
self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage())
self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent())
self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent())
self.logger.add("current_rf", lambda: self.pdp.getCurrent(0))
self.logger.add("current_rb", lambda: self.pdp.getCurrent(1))
self.logger.add("current_lf", lambda: self.pdp.getCurrent(15))
self.logger.add("current_lb", lambda: self.pdp.getCurrent(13))
def zeroEncoders(self):
self.motor_rb.setSelectedSensorPosition(0, 0, 0)
self.motor_lb.setSelectedSensorPosition(0, 0, 0)
def zeroNavx(self):
self.navx.reset()
def initialize_driveTurnlike(self):
#The PID values with the motors
self.zeroEncoders()
self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0)
self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(1.25)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(2.5)), 0)
self.motor_rb.configNominalOutputForward(.1, 0)
self.motor_lb.configNominalOutputForward(.1, 0)
self.motor_rb.configNominalOutputReverse(-0.1, 0)
self.motor_lb.configNominalOutputReverse(-0.1, 0)
self.motor_rb.configPeakOutputForward(0.4, 0)
self.motor_lb.configPeakOutputForward(0.4, 0)
self.motor_rb.configPeakOutputReverse(-0.4, 0)
self.motor_lb.configPeakOutputReverse(-0.4, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# self.motor_rb.config_kF(0, 0, 0)
# self.motor_lb.config_kF(0, 0, 0)
self.motor_rb.config_kP(0, 0.18, 0)
self.motor_lb.config_kP(0, 0.18, 0)
self.motor_rb.config_kI(0, 0, 0)
self.motor_lb.config_kI(0, 0, 0)
self.motor_rb.config_kD(0, 8, 0)
self.motor_lb.config_kD(0, 8, 0)
def uninitialize_driveTurnlike(self):
#The PID values with the motors
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
def initilize_driveForward(self):
self.mode = "Forward"
#The PID values with the motors for drive forward
self.zeroEncoders()
self.motor_rb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0)
self.motor_lb.configMotionAcceleration(int(self.getEncoderAccel(5)), 0)
self.motor_rb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0)
self.motor_lb.configMotionCruiseVelocity(int(self.getEncoderVelocity(3.5)), 0)
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
self.motor_lb.setIntegralAccumulator(0, 0, 0)
self.motor_rb.setIntegralAccumulator(0, 0, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
self.motor_rb.config_kF(0, 0, 0)
self.motor_lb.config_kF(0, 0, 0)
self.motor_rb.config_kP(0, 0.18, 0)
self.motor_lb.config_kP(0, 0.18, 0)
self.motor_rb.config_kI(0, 0.001, 0)
self.motor_lb.config_kI(0, 0.001, 0)
self.motor_rb.config_kD(0, 2, 0)
self.motor_lb.config_kD(0, 2, 0)
def initialize_velocity_closedloop(self):
self.motor_rb.configNominalOutputForward(0, 0)
self.motor_lb.configNominalOutputForward(0, 0)
self.motor_rb.configNominalOutputReverse(0, 0)
self.motor_lb.configNominalOutputReverse(0, 0)
self.motor_rb.configPeakOutputForward(1, 0)
self.motor_lb.configPeakOutputForward(1, 0)
self.motor_rb.configPeakOutputReverse(-1, 0)
self.motor_lb.configPeakOutputReverse(-1, 0)
self.motor_rb.selectProfileSlot(0, 0)
self.motor_lb.selectProfileSlot(0, 0)
# tested for counterclockwise turn
self.motor_rb.config_kF(0, 1.88, 0)
self.motor_rb.config_kP(0, 4.18, 0)
self.motor_rb.config_kI(0, 0.01, 0)
self.motor_rb.config_kD(0, 450, 0)
self.motor_lb.config_kF(0, 0.88, 0)
self.motor_lb.config_kP(0, 3.18, 0)
self.motor_lb.config_kI(0, 0.01, 0)
self.motor_lb.config_kD(0, 450, 0)
def getAngle(self):
return self.navx.getAngle()
def set_turn_velocity(self, v_degps):
velocity_ratio = 1.6
self.computed_velocity = v_encp100ms = velocity_ratio * v_degps
#self.computed_velocity = v_encp100ms = 32
self.motor_rb.set(ctre.ControlMode.Velocity, v_encp100ms)
self.motor_lb.set(ctre.ControlMode.Velocity, v_encp100ms)
def execute_driveforward(self, positionL, positionR):
self.motor_rb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionR)
self.motor_lb.set(ctre._impl.ControlMode.MotionMagic, self.ratio * positionL)
self.drive.feed()
def isFinished_driveforward(self, target):
sensorPL = self.motor_lb.getSelectedSensorPosition(0)
a1 = (target-0.2)*self.ratio
a2 = (target+0.2)*self.ratio
if a1 < sensorPL < a2:
return True
def end_driveforward(self):
self.motor_rb.set(0)
self.motor_lb.set(0)
self.mode = ""
off = end_driveforward
def getEncoderVelocity(self, fps):
return fps*self.ratio/10
def getEncoderAccel(self, fps2):
return fps2*self.ratio/10
def periodic(self):
#Variables for the Navx
angle = self.navx.getAngle()
self.navx_table.putNumber('Angle', angle)
#Variables used for the dashboard
sensorPL = self.motor_lb.getSelectedSensorPosition(0)
self.leftEncoder_table.putNumber("Position", sensorPL)
sensorPR = self.motor_rb.getSelectedSensorPosition(0)
self.rightEncoder_table.putNumber("Position", sensorPR)
sensorVL = self.motor_lb.getSelectedSensorVelocity(0)
self.leftEncoder_table.putNumber("Velocity", sensorVL)
sensorVR = self.motor_rb.getSelectedSensorVelocity(0)
self.rightEncoder_table.putNumber("Velocity", sensorVR)
voltageL = self.motor_lb.getMotorOutputPercent()
voltageR = self.motor_rb.getMotorOutputPercent()
self.leftError.putNumber("Voltage", voltageL)
self.rightError.putNumber("Voltage", voltageR)
if self.logger is not None:
self.logger.log()
class Intake(Subsystem):
def __init__(self):
super().__init__('Intake')
self.intake_motor_closeOpen = wpilib.VictorSP(8)
self.intake_motor_rightWheel = wpilib.VictorSP(9)
self.intake_motor_leftWheel = wpilib.VictorSP(7)
self.limit_switch = wpilib.DigitalOutput(0)
self.pdp = wpilib.PowerDistributionPanel(16)
self.intake_table = networktables.NetworkTables.getTable('/Intake')
self.timer = wpilib.Timer()
self.timer.start()
self.logger = None
#self.init_logger()
def initDefaultCommand(self):
self.setDefaultCommand(grabber.Grabber())
def closeGrabber(self, x):
self.motor_closeOpen_set(x)
def init_logger(self):
self.logger = DataLogger('intake.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add("voltagep_r",
lambda: self.intake_motor_rightWheel.get())
self.logger.add("voltagep_m",
lambda: self.intake_motor_closeOpen.get())
self.logger.add("voltagep_l",
lambda: self.intake_motor_leftWheel.get())
self.logger.add("voltage", lambda: self.pdp.getVoltage())
self.logger.add("current_r", lambda: self.pdp.getCurrent(0))
self.logger.add("current_m", lambda: self.pdp.getCurrent(1))
self.logger.add("current_l", lambda: self.pdp.getCurrent(15))
def openGrabber(self):
'''
if self.limit_switch == True:
self.grabberOff()
else:'''
self.motor_closeOpen_set(-0.5)
def open2Grabber(self):
self.intake_motor_closeOpen.set(-0.4)
self.cubeOut()
def grabberOff(self):
self.motor_closeOpen_set(0)
def cubeOut(self):
self.intake_motor_rightWheel.set(0.5)
self.intake_motor_leftWheel.set(-0.5)
def cubeIn(self):
self.intake_motor_rightWheel.set(-1)
self.intake_motor_leftWheel.set(1)
def intakeWheelsOff(self):
self.intake_motor_rightWheel.set(0)
self.intake_motor_leftWheel.set(0)
def motor_closeOpen_set(self, voltage_percent):
self.intake_motor_closeOpen.set(voltage_percent)
def motor_rightWheel_set(self, voltage_percent):
self.intake_motor_rightWheel.set(voltage_percent)
def motor_leftWheel_set(self, voltage_percent):
self.intake_motor_leftWheel.set(voltage_percent)
def periodic(self):
self.intake_table.putBoolean("LimitSwitch", self.limit_switch.get())
if self.logger is not None:
self.logger.log()
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)
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
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.i = 0
self.logger = DataLogger("ss_trajectory.csv")
self.drivetrain.init_logger(self.logger)
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)
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("profiled_forward.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos)
self.logger.add("adist_l", lambda: self.profiler_l.adist)
self.logger.add("err_l", lambda: self.profiler_l.err)
self.logger.add("choice_l", lambda: self.profiler_l.choice)
self.logger.add("adist_r", lambda: self.profiler_l.adist)
self.logger.add("err_r", lambda: self.profiler_l.err)
self.logger.add("choice_r", lambda: self.profiler_l.choice)
self.timer.start()
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder(
) / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder(
) / self.drivetrain.ratio
#print ('pdf exec ', self.timer.get())
self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period)
self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period)
self.target_v_l = self.drivetrain.fps_to_encp100ms(
self.profiler_l.current_target_v)
self.target_v_r = self.drivetrain.fps_to_encp100ms(
self.profiler_r.current_target_v)
self.target_a_l = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_l.current_a)
self.target_a_r = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_r.current_a)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
#self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward())
#self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward())
self.logger.log()
self.drivetrain.feed()
def isFinished(self):
return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3
and self.profiler_l.current_target_v == 0
and self.profiler_l.current_a == 0
and self.profiler_r.current_target_v == 0
and self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
self.profiler_l.setCruiseVelocityScale(1 + correction)
self.profiler_r.setCruiseVelocityScale(1 - correction)
class 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]=[]
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()
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
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()
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()
class ElevatorTest2(Command):
def __init__(self):
super().__init__("laksdjfkl")
self.elevator = self.getRobot().elevator
self.requires(self.elevator)
self.timer = wpilib.Timer()
self.done = False
self.state = 1
self.voltage = 0.75
def initialize(self):
self.logger = DataLogger('elevator-majig.csv')
self.logger.add("time", lambda: self.timer.get())
self.logger.add(
"enc_pos1",
lambda: self.elevator.motor.getSelectedSensorVelocity(0))
self.logger.add("current_motor1",
lambda: self.elevator.motor.getOutputCurrent())
self.logger.add("current_follow1",
lambda: self.elevator.other_motor.getOutputCurrent())
self.logger.add("voltage_motor1",
lambda: self.elevator.motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow1",
lambda: self.elevator.other_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor1",
lambda: self.elevator.motor.getBusVoltage())
self.logger.add("bvoltage_follow1",
lambda: self.elevator.other_motor.getBusVoltage())
self.logger.add(
"enc_pos2", lambda: self.elevator.other_right_motor.
getSelectedSensorVelocity(0))
self.logger.add("current_motor2",
lambda: self.elevator.right_motor.getOutputCurrent())
self.logger.add(
"current_follow2",
lambda: self.elevator.other_right_motor.getOutputCurrent())
self.logger.add(
"voltage_motor2",
lambda: self.elevator.right_motor.getMotorOutputVoltage())
self.logger.add(
"voltage_follow2",
lambda: self.elevator.other_right_motor.getMotorOutputVoltage())
self.logger.add("bvoltage_motor2",
lambda: self.elevator.right_motor.getBusVoltage())
self.logger.add(
"bvoltage_follow2",
lambda: self.elevator.other_right_motor.getBusVoltage())
self.timer.start()
def execute(self):
e = self.elevator.getEncoderPosition()
if self.state == 1:
self.elevator.ascend(self.voltage)
if e > 28000:
self.state = 2
self.elevator.descend(self.voltage)
if self.state == 2:
self.elevator.descend(self.voltage)
if e < 1000 and self.state == 2:
self.state = 1
self.voltage += 0.25
elif self.voltage > 1:
self.voltage = 0
self.done = True
self.logger.log()
def isFinished(self):
return self.done
def end(self):
self.elevator.hover()
self.logger.close()
class 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")
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))