From 8014409ab80cccbf928ce31da22d91dbcadeab48 Mon Sep 17 00:00:00 2001
From: Matthew Foran <matthewjforan@gmail.com>
Date: Fri, 16 Aug 2024 14:04:14 -0400
Subject: [PATCH] enabled host networking on devcontainer, commented example

---
 .devcontainer/devcontainer.json    |  3 +++
 gm6020_ros/gm6020_can              |  2 +-
 gm6020_ros/src/gm6020_can_test.cpp | 30 +++++++++++++++++++++++-------
 3 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 69c9bd1..edd3b3b 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -27,4 +27,7 @@
 	"workspaceMount": "source=${localWorkspaceFolder},target=/colcon_ws/src/,type=bind",
 	"workspaceFolder": "/colcon_ws/src",
 	//"overrideCommand": false
+	"runArgs": [
+		"--network=host"
+	]
 }
diff --git a/gm6020_ros/gm6020_can b/gm6020_ros/gm6020_can
index 5bab28d..4864130 160000
--- a/gm6020_ros/gm6020_can
+++ b/gm6020_ros/gm6020_can
@@ -1 +1 @@
-Subproject commit 5bab28dd30a72c9b94348d49ad0fdba3d25f8e02
+Subproject commit 48641301c7fc2542713b9b0ee94cdebe41a2603e
diff --git a/gm6020_ros/src/gm6020_can_test.cpp b/gm6020_ros/src/gm6020_can_test.cpp
index 79f90b0..d882bf0 100644
--- a/gm6020_ros/src/gm6020_can_test.cpp
+++ b/gm6020_ros/src/gm6020_can_test.cpp
@@ -3,18 +3,32 @@
 #include <thread>
 #include <chrono>
 #include <iostream>
-// export LD_LIBRARY_PATH=/home/$USER/phalanx/gm6020_ros/gm6020_ros/gm6020_can/target/release/:$LD_LIBRARY_PATH
-// pushd gm6020_can && cargo build --release ; popd && g++ src/gm6020_can_test.cpp -I gm6020_can/include/ -L gm6020_can/target/release/ -lgm6020_can -lpthread -o test && ./test
 
-const unsigned int RATE = 50; // Don't want it too high because CAN bus will run out of buffer
-const unsigned int PERIOD = 1.0/RATE;
-const unsigned int INC = 10;
-const int MAX = V_MAX * 10;
-const int ID = 1;
+//////
+// Basic C++ example showing how to use gm6020_can library. Corresponds to gm6020_can/examples/gm6020_can_test.rs
+//////
+/*
+export LD_LIBRARY_PATH=/colcon_ws/src/gm6020_ros/gm6020_can/target/release
+pushd gm6020_can && cargo build --release ; popd && g++ src/gm6020_can_test.cpp -I gm6020_can/include/ -L gm6020_can/target/release/ -lgm6020_can -lpthread -o test && ./test
+*/
+
+const unsigned int RATE = 100; // Should be above 100Hz. At slower rates the feedback values get weird and eventually the commands stop going out. Could be hardware-dependent.
+const unsigned int PERIOD = (1.0/RATE)*1000;
+const unsigned int INC = 10; // Time between commands
+const int MAX = V_MAX * 10;  // Need the 10x multiplier so we can easily increment in for loops (can't increment floats).
+const int ID = 1;            // Motor ID [1,7]
 int main() {
+    // Open SocketCAN device
     Gm6020Can * gmc = init("can0");
+    if (gmc == nullptr){
+        std::cerr<<"Unable to open specified SocketCAN device"<<std::endl;
+        return -1;
+    }
+
+    // Start another thread to collect feedback values
     run(gmc, PERIOD);
 
+    // Ramp up, ramp down, ramp up (negative), ramp down (negative)
     for (int voltage = 0; voltage <= MAX; voltage += 2) {
         cmd_single(gmc, CmdMode::Voltage, ID, voltage / 10.0);
         std::this_thread::sleep_for (std::chrono::milliseconds(INC));
@@ -31,6 +45,8 @@ int main() {
         cmd_single(gmc, CmdMode::Voltage, ID, voltage / 10.0);
         std::this_thread::sleep_for (std::chrono::milliseconds(INC));
     }
+
+    // Send constant voltage command and read out position feedback
     cmd_single(gmc, CmdMode::Voltage, ID, 1.0);
     while (true){
         std::this_thread::sleep_for (std::chrono::milliseconds(50));