commit da3b20ea94a6fb2c85f37a5416334328a5b5ca38
Author: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date:   Mon Dec 1 20:32:11 2025 -0500

    initial commit
    
    adapted the motion example for a differential drive chassis

diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino
new file mode 100644
index 0000000..d9bf538
--- /dev/null
+++ b/robot_controller/robot_controller.ino
@@ -0,0 +1,60 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <smartmotor.h>
+#include <SMC_gains.h>
+
+SmartMotor left_motor(0x0A); 
+SmartMotor right_motor(0x0B); 
+
+void setup() {
+    // INIT SERIAL
+    Serial.begin(115200);
+    while(!Serial);
+
+    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
+    
+    // TUNE POSITION PID
+    left_motor.tune_pos_pid(0.65,0.060,0.065); 
+    right_motor.tune_pos_pid(0.65,0.060,0.065); 
+    delay(1000);
+
+
+    // SET MOTOR POSITION
+    //int32_t angle=360;
+    //uint8_t status= left_motor.write_angle(angle);
+    //delay(1000);
+
+    // READ MOTOR POSITION
+    int32_t pos = left_motor.read_angle();
+    Serial.print("Pos: ");
+    Serial.print(pos);
+    Serial.println(" deg");
+}
+
+int32_t angle = 0;
+
+enum ROBOT_STATE {
+  FORWARD,
+  TURN,
+  REVERSE
+};
+
+enum ROBOT_STATE robot_state = FORWARD;
+
+void loop() {
+    angle = (millis() / 2000) * 360;
+    left_motor.write_angle(angle);
+    right_motor.write_angle(angle);
+    delay(20);
+
+    // READ MOTOR POSITION
+    int32_t pos = left_motor.read_angle();
+    int32_t error = angle - pos;
+    Serial.print("Pos: ");
+    Serial.print(pos);
+    Serial.print(" Setpoint: ");
+    Serial.print(angle);
+    Serial.print(" Err: ");
+    Serial.print(error);
+    Serial.println("");
+}