feat: began working on cleaning up explorative main file

minumel_arm_clean_old.txt

@@ -0,0 +1,184 @@
+#include <Adafruit_PWMServoDriver.h>
+#include <SPI.h>
+#include <mcp_can.h>
+#include "defs.hpp"
+
+Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
+
+int angleToPulse(int theta, int motorIndex) {
+  return map(theta, 0, servo_range[motorIndex], SERVO_PMIN, SERVO_PMAX);
+}
+
+// ----- UART command handling -----
+String inString = "";
+
+String handleUartComm() {
+  if (Serial.available() > 0) {
+    unsigned char inChar = Serial.read();
+    // Read finished procedure
+    if (inChar == '\n') {
+      // Copy return value
+      String retStr = inString;
+
+      // Reset
+      inString = "";
+      return retStr;
+    }
+    // Store byte procedure
+    else {
+      inString += (char)inChar;
+      return "";
+    }
+  } else {
+    return "";
+  }
+}
+
+// ----- CAN config -----
+const int CAN_CS_PIN = 10;           // CS pin for MCP2515
+MCP_CAN CAN0(CAN_CS_PIN);
+
+// Choose depending on your module crystal:
+//   MCP_16MHZ for most shields
+//   MCP_8MHZ for many cheap blue boards
+const byte CAN_CLOCK = MCP_8MHZ;    // change to MCP_8MHZ if needed
+
+// CAN bitrate and ID we listen to
+const unsigned long SERVO_CAN_ID = 0x100;  // must match sender
+
+// ----- String utils -----
+int splitByChar(String in, char c, String* out) {
+  int start = 0;
+  int end;
+  int count = 0;
+
+  while (true) {
+    end = in.indexOf(c, start);
+
+    if (end == -1) {
+      out[count++] = in.substring(start);
+      break;
+    }
+
+    out[count++] = in.substring(start, end);
+    start = end + 1;
+  }
+
+  return count;
+}
+
+// Process a complete command like "M 0 150"
+void processCommand(String command) {
+  command.trim();  // remove any stray whitespace
+
+  if (command.length() == 0) {
+    return;
+  }
+
+  String argv[5];
+  int argc = splitByChar(command, ' ', argv);
+
+  // Expecting command like: MA 0 150
+  // Motor Absolute sets absolute motor position
+  if (argc >= 3 && argv[0] == "MA") {
+    int motorIndex = argv[1].toInt();
+    int theta      = argv[2].toInt();
+
+    // Safety range check
+    if (theta < min_safe_angles[motorIndex]) {
+      // Serial.println("Refusing to move motor to " + String(theta));
+    } 
+    else if (theta > max_safe_angles[motorIndex]) {
+      // Serial.println("Refusing to move motor to " + String(theta));
+    }
+    else {
+      board1.setPWM(motorIndex, 0, angleToPulse(theta, motorIndex));
+      currentPose[motorIndex] = theta;
+    }
+  }
+  // Expecting command like: MR 0 150
+  // Motor Relative sets relatove motor position, e.g. move by 10
+  else if (argc >= 3 && argv[0] == "MR") {
+    int motorIndex = argv[1].toInt();
+    int delta      = argv[2].toInt();
+    int targetPosition = constrain(currentPose[motorIndex] + delta, min_safe_angles[motorIndex], max_safe_angles[motorIndex]);
+
+    if (targetPosition != currentPose[motorIndex]) {
+      Serial.println("Moving motor " + String(argv[1]) + " to " + String(targetPosition));
+      board1.setPWM(motorIndex, 0, angleToPulse(targetPosition, motorIndex));
+      currentPose[motorIndex] = targetPosition;
+    }
+  }
+  // Expecting command like: MR 0 150
+  // Command to reset pose
+  else if (argc == 1 && argv[0] == "R") {
+  takeInitialPose();
+  } else {
+    // Serial.println("Received unknown or malformed command: " + command);
+  }
+}
+
+void takeInitialPose() {
+  for (int motorIndex = 0; motorIndex < 4; motorIndex++) {
+    board1.setPWM(motorIndex, 0, angleToPulse(initial_pose[motorIndex], motorIndex));
+    currentPose[motorIndex] = initial_pose[motorIndex];
+    delay(500);
+  }
+}
+
+void setup() {
+  Serial.begin(9600);
+
+  board1.begin();
+  board1.setPWMFreq(60); // Servos operate at ~60 Hz
+
+  takeInitialPose();
+
+  while (!Serial) {
+    ; // wait for serial port to connect (for native USB boards)
+  }
+
+  // ----- CAN init -----
+  while (CAN0.begin(MCP_ANY, CAN_500KBPS, CAN_CLOCK) != CAN_OK) {
+    Serial.println("CAN init failed, retrying...");
+    delay(200);
+  }
+  CAN0.setMode(MCP_NORMAL);
+  Serial.println("CAN init OK");
+}
+
+void loop() {
+  // ----- UART handling -----
+  String uartCommand = handleUartComm();
+  if (uartCommand != "") {
+    //Serial.println(uartCommand);
+    processCommand(uartCommand);
+  }
+
+  // ----- CAN handling -----
+  // Check if a CAN message is available
+  if (CAN_MSGAVAIL == CAN0.checkReceive()) {
+    unsigned long canId;
+    byte len;
+    byte buf[8];
+
+    // Read CAN frame
+    CAN0.readMsgBuf(&canId, &len, buf);
+
+    // Only accept frames with the expected ID (optional but recommended)
+    if (canId == SERVO_CAN_ID) {
+      // Interpret payload as ASCII string, e.g. "M 0 150"
+      String canCommand = "";
+
+      for (byte i = 0; i < len; i++) {
+        canCommand += (char)buf[i];
+      }
+
+      Serial.println(canCommand);
+      processCommand(canCommand);
+    }
+  }
+
+  // You can add a small delay if you want to chill the loop a bit
+  // delay(1);
+}