Add arduino code that runs the robot

arduino/angle_utility/angle_utility.ino

@@ -0,0 +1,67 @@
+#include <Servo.h>
+
+Servo servo1;
+Servo servo2;
+
+int angle1 = 90; // Initial angle for servo 1
+int angle2 = 90; // Initial angle for servo 2
+
+void setup() {
+  // shoulder
+  servo1.attach(9); // Assuming servo 1 is attached to pin 9
+  // elbow
+  servo2.attach(10); // Assuming servo 2 is attached to pin 10
+
+  servo1.write(angle1);
+  servo2.write(angle2);
+
+  Serial.begin(9600);
+  Serial.println("Servo Control Started. Use h, j, k, l to control servos.");
+}
+
+void loop() {
+  if (Serial.available()) {
+    char ch = Serial.read();
+
+    switch(ch) {
+      case 'h':
+        angle1 = max(0, angle1 - 5);
+        servo1.write(angle1);
+        break;
+      case 'H':
+        angle1 = max(0, angle1 - 1);
+        servo1.write(angle1);
+        break;
+      case 'j':
+        angle1 = min(180, angle1 + 5);
+        servo1.write(angle1);
+        break;
+      case 'J':
+        angle1 = min(180, angle1 + 1);
+        servo1.write(angle1);
+        break;
+      case 'k':
+        angle2 = max(0, angle2 - 5);
+        servo2.write(angle2);
+        break;
+      case 'K':
+        angle2 = max(0, angle2 - 1);
+        servo2.write(angle2);
+        break;
+      case 'l':
+        angle2 = min(180, angle2 + 5);
+        servo2.write(angle2);
+        break;
+      case 'L':
+        angle2 = min(180, angle2 + 1);
+        servo2.write(angle2);
+        break;
+    }
+
+    // Print current angles after every change
+    Serial.print("shoulder: ");
+    Serial.print(angle1);
+    Serial.print(" | elbow: ");
+    Serial.println(angle2);
+  }
+}

arduino/sample_location/sample_location.ino

@@ -0,0 +1,85 @@
+#include <Servo.h>
+
+Servo shoulder;
+Servo elbow;
+
+struct robot_state {
+  int shoulder_angle;
+  int elbow_angle;
+};
+
+template <typename T>
+struct optional {
+  T value;
+  bool valid;
+};
+
+optional<robot_state> get_command(char c) {
+  switch (c) {
+    case '0':
+      return {{135, 94}, true}; //
+    case '1':
+      return {{96, 49}, true}; //
+    case '2':
+      return {{66, 29}, true};//
+    case '3':
+      return {{177, 154}, true}; //
+    case '4':
+      return {{176, 175}, true};//
+    case '5':
+      return {{52, 0}, true};
+     case 'r':
+      return {{90, 90}, true};
+    default:
+      return {{0, 0}, false};
+  }
+}
+
+void send_command(robot_state const& command){
+  shoulder.write(command.shoulder_angle);
+  elbow.write(command.elbow_angle);
+}
+
+bool read_sensor() {
+  auto const value = digitalRead(2);
+  return value == 0 ? true : false;
+}
+
+void setup() {
+  //start serial connection
+  Serial.begin(9600);
+  //configure pin 2 as an input and enable the internal pull-up resistor
+  pinMode(2, INPUT_PULLUP);
+  pinMode(13, OUTPUT);
+
+  shoulder.attach(9);
+  elbow.attach(10);
+}
+
+void loop() { 
+  // Only respond to new commands
+  if (!Serial.available()) {
+    return;
+  }
+  // parse the command to joint state
+  auto const joint_state = get_command(Serial.read());
+  if (!joint_state.valid) {
+    return;
+  }
+  send_command(joint_state.value);
+
+  // allow arm time to travel
+  delay(1000);
+
+  auto const detected = read_sensor();
+
+  if (detected) {
+    digitalWrite(13, HIGH);
+    Serial.println(1);
+    return;
+  }
+
+  digitalWrite(13, LOW);
+  Serial.println(0);
+
+}