From 52de9864646b18b364fea06561bfe283f835f217 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 28 Mar 2012 19:34:17 +0200 Subject: digital/asserv, digital/mimot: use human readable parameters --- digital/asserv/tools/asserv/asserv.py | 53 ++++++++++++--------- digital/asserv/tools/asserv/init.py | 86 +++++++++++++++++------------------ digital/asserv/tools/step.py | 2 +- digital/mimot/tools/mimot/init.py | 28 ++++++------ digital/mimot/tools/mimot/mimot.py | 45 ++++++++++-------- 5 files changed, 115 insertions(+), 99 deletions(-) (limited to 'digital') diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 30638831..f96158b7 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -106,19 +106,22 @@ class Proto: self._index['a%d' % i] = i self.param = dict ( scale = 1, - E = 1023, I = 1023, D = 1023, - c = 1, f = 0x1000, - l = 0x2000, - w = 0x00, + e_sat = 1023, i_sat = 1023, d_sat = 1023, + encoder_right_correction = 1, footing = 0x1000, + angle_limit = 0x2000, + l_reverse = False, r_reverse = False, ) mparam = dict ( kp = 0, ki = 0, kd = 0, - a = 1, sm = 0, ss = 0, - be = 2048, bs = 0x10, bc = 20, + acc = 1, speed_max = 0, speed_slow = 0, + bd_error_limit = 2048, bd_speed_limit = 0x10, + bd_counter_limit = 20, ) for m in self._index: for k in mparam: - self.param[m + k] = mparam[k] + self.param[m + '_' + k] = mparam[k] + for i in xrange (aux_nb): + self.param['a%d_reverse' % i] = False self.param.update (param) self.send_param () @@ -302,20 +305,28 @@ class Proto: return int (round (x * (1 << 24))) for m in self._index: index = self._index [m] - self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp'])) - self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki'])) - self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + 'kd'])) - self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + 'a'])) - self.proto.send ('p', 'cBBB', 's', index, p[m + 'sm'], p[m + 'ss']) - self.proto.send ('p', 'cBHHB', 'b', index, p[m + 'be'], - p[m + 'bs'], p[m + 'bc']) - self.proto.send ('p', 'cH', 'E', p['E']) - self.proto.send ('p', 'cH', 'I', p['I']) - self.proto.send ('p', 'cH', 'D', p['D']) - self.proto.send ('p', 'cL', 'c', f824 (p['c'])) - self.proto.send ('p', 'cH', 'f', p['f']) - self.proto.send ('p', 'cH', 'l', p['l']) - self.proto.send ('p', 'cB', 'w', p['w']) + self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + '_kp'])) + self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + '_ki'])) + self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + '_kd'])) + self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + '_acc'])) + self.proto.send ('p', 'cBBB', 's', index, p[m + '_speed_max'], + p[m + '_speed_slow']) + self.proto.send ('p', 'cBHHB', 'b', index, + p[m + '_bd_error_limit'], p[m + '_bd_speed_limit'], + p[m + '_bd_counter_limit']) + self.proto.send ('p', 'cH', 'E', p['e_sat']) + self.proto.send ('p', 'cH', 'I', p['i_sat']) + self.proto.send ('p', 'cH', 'D', p['d_sat']) + self.proto.send ('p', 'cL', 'c', f824 (p['encoder_right_correction'])) + self.proto.send ('p', 'cH', 'f', p['footing']) + self.proto.send ('p', 'cH', 'l', p['angle_limit']) + output_index = [ 'l', 'r' ] + [ 'a%d' % i + for i in xrange (self.aux_nb) ] + reverse = 0 + for i, m in enumerate (output_index): + if p[m + '_reverse']: + reverse |= 1 << i + self.proto.send ('p', 'cB', 'w', reverse) def write_eeprom (self): """Request an EEPROM write.""" diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index c845af03..5b852d71 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -1,25 +1,25 @@ """Default parameters for asserv.""" host_marcel = dict ( - scale = 0.0395840674352314, f = 0xdd1, - tkp = 1, tkd = 16, - ta = 0.75, tsm = 0x20, tss = 0x10, - akp = 2, akd = 16, - aa = 0.25, asm = 0x20, ass = 0x10, - a0kp = 4, - a0a = 4, a0sm = 0x60, a0ss = 0x10, - a1kp = 2, - a1a = 16, a1sm = 0x46, a1ss = 0x10, - E = 0x3ff, D = 0x1ff, - l = 0x1000, + scale = 0.0395840674352314, footing = 0xdd1, + t_kp = 1, t_kd = 16, + t_acc = 0.75, t_speed_max = 0x20, t_speed_slow = 0x10, + a_kp = 2, a_kd = 16, + a_acc = 0.25, a_speed_max = 0x20, a_speed_slow = 0x10, + a0_kp = 4, + a0_acc = 4, a0_speed_max = 0x60, a0_speed_slow = 0x10, + a1_kp = 2, + a1_acc = 16, a1_speed_max = 0x46, a1_speed_slow = 0x10, + e_sat = 0x3ff, d_sat = 0x1ff, + angle_limit = 0x1000, ) host_robospierre = dict ( - scale = 0.0395840674352314, f = 0xdd1, - tkp = 1, tkd = 16, - ta = 0.75, tsm = 0x60, tss = 0x10, - akp = 2, akd = 16, - aa = 0.25, asm = 0x60, ass = 0x10, - E = 0x3ff, D = 0x1ff, - l = 0x1000, + scale = 0.0395840674352314, footing = 0xdd1, + t_kp = 1, t_kd = 16, + t_acc = 0.75, t_speed_max = 0x60, t_speed_slow = 0x10, + a_kp = 2, a_kd = 16, + a_acc = 0.25, a_speed_max = 0x60, a_speed_slow = 0x10, + e_sat = 0x3ff, d_sat = 0x1ff, + angle_limit = 0x1000, ) host = { 'giboulee': host_marcel, @@ -27,34 +27,32 @@ host = { 'robospierre': host_robospierre, } target_marcel = dict ( - scale = 0.0415178942124, f = 0xcef, - c = float (0x00ffbabf) / (1 << 24), - tkp = 1, tkd = 16, - ta = 0.75, tsm = 0x20, tss = 0x10, - akp = 2, akd = 16, - aa = 0.25, asm = 0x20, ass = 0x10, - a0kp = 4, a0kd = 16, - a0a = 4, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x04, a0bc = 10, - a1kp = 2, - a1a = 16, a1sm = 0x46, a1ss = 0x10, - a1be = 256, a1bs = 0x0c, a1bc = 10, - E = 0x3ff, D = 0x1ff, - l = 0x1000, - w = 0x09, + scale = 0.0415178942124, footing = 0xcef, + encoder_right_correction = float (0x00ffbabf) / (1 << 24), + t_kp = 1, t_kd = 16, + t_acc = 0.75, t_speed_max = 0x20, t_speed_slow = 0x10, + a_kp = 2, a_kd = 16, + a_acc = 0.25, a_speed_max = 0x20, a_speed_slow = 0x10, + a0_kp = 4, a0_kd = 16, + a0_acc = 4, a0_speed_max = 0x60, a0_speed_slow = 0x10, + a0_bd_error_limit = 256, a0_bd_speed_limit = 0x04, a0_bd_counter_limit = 10, + a1_kp = 2, + a1_acc = 16, a1_speed_max = 0x46, a1_speed_slow = 0x10, + a1_bd_error_limit = 256, a1_bd_speed_limit = 0x0c, a1_bd_counter_limit = 10, + e_sat = 0x3ff, d_sat = 0x1ff, + angle_limit = 0x1000, ) target_robospierre = dict ( - scale = 0.0317975134344, f = 0x134e, - c = float (0xffa897) / (1 << 24), - tkp = 1, tkd = 16, - ta = 0.75, tsm = 0x60, tss = 0x10, - tbe = 256, tbs = 0x08, tbc = 40, - akp = 2, akd = 16, - aa = 0.5, asm = 0x60, ass = 0x10, - abe = 128, abs = 0x08, abc = 40, - E = 0x3ff, D = 0x1ff, - l = 0x1000, - w = 0x00, + scale = 0.0317975134344, footing = 0x134e, + encoder_right_correction = float (0xffa897) / (1 << 24), + t_kp = 1, t_kd = 16, + t_acc = 0.75, t_speed_max = 0x60, t_speed_slow = 0x10, + t_bd_error_limit = 256, t_bd_speed_limit = 0x08, t_bd_counter_limit = 40, + a_kp = 2, a_kd = 16, + a_acc = 0.5, a_speed_max = 0x60, a_speed_slow = 0x10, + a_bd_error_limit = 128, a_bd_speed_limit = 0x08, a_bd_counter_limit = 40, + e_sat = 0x3ff, d_sat = 0x1ff, + angle_limit = 0x1000, ) target = { 'giboulee': target_marcel, diff --git a/digital/asserv/tools/step.py b/digital/asserv/tools/step.py index b9f798ef..4eb78379 100644 --- a/digital/asserv/tools/step.py +++ b/digital/asserv/tools/step.py @@ -4,7 +4,7 @@ import asserv from utils.init_proto import init_proto def step (name, offset, kp, ki, kd, plots, **param): - p = { name + 'kp': kp, name + 'ki': ki, name + 'kd': kd} + p = { name + '_kp': kp, name + '_ki': ki, name + '_kd': kd} p.update (param) a = init_proto (None, asserv.Proto, init = p) a.stats (*plots) diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index 87e00642..5dbe1a87 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -1,21 +1,21 @@ """Default parameters for asserv.""" target_marcel = dict ( - a0kp = 4, - a0a = 16, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, - a1kp = 4, - a1a = 16, a1sm = 0x60, a1ss = 0x10, - a1be = 256, a1bs = 0x18, a1bc = 5, - E = 0x3ff, D = 0x1ff, + a0_kp = 4, + a0_acc = 16, a0_speed_max = 0x60, a0_speed_slow = 0x10, + a0_error_limit = 256, a0_bd_speed_limit = 0x18, a0_bd_counter_limit = 5, + a1_kp = 4, + a1_acc = 16, a1_speed_max = 0x60, a1_speed_slow = 0x10, + a1_error_limit = 256, a1_bd_speed_limit = 0x18, a1_bd_counter_limit = 5, + e_sat = 0x3ff, d_sat = 0x1ff, ) target_robospierre = dict ( - a0kp = 8, a0kd = 1, - a0a = 2, a0sm = 0x60, a0ss = 0x10, - a0be = 32, a0bs = 0x08, a0bc = 125, - a1kp = 4, - a1a = 0.5, a1sm = 0x30, a1ss = 0x08, - a1be = 64, a1bs = 0x08, a1bc = 5, - E = 0x3ff, D = 0x1ff, + a0_kp = 8, a0_kd = 1, + a0_acc = 2, a0_speed_max = 0x60, a0_speed_slow = 0x10, + a0_error_limit = 32, a0_bd_speed_limit = 0x08, a0_bd_counter_limit = 125, + a1_kp = 4, + a1_acc = 0.5, a1_speed_max = 0x30, a1_speed_slow = 0x08, + a1_error_limit = 64, a1_bd_speed_limit = 0x08, a1_bd_counter_limit = 5, + e_sat = 0x3ff, d_sat = 0x1ff, ) target = { 'marcel': target_marcel, diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py index 013a85ff..98101eb8 100644 --- a/digital/mimot/tools/mimot/mimot.py +++ b/digital/mimot/tools/mimot/mimot.py @@ -71,14 +71,15 @@ class Proto: self.proto.register (s, f, make_handle (s)) self.stats_enabled = None self.param = dict ( - a0kp = 0, a0ki = 0, a0kd = 0, - a0a = 1, a0sm = 0, a0ss = 0, - a0be = 2048, a0bs = 0x10, a0bc = 20, - a1kp = 0, a1ki = 0, a1kd = 0, - a1a = 1, a1sm = 0, a1ss = 0, - a1be = 2048, a1bs = 0x10, a1bc = 20, - E = 1023, I = 1023, D = 1023, - w = 0x00, + a0_kp = 0, a0_ki = 0, a0_kd = 0, + a0_acc = 1, a0_speed_max = 0, a0_speed_slow = 0, + a0_bd_error_limit = 2048, a0_bd_speed_limit = 0x10, a0_bd_counter_limit = 20, + a0_reverse = False, + a1_kp = 0, a1_ki = 0, a1_kd = 0, + a1_acc = 1, a1_speed_max = 0, a1_speed_slow = 0, + a1_bd_error_limit = 2048, a1_bd_speed_limit = 0x10, a1_bd_counter_limit = 20, + a1_reverse = False, + e_sat = 1023, i_sat = 1023, d_sat = 1023, ) self.param.update (param) self.send_param () @@ -169,17 +170,23 @@ class Proto: return int (round (x * (1 << 8))) for m in ('a0', 'a1'): index = self._index [m] - self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp'])) - self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki'])) - self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + 'kd'])) - self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + 'a'])) - self.proto.send ('p', 'cBBB', 's', index, p[m + 'sm'], p[m + 'ss']) - self.proto.send ('p', 'cBHHB', 'b', index, p[m + 'be'], - p[m + 'bs'], p[m + 'bc']) - self.proto.send ('p', 'cH', 'E', p['E']) - self.proto.send ('p', 'cH', 'I', p['I']) - self.proto.send ('p', 'cH', 'D', p['D']) - self.proto.send ('p', 'cB', 'w', p['w']) + self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + '_kp'])) + self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + '_ki'])) + self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + '_kd'])) + self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + '_acc'])) + self.proto.send ('p', 'cBBB', 's', index, p[m + '_speed_max'], + p[m + '_speed_slow']) + self.proto.send ('p', 'cBHHB', 'b', index, + p[m + '_bd_error_limit'], p[m + '_bd_speed_limit'], + p[m + '_bd_counter_limit']) + self.proto.send ('p', 'cH', 'E', p['e_sat']) + self.proto.send ('p', 'cH', 'I', p['i_sat']) + self.proto.send ('p', 'cH', 'D', p['d_sat']) + reverse = 0 + for i, m in enumerate (('a0', 'a1')): + if p[m + '_reverse']: + reverse |= 1 << i + self.proto.send ('p', 'cB', 'w', reverse) def write_eeprom (self): """Request an EEPROM write.""" -- cgit v1.2.3