diff --git a/calibrate.py b/calibrate.py index 905ed0c..151fcf1 100644 --- a/calibrate.py +++ b/calibrate.py @@ -8,7 +8,7 @@ import config_util -def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo): +def calibrate_encoder_to_ankle_conversion(c,exo: exoboot.Exo): '''This routine can be used to manually calibrate the relationship between ankle and motor angles. Move through the full RoM!!!''' exo.update_gains(Kp=constants.DEFAULT_KP, Ki=constants.DEFAULT_KI, @@ -18,8 +18,8 @@ def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo): for _ in range(1000): exo.command_current(exo.motor_sign*1000) time.sleep(0.02) - exo.read_data() - exo.write_data() + exo.read_data(c) + exo.write_data(c,False) print('Done! File saved.') @@ -29,6 +29,6 @@ def calibrate_encoder_to_ankle_conversion(exo: exoboot.Exo): if len(exo_list) > 1: raise ValueError("Just turn on one exo for calibration") exo = exo_list[0] - exo.standing_calibration() - calibrate_encoder_to_ankle_conversion(exo=exo) + exo.standing_calibration(config) + calibrate_encoder_to_ankle_conversion(config,exo=exo) exo.close() \ No newline at end of file diff --git a/config_util.py b/config_util.py index 1d666b2..5817243 100644 --- a/config_util.py +++ b/config_util.py @@ -68,10 +68,10 @@ class ConfigurableConstants(): SWING_ONLY: bool = False # 4 point Spline - RISE_FRACTION: float = 0.2 - PEAK_FRACTION: float = 0.53 - FALL_FRACTION: float = 0.60 - PEAK_TORQUE: float = 5 + RISE_FRACTION: float = 0.075 #0.2 + PEAK_FRACTION: float = 0.33535#0.53 + FALL_FRACTION: float = 0.44478#0.60 + PEAK_TORQUE: float = 18.96235#5 SPLINE_BIAS: float = 3 # Nm # Impedance diff --git a/constants.py b/constants.py index 6605e95..230f14a 100644 --- a/constants.py +++ b/constants.py @@ -9,8 +9,8 @@ class ExobootModel(Enum): EB51 = 1 -ExoType: Type[ExobootModel] = ExobootModel.EB51 -#ExoType = Type[ExobootModel] = ExobootModel.EB45 +#ExoType: Type[ExobootModel] = ExobootModel.EB51 +ExoType: Type[ExobootModel] = ExobootModel.EB45 DEFAULT_BAUD_RATE = 230400 TARGET_FREQ = 200 @@ -20,14 +20,16 @@ class ExobootModel(Enum): MIN_ANKLE_ANGLE = -45 # degrees, dorsiflexion # These polynomials are derived from the calibration routine (calibrate.py), analyzed with transmission_analysis.py - LEFT_ANKLE_TO_MOTOR = [ + """LEFT_ANKLE_TO_MOTOR = [ 1.42930145e-05, 9.60847697e-04, 9.66033271e-03, 1.21997272e+00, -7.39600859e+02, -2.96580581e+04 - ] - RIGHT_ANKLE_TO_MOTOR = [ + ]""" + LEFT_ANKLE_TO_MOTOR = [ 4.04430823e-06, 8.55008776e-04, 4.16160420e-02, 1.52798433e+00, -7.60189930e+02, -2.98125682e+04] + """RIGHT_ANKLE_TO_MOTOR = [ -1.02595964e-05, -9.28352253e-04, -3.10556810e-02, -1.40767218e+00, 7.57989378e+02, -3.48608044e+03 - ] + ]""" + RIGHT_ANKLE_TO_MOTOR = [-2.07317154e-06, -1.02471558e-03, -4.25686613e-02, -9.64798203e-01, 7.71135655e+02, 1.11783125e+04] # These points are used to create a Pchip spline, which defines the transmission ratio as a function of ankle angle ANKLE_PTS_LEFT = np.array([-40, -20, 0, 10, 20, 30, 40, 45.6, 50, @@ -41,7 +43,7 @@ class ExobootModel(Enum): -11]) # Nm/Nm LEFT_ANKLE_ANGLE_OFFSET = 201 #-92 # 7, - RIGHT_ANKLE_ANGLE_OFFSET = -150 #-200# deg + RIGHT_ANKLE_ANGLE_OFFSET = -198# deg elif (ExoType == ExobootModel.EB51): @@ -50,27 +52,25 @@ class ExobootModel(Enum): MIN_ANKLE_ANGLE = -95 #EB-51 - LEFT_ANKLE_TO_MOTOR = np.array([ - -1.39600219e-06, -1.19669863e-04, 5.18587784e-02, 3.48021800e+00, - -5.71389166e+02, -2.84883844e+04 - ]) + #LEFT_ANKLE_TO_MOTOR = np.array([-2.27233618e-06, 3.55519545e-05, 4.73297332e-02, 3.18534948e+00,-5.63299330e+02, -2.85391796e+04]) + LEFT_ANKLE_TO_MOTOR = np.array([-2.41577803e-06, 6.03503507e-05, 4.69647778e-02, 3.14213596e+00,-5.63452288e+02, 2.04918050e+04]) #LEFT_ANKLE_TO_MOTOR = np.array([-2.32052284e-06, 6.29683551e-05, 4.45431399e-02, 3.09556691e+00,-5.49941515e+02, -1.11211736e+04]) #used for the test trials - RIGHT_ANKLE_TO_MOTOR = np.array([ - 5.05377935e-06, -2.83727629e-04, -7.31477078e-02, -1.56178494e+00, - 6.34368537e+02, 9.35489733e+03 - ]) + RIGHT_ANKLE_TO_MOTOR = np.array([5.05377935e-06, -2.83727629e-04, -7.31477078e-02, -1.56178494e+00, + 6.34368537e+02, 9.35489733e+03]) #RIGHT_ANKLE_TO_MOTOR = np.array([ 5.37977952e-06, -2.76399833e-04, -7.58092528e-02, -1.65003833e+00,6.38860770e+02, 9.21647151e+03]) #used for the test trials #EB-51 + ANKLE_PTS_LEFT = np.array([ - -67, -60, -47, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100, - 112 + -67, -60, -50, -40, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 70, 80, 86 ]) TR_PTS_LEFT = np.array([ - 14.85, 14, 11.89, 12.74, 14.28, 13.71, 12.54, 10.43, 8, 5.5, 2.3, 0.4, - -3.3, -10, -11.30, -10.95, -8.75 + 15.5, 13.35, 11.95, 12.03, 13.82, 14.3, 13.96, 12.77, 10.56, 7.1, 3.19, + 0.66, -3.78, -9.55, -12.58, -13.09 ]) + #ANKLE_PTS_LEFT = np.array([-67, -60, -47, -40, -30, -20, -10, 0, 10, 20, 30, 40, 45.6, 55, 80, 90,100, 112]) + #TR_PTS_LEFT = np.array([13.73, 13.5, 13.76, 13.8, 14.04,14.28, 13.71, 12.54, 10.43, 8, 5.5, 2.3, 0.4, -3.3, -10, -11.30, -10.95, -8.75]) #ANKLE_PTS_LEFT = np.array([-67, -60, -40, -20, -10 ,0, 10, 20, 30, 40, 45.6, 55, 80, 90, 100]) #used for the test trials #TR_PTS_LEFT = np.array([14.85, 14, 13.8, 13.7, 13.16, 12, 10.43, 8, 5.5, 2.3, 0.4, -3.3, -10, -11.30, -10.95]) #used for the test trials @@ -91,8 +91,8 @@ class ExobootModel(Enum): # RIGHT_ANKLE_TO_TR = np.array([ 4.83188447e-07, -3.83712114e-05, -3.61934700e-03, 4.54812251e-01, # -2.89416189e+01]) ## NEED TO CHANGE FOR RIGHT ANKLE - #EB-51 - LEFT_ANKLE_ANGLE_OFFSET = -67 # deg + #EB-67#-51 + LEFT_ANKLE_ANGLE_OFFSET = -67#-95#-67 # deg RIGHT_ANKLE_ANGLE_OFFSET = 87.1 #100 # deg # Add to these lists if dev_ids change, or new exos or actpacks are purchased! diff --git a/custom_configs/default_config.py b/custom_configs/default_config.py index 2aa4178..1a78e60 100644 --- a/custom_configs/default_config.py +++ b/custom_configs/default_config.py @@ -1,48 +1,49 @@ - -import config_util -config = config_util.ConfigurableConstants() -# config.HS_GYRO_DELAY = 0.05 # For example -config.MAX_ALLOWABLE_CURRENT = 24000 # mA -config.REEL_IN_MV = 900 -config.REEL_IN_TIMEOUT = 0.075 # 0.2 -config.SWING_SLACK = 3500 #5000 -config.TOE_OFF_FRACTION = 0.65 -config.DO_INCLUDE_GEN_VARS = True - -''' Here are the variables that are updatable in config, and their defaults: - - TARGET_FREQ: float = 200 # Hz - ACTPACK_FREQ: float = 200 # Hz - DO_DEPHY_LOG: bool = True - DEPHY_LOG_LEVEL: int = 4 - TASK: Type[Task] = Task.WALKING - STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE - MAX_ALLOWABLE_CURRENT = 20000 # mA - - # Gait State details - HS_GYRO_THRESHOLD: float = 100 - HS_GYRO_FILTER_N: int = 2 - HS_GYRO_FILTER_WN: float = 3 - HS_GYRO_DELAY: float = 0.05 - SWING_SLACK: int = 10000 - TOE_OFF_FRACTION: float = 0.62 - REEL_IN_TIMEOUT: float = 0.2 - - # 4 point Spline - RISE_FRACTION: float = 0.2 - PEAK_FRACTION: float = 0.53 - FALL_FRACTION: float = 0.63 - PEAK_TORQUE: float = 5 - SPLINE_BIAS: float = 3 # Nm - - # Impedance - K_VAL: int = 500 - B_VAL: int = 0 - SET_POINT: float = 0 # Deg - - READ_ONLY = False # Does not require Lipos - DO_READ_FSRS = False - - PRINT_HS = True # Print heel strikes - SLIP_DETECT_ACTIVE = False -''' + +import config_util +config = config_util.ConfigurableConstants() +# config.HS_GYRO_DELAY = 0.05 # For example +config.MAX_ALLOWABLE_CURRENT = 25000 # mA +config.REEL_IN_MV = 900 +config.REEL_IN_TIMEOUT = 0.075 # 0.2 +config.SWING_SLACK = 3500 #5000 +config.TOE_OFF_FRACTION = 0.65 +config.DO_INCLUDE_GEN_VARS = True + + +''' Here are the variables that are updatable in config, and their defaults: + + TARGET_FREQ: float = 200 # Hz + ACTPACK_FREQ: float = 200 # Hz + DO_DEPHY_LOG: bool = True + DEPHY_LOG_LEVEL: int = 4 + TASK: Type[Task] = Task.WALKING + STANCE_CONTROL_STYLE: Type[StanceCtrlStyle] = StanceCtrlStyle.FOURPOINTSPLINE + MAX_ALLOWABLE_CURRENT = 20000 # mA + + # Gait State details + HS_GYRO_THRESHOLD: float = 100 + HS_GYRO_FILTER_N: int = 2 + HS_GYRO_FILTER_WN: float = 3 + HS_GYRO_DELAY: float = 0.05 + SWING_SLACK: int = 10000 + TOE_OFF_FRACTION: float = 0.62 + REEL_IN_TIMEOUT: float = 0.2 + + # 4 point Spline + RISE_FRACTION: float = 0.2 + PEAK_FRACTION: float = 0.53 + FALL_FRACTION: float = 0.63 + PEAK_TORQUE: float = 5 + SPLINE_BIAS: float = 3 # Nm + + # Impedance + K_VAL: int = 500 + B_VAL: int = 0 + SET_POINT: float = 0 # Deg + + READ_ONLY = False # Does not require Lipos + DO_READ_FSRS = False + + PRINT_HS = True # Print heel strikes + SLIP_DETECT_ACTIVE = False +''' diff --git a/exoboot.py b/exoboot.py index ab3391d..ec6efcd 100644 --- a/exoboot.py +++ b/exoboot.py @@ -185,6 +185,8 @@ class DataContainer: commanded_torque: float = None slack: int = None temperature: int = None + is_clipping: bool = False + max_allowable_torque: float = 0 #Control Parameters rise: float = 0.2 @@ -318,7 +320,7 @@ def read_data(self, self.data.gyro_y = -1 * self.motor_sign * \ actpack_data.gyroy * constants.GYRO_GAIN #Remove -1 for EB-51 - self.data.gyro_z = self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device + self.data.gyro_z = -1 * self.motor_sign * actpack_data.gyroz * constants.GYRO_GAIN # sign may be different from Max's device '''Motor angle and current are kept in Dephy's orientation, but ankle angle and torque are converted to positive = plantarflexion.''' self.data.motor_angle = actpack_data.mot_ang @@ -465,13 +467,16 @@ def command_torque(self, print('desired_torque: ', desired_torque) raise ValueError('Cannot apply negative torques') max_allowable_torque = self.calculate_max_allowable_torque() + self.data.max_allowable_torque = max_allowable_torque if desired_torque > max_allowable_torque: if self.is_clipping is False: # Only print once when clipping occurs before reset logging.warning('Torque was clipped!') desired_torque = max_allowable_torque self.is_clipping = True + self.data.is_clipping = self.is_clipping else: self.is_clipping = False + self.data.is_clipping = self.is_clipping # Softly reduce desired torque at high ankle angles when TR approaches 0 if do_ease_torque_off: