|
| 1 | +#!/usr/bin/env python3 |
| 2 | +"""Print the PiPER's protection settings and driver status, without moving or changing anything. |
| 3 | +
|
| 4 | +Runs on the arm Pi, in the venv that has piper_sdk (~/piper-venv): |
| 5 | +
|
| 6 | + ~/piper-venv/bin/python piper_status.py |
| 7 | +
|
| 8 | +Shows what decides how the arm reacts when it is blocked: each joint's collision protection |
| 9 | +level (0 means no collision detection), each driver's fault flags (under-voltage, |
| 10 | +over-current, motor and driver over-temperature, collision, stall), the temperatures, and the |
| 11 | +arm status code. |
| 12 | +
|
| 13 | +The only frames it sends are queries: ConnectPort's limit and firmware queries, and 0x477 with |
| 14 | +Byte 0 = 0x02 (query collision protection level). It never enables the motors, commands |
| 15 | +motion, or writes a setting. can0 must already be up at 1 Mbit/s; this script never touches |
| 16 | +the interface. |
| 17 | +""" |
| 18 | +import argparse |
| 19 | +import os |
| 20 | +import sys |
| 21 | +import time |
| 22 | + |
| 23 | +FLAGS = ["voltage_too_low", "motor_overheating", "driver_overcurrent", "driver_overheating", |
| 24 | + "collision_status", "driver_error_status", "stall_status"] |
| 25 | + |
| 26 | + |
| 27 | +def wait_for(pred, timeout_s=3.0): |
| 28 | + t0 = time.time() |
| 29 | + while time.time() - t0 < timeout_s: |
| 30 | + if pred(): |
| 31 | + return True |
| 32 | + time.sleep(0.05) |
| 33 | + return False |
| 34 | + |
| 35 | + |
| 36 | +def main(): |
| 37 | + ap = argparse.ArgumentParser(description=__doc__, |
| 38 | + formatter_class=argparse.RawDescriptionHelpFormatter) |
| 39 | + ap.add_argument("--can", default="can0") |
| 40 | + args = ap.parse_args() |
| 41 | + |
| 42 | + state = open(f"/sys/class/net/{args.can}/operstate").read().strip() \ |
| 43 | + if os.path.exists(f"/sys/class/net/{args.can}") else "missing" |
| 44 | + if state not in ("up", "unknown"): |
| 45 | + sys.exit(f"{args.can} is {state}; bring it up at 1 Mbit/s first") |
| 46 | + |
| 47 | + from piper_sdk import C_PiperInterface_V2 |
| 48 | + |
| 49 | + piper = C_PiperInterface_V2(args.can) |
| 50 | + piper.ConnectPort() |
| 51 | + if not wait_for(lambda: piper.GetArmLowSpdInfoMsgs().Hz > 0): |
| 52 | + sys.exit("no driver feedback on the bus: is the arm powered and wired to the adapter?") |
| 53 | + |
| 54 | + # Query only: param_setting, 0x48X feedback and end load are left at "no change". |
| 55 | + def crash_levels(): |
| 56 | + piper.ArmParamEnquiryAndConfig(param_enquiry=0x02) |
| 57 | + time.sleep(0.05) |
| 58 | + return piper.GetCrashProtectionLevelFeedback().time_stamp > 0 |
| 59 | + have_crash = wait_for(crash_levels) |
| 60 | + time.sleep(0.5) # let a few more feedback frames arrive |
| 61 | + |
| 62 | + print("firmware:", piper.GetPiperFirmwareVersion()) |
| 63 | + st = piper.GetArmStatus().arm_status |
| 64 | + print(f"ctrl mode: {st.ctrl_mode} arm status: {st.arm_status} motion: {st.motion_status}" |
| 65 | + f" err code: {st.err_code}") |
| 66 | + errs = [k for k, v in vars(st.err_status).items() if v] |
| 67 | + print("err flags:", ", ".join(errs) or "none") |
| 68 | + |
| 69 | + js = piper.GetArmJointMsgs().joint_state |
| 70 | + q = [js.joint_1, js.joint_2, js.joint_3, js.joint_4, js.joint_5, js.joint_6] |
| 71 | + print("joints (deg):", "[" + ", ".join(f"{v / 1000:7.2f}" for v in q) + "]") |
| 72 | + |
| 73 | + if have_crash: |
| 74 | + c = piper.GetCrashProtectionLevelFeedback().crash_protection_level_feedback |
| 75 | + levels = [getattr(c, f"joint_{i}_protection_level") for i in range(1, 7)] |
| 76 | + print("collision protection level J1-J6:", levels, "(0 = no collision detection)") |
| 77 | + else: |
| 78 | + print("collision protection level: no reply to the 0x477 query") |
| 79 | + |
| 80 | + low = piper.GetArmLowSpdInfoMsgs() |
| 81 | + print("joint volts driver_C motor_C enabled faults") |
| 82 | + for i in range(1, 7): |
| 83 | + m = getattr(low, f"motor_{i}") |
| 84 | + faults = [f for f in FLAGS if getattr(m.foc_status, f)] |
| 85 | + print(f"J{i} {m.vol / 10:9.1f} {m.foc_temp:8d} {m.motor_temp:7d}" |
| 86 | + f" {str(m.foc_status.driver_enable_status):>7} {', '.join(faults) or 'none'}") |
| 87 | + |
| 88 | + |
| 89 | +if __name__ == "__main__": |
| 90 | + main() |
0 commit comments