From f339c6c4e59a2ee2338bdf3f96bad1092e9e7308 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Sun, 9 May 2021 20:33:36 -0400
Subject: [PATCH] Calibrate and enter closed loop in the example

---
 docs/can-guide.md    |  4 ++++
 tools/can_example.py | 49 +++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 48 insertions(+), 5 deletions(-)

diff --git a/docs/can-guide.md b/docs/can-guide.md
index 361430e2..1e526898 100644
--- a/docs/can-guide.md
+++ b/docs/can-guide.md
@@ -101,6 +101,10 @@ $ candump can0 -xct z -n 10
  (000.401972)  can0  RX - -  021   [8]  00 00 00 00 08 00 00 00
 ```
 
+Alternatively, if you have python can installed (`pip3 install python-can`), you can use the can.viewer script:
+
+`python3 -m can.viewer -c "can0" -i "socketcan"` which will give you a nice readout.  See [the python-can docs](https://python-can.readthedocs.io/en/master/scripts.html#can-viewer) for an example.
+
 ## Commanding the ODrive
 
 Now that we've verified the communication is working, we can try commanding the ODrive.  Make sure your ODrive is configured and working properly over USB with `odrivetool` before continuing.  See the [Getting Started Guide](getting-started.md) for help with first-time configuration.
diff --git a/tools/can_example.py b/tools/can_example.py
index c2836d68..8bf6c65d 100644
--- a/tools/can_example.py
+++ b/tools/can_example.py
@@ -1,14 +1,53 @@
 import can
 
-bus = can.interface.Bus('can0', bustype='socketcan')
-axisID = 0x1 << 5
-msgID = axisID | 0x7
+bus = can.interface.Bus("can0", bustype="socketcan")
+axisID = 0x1
+msgID = axisID << 5 | 0x7
 
-msg = can.Message(arbitration_id=msgID, data=[0, 0, 0, 0, 3, 0, 0, 0], dlc=8)
+print("Requesting AXIS_STATE_FULL_CALIBRATION_SEQUENCE (0x03) on axisID: " + str(axisID))
+msg = can.Message(arbitration_id=msgID, data=[3, 0, 0, 0, 0, 0, 0, 0], dlc=8, is_extended_id=False)
+print(msg)
+try:
+    bus.send(msg)
+    print("Message sent on {}".format(bus.channel_info))
+except can.CanError:
+    print("Message NOT sent!  Please verify can0 is working first")
+
+print("Waiting for calibration to finish...")
+while True:
+    msg = bus.recv()
+    if msg.arbitration_id == (axisID << 5 | 0x01):
+        if msg.data[4] == 0x1:
+            break
+
+print("\nAxis has returned to Idle state.")
+while True:
+    msg = bus.recv()
+    if(msg.arbitration_id == (axisID << 5 | 0x01)):
+        errorCode = msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24
+        print("\nReceived Axis heartbeat message:")
+        if errorCode == 0x0:
+            print("No errors")
+        else:
+            print("Axis error!  Error code: "+str(hex(errorCode)))
+        break
+
+print("\nPutting axis",axisID,"into AXIS_STATE_CLOSED_LOOP_CONTROL...")
+msg = can.Message(arbitration_id=msgID, data=[8, 0, 0, 0, 0, 0, 0, 0], dlc=8, is_extended_id=False)
 print(msg)
 
 try:
     bus.send(msg)
     print("Message sent on {}".format(bus.channel_info))
 except can.CanError:
-    print("Message NOT sent!")
\ No newline at end of file
+    print("Message NOT sent!")
+
+while True:
+    msg = bus.recv()
+    if msg.arbitration_id == (axisID << 5 | 0x01):
+        print("\nReceived Axis heartbeat message:")
+        if msg.data[4] == 0x8:
+            print("Axis has entered closed loop")
+        else:
+            print("Axis failed to enter closed loop")
+        break
\ No newline at end of file