Support stepper types in the Firmware Updater tool

python/stretch_factory/firmware_updater.py

@@ -32,6 +32,7 @@ def __init__(self, use_device,args):
         self.args=args
         state_from_yaml = self.from_yaml()
         self.home_dir = os.path.expanduser('~')
+        self.stepper_type = None
 
         if args.resume:
             if not state_from_yaml:
@@ -339,6 +340,22 @@ def all_completed(self,state_name):
         for d in self.target:
             all_completed=all_completed and self.state['completed'][d][state_name]
         return all_completed
+
+    def extract_stepper_type(self, device_name):
+                if 'hello-motor' in device_name:
+                    st = stretch_body.stepper.Stepper('/dev/' + device_name)
+                    for i in st.supported_protocols.keys():
+                        recent_protocol = i.strip('p')
+                    if int(recent_protocol) >= 5:
+                        if not st.startup():
+                            click.secho('FAIL: Unable to establish comms with device %s' % device_name.upper(), fg="red")
+                            return False
+                        else:
+                            if int(st.board_info['protocol_version'].strip('p')) >= 5:
+                                self.stepper_type = st.board_info['stepper_type']
+                                time.sleep(0.5)
+                            st.stop()
+                            del st
 
 # ########################################################################################################################
     def do_device_flash(self, device_name, tag, repo_path=None, verbose=False, port_name=None):
@@ -352,14 +369,15 @@ def do_device_flash(self, device_name, tag, repo_path=None, verbose=False, port_
 
         sketch_name=fwu.get_sketch_name(device_name)
 
-
         if port_name is None:
             print('Looking for device %s on bus' % device_name)
             if not fwu.wait_on_device(device_name, timeout=5.0):
                 print('Failure: Device not on bus.')
                 return False, False
             port_name = fwu.get_port_name(device_name)
 
+
+        self.extract_stepper_type(device_name)
         fwu.user_msg_log('Device: %s Port: %s' % (device_name, port_name), user_display=verbose)
 
         if port_name is not None and sketch_name is not None:
@@ -555,6 +573,13 @@ def flash_stepper_calibration(self, device_name):
                 data = motor.read_encoder_calibration_from_YAML()
                 print('Writing calibration data to flash...')
                 motor.write_encoder_calibration_to_flash(data)
+                print('\n')
+
+                if int(motor.board_info['protocol_version'].strip('p')) >= 5 and self.stepper_type is not None:
+                    print('Writing stepper type to flash...')
+                    motor.write_stepper_type_to_flash(self.stepper_type)
+                    print('Success writing stepper type to Flash')
+                    print('\n')
 
                 print('Successful write of FLASH.')
                 fwu.wait_on_device(device_name)

python/tools/REx_stepper_type.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import stretch_body.stepper as stepper
+import stretch_body.pimu as pimu
+import stretch_body.hello_utils as hu
+import argparse
+import time
+
+
+motor_names = ['hello-motor-left-wheel', 'hello-motor-right-wheel', 'hello-motor-arm', 'hello-motor-lift']
+parser = argparse.ArgumentParser(description='Read and write stepper type to/from flash memory of stepper boards')
+group2 = parser.add_mutually_exclusive_group(required=True)
+group2.add_argument("--read", help="Read the current stepper type from flash",action="store_true")
+group2.add_argument("--write", help="Write stepper_type to stepper flash",action="store_true")
+args = parser.parse_args()
+
+
+def stepper_type():
+    for i in motor_names:
+        motor = stepper.Stepper(f'/dev/{i}')
+        for x in motor.supported_protocols.keys():
+            recent_protocol = x.strip('p')
+        if int(recent_protocol) >= 5:
+            if not motor.startup():
+                print(f"Error with communication to {i}")
+                exit(1)
+
+            if motor.board_info['protocol_version'] == 'p5':
+                if args.write:
+                    print(f"Now setting {i} stepper type to flash...")
+                    motor.write_stepper_type_to_flash(i)
+                    time.sleep(1)
+                    motor.read_stepper_type_from_flash()
+                    if i == motor.board_info['stepper_type']:
+                        print(f"Success {i} stepper type to flash\n")
+                    else:
+                        print(f"Error setting stepper type to {i}, please try again\n")
+                    motor.stop()
+                    time.sleep(1)
+
+                if args.read:
+                    print(f"Now reading stepper_type from {i}....")
+                    motor.read_stepper_type_from_flash()
+                    time.sleep(1)
+
+                    print(f"stepper_type == {motor.board_info['stepper_type']}\n")
+                    time.sleep(1)
+                    motor.stop()
+            else:
+                print(f"Protocol version for {i} is incorrect please update firmware to proper version")
+                exit(1)
+
+        elif int(recent_protocol) < 5:
+            print("Stretch Body Protocol Version is below p5 please update Stretch Body")
+            exit(1)
+
+stepper_type()
+