Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Features/prince #206

Merged
merged 6 commits into from
Oct 7, 2023
Merged

Features/prince #206

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4cb13f6
s3 init
aedsinger Sep 5, 2023
8effe11
add s3 params
aedsinger Sep 6, 2023
85ca5af
Stepper updated for P4 and battery voltage
aedsinger Sep 7, 2023
67c64a5
move to SE3 name
aedsinger Sep 11, 2023
526dcc2
imu wip
aedsinger Oct 6, 2023
4b28b9b
finalize P4 firmware
aedsinger Oct 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
191 changes: 116 additions & 75 deletions body/stretch_body/pimu.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,58 +56,45 @@ def pretty_print(self):
print('Timestamp (s)', self.status['timestamp'])
print('-----------------------')

def unpack_status(self, s):
raise NotImplementedError('This method not supported by IMU for firmware.')
def unpack_status(self, s, unpack_to=None):
if unpack_to is None:
unpack_to = self.status
sidx = 0
unpack_to['ax'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['ay'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['az'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['gx'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['gy'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['gz'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['mx'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['my'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['mz'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
unpack_to['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
unpack_to['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
unpack_to['qw'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['qx'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['qy'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['qz'] = unpack_float_t(s[sidx:]);sidx += 4
unpack_to['bump'] = unpack_float_t(s[sidx:]);sidx += 4
return sidx


# ######################## IMU PROTOCOL P0 #################################
class IMU_Protocol_P0(IMUBase):
def unpack_status(self, s):
# take in an array of bytes
# this needs to exactly match the C struct format
sidx=0
self.status['ax']= unpack_float_t(s[sidx:]);sidx += 4
self.status['ay'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['az'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gy'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['mx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['my'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['mz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['qw'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qy'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['bump'] = unpack_float_t(s[sidx:]);sidx += 4
def unpack_status(self, s, unpack_to=None):
if unpack_to is None:
unpack_to = self.status
sidx = IMUBase.unpack_status(self, s, unpack_to)
self.status['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:]));sidx += 4
return sidx

# ######################## IMU PROTOCOL P1 #################################
class IMU_Protocol_P1(IMUBase):
def unpack_status(self, s):
# take in an array of bytes
# this needs to exactly match the C struct format
sidx=0
self.status['ax']= unpack_float_t(s[sidx:]);sidx += 4
self.status['ay'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['az'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gy'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['gz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['mx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['my'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['mz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['roll'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['pitch'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['heading'] = deg_to_rad(unpack_float_t(s[sidx:]));sidx += 4
self.status['qw'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qx'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qy'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['qz'] = unpack_float_t(s[sidx:]);sidx += 4
self.status['bump'] = unpack_float_t(s[sidx:]);sidx += 4
def unpack_status(self, s, unpack_to=None):
if unpack_to is None:
unpack_to = self.status
sidx = IMUBase.unpack_status(self, s, unpack_to)
return sidx

# ######################## IMU #################################
Expand All @@ -116,7 +103,8 @@ def __init__(self):
IMUBase.__init__(self)
# Order in descending order so more recent protocols/methods override less recent
self.supported_protocols = {'p0': (IMU_Protocol_P0,), 'p1': (IMU_Protocol_P1,IMU_Protocol_P0,),
'p2': (IMU_Protocol_P1,IMU_Protocol_P0,),'p3': (IMU_Protocol_P1,IMU_Protocol_P0,)}
'p2': (IMU_Protocol_P1,IMU_Protocol_P0,),'p3': (IMU_Protocol_P1,IMU_Protocol_P0,),
'p4': (IMU_Protocol_P1,IMU_Protocol_P0,)}

# ##################################################################################
class PimuBase(Device):
Expand Down Expand Up @@ -193,7 +181,7 @@ def __init__(self, event_reset=False, usb=None):
'cliff_event': False, 'fan_on': False, 'buzzer_on': False, 'low_voltage_alert':False,'high_current_alert':False,'over_tilt_alert':False,
'charger_connected':False, 'boot_detected':False,'imu': self.imu.status,'debug':0,'state':0,'trace_on':0,
'motor_sync_rate': 0, 'motor_sync_cnt': 0, 'motor_sync_queues': 0, 'motor_sync_drop': 0,
'transport': self.transport.status}
'transport': self.transport.status, 'current_charge':0}

self.status_zero=self.status.copy()
self.status_aux = {'foo': 0}
Expand Down Expand Up @@ -294,6 +282,8 @@ def pretty_print(self):
print('------ Pimu -----')
print('Voltage',self.status['voltage'])
print('Current', self.status['current'])
if self.board_info['hardware_id']>=3:
print('Current Charge',self.status['current_charge'])
print('CPU Temp',self.status['cpu_temp'])
print('Board Temp', self.status['temp'])
print('State', self.status['state'])
Expand Down Expand Up @@ -422,10 +412,36 @@ def get_temp(self,raw):
return C

def get_current(self,raw):
if self.board_info['hardware_id']>=3:
return self.get_current_efuse(raw)
else:
return self.get_current_shunt(raw)
def get_current_shunt(self,raw):
"""
RE1 / RE2 Pimus using shunt resistor for current measurement
"""
raw_to_mV = 3300 / 1024.0
mV = raw * raw_to_mV
mA = mV/.408 # conversion per circuit
return mA/1000.0

def get_current_efuse(self,raw):
"""
S3 Pimu's using Efuse current measurement
"""
raw_to_mV = 3300 / 1024.0
mV = raw * raw_to_mV
mA = mV/.224 # conversion per circuit
return mA/1000.0

def get_current_charge(self,raw):
"""
S3 Pimu's shunt measurement of charger current
"""
raw_to_mV = 3300 / 1024.0
mV = raw * raw_to_mV
mA = mV/.215 # conversion per circuit
return mA/1000.0
# ################Data Packing #####################

def unpack_board_info(self,s):
Expand Down Expand Up @@ -476,7 +492,7 @@ def pack_trigger(self,s,sidx):
pack_uint32_t(s,sidx,self._trigger); sidx+=4
return sidx

def unpack_status(self,s):
def unpack_status(self,s, unpack_to=None):
raise NotImplementedError('This method not supported for firmware on protocol {0}.'
.format(self.board_info['protocol_version']))

Expand Down Expand Up @@ -579,38 +595,41 @@ def step_sentry(self,robot=None):

# ######################## PIMU PROTOCOL PO #################################


class Pimu_Protocol_P0(PimuBase):
def unpack_status(self,s):
def unpack_status(self,s, unpack_to=None):
if unpack_to is None:
unpack_to = self.status
sidx=0
sidx +=self.imu.unpack_status((s[sidx:]))
self.status['voltage']=self.get_voltage(unpack_float_t(s[sidx:]));sidx+=4
self.status['current'] = self.get_current(unpack_float_t(s[sidx:]));sidx+=4
self.status['temp'] = self.get_temp(unpack_float_t(s[sidx:]));sidx+=4
unpack_to['voltage']=self.get_voltage(unpack_float_t(s[sidx:]));sidx+=4
unpack_to['current'] = self.get_current(unpack_float_t(s[sidx:]));sidx+=4
unpack_to['temp'] = self.get_temp(unpack_float_t(s[sidx:]));sidx+=4

for i in range(4):
self.status['cliff_range'][i]=unpack_float_t(s[sidx:])
unpack_to['cliff_range'][i]=unpack_float_t(s[sidx:])
sidx+=4

self.status['state'] = unpack_uint32_t(s[sidx:])
unpack_to['state'] = unpack_uint32_t(s[sidx:])
sidx += 4

self.status['at_cliff']=[]
self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_0) != 0)
self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_1) != 0)
self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_2) != 0)
self.status['at_cliff'].append((self.status['state'] & self.STATE_AT_CLIFF_3) != 0)
self.status['runstop_event'] = (self.status['state'] & self.STATE_RUNSTOP_EVENT) != 0
self.status['cliff_event'] = (self.status['state'] & self.STATE_CLIFF_EVENT) != 0
self.status['fan_on'] = (self.status['state'] & self.STATE_FAN_ON) != 0
self.status['buzzer_on'] = (self.status['state'] & self.STATE_BUZZER_ON) != 0
self.status['low_voltage_alert'] = (self.status['state'] & self.STATE_LOW_VOLTAGE_ALERT) != 0
self.status['high_current_alert'] = (self.status['state'] & self.STATE_HIGH_CURRENT_ALERT) != 0
self.status['over_tilt_alert'] = (self.status['state'] & self.STATE_OVER_TILT_ALERT) != 0
self.status['charger_connected'] = (self.status['state'] & self.STATE_CHARGER_CONNECTED) != 0
self.status['boot_detected'] = (self.status['state'] & self.STATE_BOOT_DETECTED) != 0
self.status['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:])); sidx += 4
self.status['bump_event_cnt'] = unpack_uint16_t(s[sidx:]);sidx += 2
self.status['debug'] = unpack_float_t(s[sidx:]); sidx += 4
unpack_to['at_cliff']=[]
unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_0) != 0)
unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_1) != 0)
unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_2) != 0)
unpack_to['at_cliff'].append((unpack_to['state'] & self.STATE_AT_CLIFF_3) != 0)
unpack_to['runstop_event'] = (unpack_to['state'] & self.STATE_RUNSTOP_EVENT) != 0
unpack_to['cliff_event'] = (unpack_to['state'] & self.STATE_CLIFF_EVENT) != 0
unpack_to['fan_on'] = (unpack_to['state'] & self.STATE_FAN_ON) != 0
unpack_to['buzzer_on'] = (unpack_to['state'] & self.STATE_BUZZER_ON) != 0
unpack_to['low_voltage_alert'] = (unpack_to['state'] & self.STATE_LOW_VOLTAGE_ALERT) != 0
unpack_to['high_current_alert'] = (unpack_to['state'] & self.STATE_HIGH_CURRENT_ALERT) != 0
unpack_to['over_tilt_alert'] = (unpack_to['state'] & self.STATE_OVER_TILT_ALERT) != 0
unpack_to['charger_connected'] = (unpack_to['state'] & self.STATE_CHARGER_CONNECTED) != 0
unpack_to['boot_detected'] = (unpack_to['state'] & self.STATE_BOOT_DETECTED) != 0
unpack_to['timestamp'] = self.timestamp.set(unpack_uint32_t(s[sidx:])); sidx += 4
unpack_to['bump_event_cnt'] = unpack_uint16_t(s[sidx:]);sidx += 2
unpack_to['debug'] = unpack_float_t(s[sidx:]); sidx += 4
return sidx

# ######################## PIMU PROTOCOL P1 #################################
Expand Down Expand Up @@ -655,8 +674,6 @@ def unpack_status(self, s, unpack_to=None):





# ######################## PIMU PROTOCOL P2 #################################
class Pimu_Protocol_P2(PimuBase):

Expand Down Expand Up @@ -713,7 +730,7 @@ def disable_firmware_trace(self):
self._trigger = self._trigger | self.TRIGGER_DISABLE_TRACE
self._dirty_trigger = True

def unpack_status(self, s, unpack_to=None):
def unpack_status(self, s, unpack_to=None): #P2
if unpack_to is None:
unpack_to = self.status

Expand Down Expand Up @@ -752,7 +769,7 @@ def unpack_status(self, s, unpack_to=None):
unpack_to['debug'] = unpack_float_t(s[sidx:]); sidx += 4
return sidx

# ######################## PIMU PROTOCOL P2 #################################
# ######################## PIMU PROTOCOL P3 #################################
class Pimu_Protocol_P3(PimuBase):


Expand Down Expand Up @@ -844,6 +861,29 @@ def rpc_load_test_pull_reply(self, reply):
print('Successful load test pull')
else:
print('Error RPC_REPLY_LOAD_TEST_PULL', reply[0])

# ######################## PIMU PROTOCOL P4 #################################
class Pimu_Protocol_P4(PimuBase):
def unpack_status(self, s, unpack_to=None): # P4
if unpack_to is None:
unpack_to = self.status
sidx = 0
sidx = sidx + Pimu_Protocol_P2.unpack_status(self, s, unpack_to)
self.status['current_charge'] = self.get_current_charge(unpack_float_t(s[sidx:]));sidx += 4
# unpack_to['voltage'] = self.get_voltage(unpack_float_t(s[sidx:]));
# sidx += 4
return sidx
# def pack_gains(self, s, sidx):
# sidx = sidx + Stepper_Protocol_P3.pack_gains(self, s, sidx)
# pack_float_t(s, sidx, self.gains['voltage_LPF']);
# sidx += 4
# return sidx
#
# def unpack_gains(self, s):
# sidx = Stepper_Protocol_P3.unpack_gains(self, s)
# self.gains_flash['voltage_LPF'] = unpack_float_t(s[sidx:]);
# sidx += 4
# return sidx
# ######################## PIMU #################################
class Pimu(PimuBase):
"""
Expand All @@ -854,7 +894,8 @@ def __init__(self, event_reset=False, usb=None):
# Order in descending order so more recent protocols/methods override less recent
self.supported_protocols = {'p0': (Pimu_Protocol_P0,), 'p1': (Pimu_Protocol_P1, Pimu_Protocol_P0,),
'p2': (Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,),
'p3': (Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,)}
'p3': (Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,),
'p4': (Pimu_Protocol_P4, Pimu_Protocol_P3, Pimu_Protocol_P2, Pimu_Protocol_P1, Pimu_Protocol_P0,)}

def startup(self, threaded=False):
"""
Expand Down
6 changes: 5 additions & 1 deletion body/stretch_body/robot_params.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,11 @@ class RobotParams:
_user_params = hello_utils.read_fleet_yaml('stretch_user_params.yaml')
_config_params = hello_utils.read_fleet_yaml('stretch_configuration_params.yaml')
_robot_params=nominal_system_params
param_module_name = 'stretch_body.robot_params_'+_config_params['robot']['model_name']
if 'robot' in _user_params and 'model_name' in _user_params['robot']:
param_module_name = 'stretch_body.robot_params_'+_user_params['robot']['model_name']
else:
param_module_name = 'stretch_body.robot_params_' + _config_params['robot']['model_name']

_nominal_params = getattr(importlib.import_module(param_module_name), 'nominal_params')
hello_utils.overwrite_dict(_robot_params, _nominal_params)
for external_params_module in _config_params.get('params', []):
Expand Down
4 changes: 4 additions & 0 deletions body/stretch_body/robot_params_RE1V0.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,7 @@
'pKi_limit': 150,
'pKp_d': 8.0,
'pLPF': 60,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 2.0,
'safety_hold': 0,
Expand Down Expand Up @@ -297,6 +298,7 @@
'pKi_limit': 200,
'pKp_d': 8.0,
'pLPF': 200,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 1.0,
'safety_hold': 0,
Expand Down Expand Up @@ -333,6 +335,7 @@
'pKi_limit': 100.0,
'pKp_d': 10.0,
'pLPF': 60,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 6.0,
'safety_hold': 1,
Expand Down Expand Up @@ -369,6 +372,7 @@
'pKi_limit': 200,
'pKp_d': 8.0,
'pLPF': 200,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 1.0,
'safety_hold': 0,
Expand Down
4 changes: 4 additions & 0 deletions body/stretch_body/robot_params_RE2V0.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,7 @@
'pKi_limit': 150,
'pKp_d': 8.0,
'pLPF': 60,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 2.0,
'safety_hold': 0,
Expand Down Expand Up @@ -294,6 +295,7 @@
'pKi_limit': 200.0,
'pKp_d': 12.0,
'pLPF': 80.0,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 1.0,
'safety_hold': 0,
Expand Down Expand Up @@ -330,6 +332,7 @@
'pKi_limit': 100.0,
'pKp_d': 6.0,
'pLPF': 100,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 6.0,
'safety_hold': 1,
Expand Down Expand Up @@ -366,6 +369,7 @@
'pKi_limit': 200.0,
'pKp_d': 12.0,
'pLPF': 80.0,
'voltage_LPF':1.0,
'phase_advance_d': 1.8,
'pos_near_setpoint_d': 1.0,
'safety_hold': 0,
Expand Down
Loading