diff --git a/bam/feetech/record.py b/bam/feetech/record.py index 5046f7d..9a61655 100644 --- a/bam/feetech/record.py +++ b/bam/feetech/record.py @@ -55,7 +55,7 @@ # motor.disable_torque() # control.set_kps(ids, [32]) io.set_P_coefficient({1: args.kp}) - io.set_D_coefficient({1:0}) + io.set_D_coefficient({1: 0}) # motor.kp = args.kp @@ -72,6 +72,14 @@ } +def convert_load(raw_load): + sign = -1 + if raw_load > 1023: + raw_load -= 1024 + sign = 1 + return sign * raw_load * 0.001 + + def read_data(): # position = control.read_present_position(ids)[0] @@ -83,8 +91,7 @@ def read_data(): # speed = control.read_present_velocity(ids)[0] speed = np.deg2rad(io.get_present_speed([1])[0]) - - load = 0 # TMP + load = convert_load(io.get_present_load([1])[0]) volts = io.get_present_voltage([1])[0] * 0.1 # volts = 0 diff --git a/bam/trajectory.py b/bam/trajectory.py index c6ae3af..ac4b236 100644 --- a/bam/trajectory.py +++ b/bam/trajectory.py @@ -78,6 +78,17 @@ def __call__(self, t: float): return angle, True +class Brutal(Trajectory): + duration = 6.0 + + def __call__(self, t: float): + + if t > self.duration / 4 and t < self.duration / 1.5: + return np.pi / 2, True + else: + return 0.0, True + + class Nothing(Trajectory): duration = 6.0 @@ -90,6 +101,7 @@ def __call__(self, t: float): "sin_time_square": SinusTimeSquare(), "up_and_down": UpAndDown(), "sin_sin": SinSin(), + "brutal": Brutal(), "nothing": Nothing(), }