initial commit

Makefile

@@ -0,0 +1,30 @@
+BAUDRATE=9600
+FQBN='arduino:avr:uno'
+# FQBN='esp8266:esp8266:nodemcu'
+PORT=/dev/ttyACM0
+PROJ=servoTest
+# TEST=pwmTest
+
+
+run: conf upload 
+
+#init: 
+
+conf: $(PROJ)
+	printf '#ifndef CONFIG\n#define CONFIG\n#define BAUDRATE $(BAUDRATE)\n#endif' > $</config.h;
+
+compile: $(PROJ)
+	arduino-cli compile --fqbn $(FQBN) $(PROJ)
+
+upload: compile
+	arduino-cli upload $(PROJ) -p $(PORT) -b $(FQBN)
+
+serial: 
+	minicom --device $(PORT) -b $(BAUDRATE)
+
+
+# test: 
+# 	arduino-cli compile --fqbn $(FQBN) $(TEST)
+# 	arduino-cli upload $(TEST) -p $(PORT) -b $(FQBN)
+
+

servoTest/config.h

@@ -0,0 +1,4 @@
+#ifndef CONFIG
+#define CONFIG
+#define BAUDRATE 9600
+#endif

servoTest/servoTest.ino

@@ -0,0 +1,187 @@
+#include <Servo.h>
+#include <avr/io.h>
+#include "config.h"
+
+#define S_left_pin 10
+#define S_right_pin 5
+#define S_turn_pin 6
+#define S_grip_pin 9
+
+Servo S_left,S_right,S_turn,S_grip;
+
+const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
+const byte axCount = 4;
+
+int ServoPos[] = {90,90,90,90};
+bool absPos; // absolute /relative positioning
+
+typedef enum {
+    L,
+    R,
+    T,
+    G
+}ax;
+
+
+void error(String msg){
+    Serial.print("error: ");
+    Serial.println(msg);
+}
+
+void movePos(ax dir){
+    int val = axis[dir]->read();
+    if(ServoPos[dir] < 20 || ServoPos[dir] > 175){
+      error("move out of range");
+      return;
+      }
+    if(val != ServoPos[dir]){
+      int delta = ServoPos[dir]-val;
+      while(delta){
+        if(delta < 0){
+          axis[dir]->write((val--) - 1);
+          delta+=1;
+        }else{
+          axis[dir]->write((val++) + 1);
+          delta-=1;
+          }
+          delay(50);
+        }
+
+    }
+}
+
+void setup() {
+  S_left.attach(S_left_pin);
+  S_right.attach(S_right_pin);
+  S_turn.attach(S_turn_pin);
+  S_grip.attach(S_grip_pin);
+
+  Serial.begin(BAUDRATE);
+  Serial.println("reading Servo positions");
+  absPos = true;
+  for(int i =0; i < axCount; i++){
+      ServoPos[i] = axis[i]->read();
+      Serial.print("--");
+      Serial.println(ServoPos[i]);
+  }
+  delay(1000);
+}
+
+
+//Serial Input buffer
+char serialBuf[8];
+int bufSize = 8;
+
+
+void loop() {
+
+   // print changes 
+   for(int i =0; i < axCount; i++){
+      int val = axis[i]->read();
+      if(val != ServoPos[i]){
+        Serial.print("-- ");
+        Serial.println(i);
+        Serial.print(val);
+        Serial.print("-->");
+        Serial.println(ServoPos[i]);
+        movePos(i);
+      }
+      delay(50);
+  }
+  
+  //read position command from serial 
+  int serialIn = Serial.available();
+  if(serialIn){
+    int r = Serial.readBytesUntil('\n',serialBuf, bufSize);
+    Serial.print("read: ");
+    Serial.println(r);
+    //Serial.println(serialBuf);
+    if(r < 2){
+        error("command to short!");
+        return;
+    }
+    String numStr = "";
+    for(int i = 1; i < r; i++){
+        if(isDigit(serialBuf[i]) || isPunct(serialBuf[i])){
+            numStr += serialBuf[i];
+        }else{
+            break;
+        }
+      }
+    Serial.print("NumStr: ");
+    Serial.println(numStr);
+    int pos = numStr.toInt();
+
+    switch(serialBuf[0]){
+        case 'L':
+          Serial.println("Servo_Command L");
+          Serial.println(pos);
+          if(absPos){
+            ServoPos[L] = constrain(pos, 20, 165);
+          }else{
+            ServoPos[L] += pos;
+          }
+          Serial.println(ServoPos[L]);
+          break;
+        case 'R':
+          Serial.println("Servo_Command R");
+          Serial.println(pos);
+
+          if(absPos){
+            ServoPos[R] = constrain(pos, 20, 170);
+          }else{
+              ServoPos[R] += pos;
+          }
+
+          Serial.println(ServoPos[R]);
+
+          break;
+        case 'T':
+          Serial.println("Servo_Command T");
+          Serial.println(pos);
+          if(absPos){
+          ServoPos[T] = constrain(pos, 20, 170);
+          }else{
+              ServoPos[T] += pos;
+          }
+
+          Serial.println(ServoPos[T]);
+          break;
+        case 'G':
+          Serial.println("Servo_Command G");
+          Serial.println(pos);
+          if(absPos){
+          ServoPos[G] = constrain(pos, 90, 170);
+          }else{
+              ServoPos[G] += pos;
+          }
+
+          Serial.println(ServoPos[G]);
+          break;
+        case 'H':
+          Serial.println((absPos)?"Absolute Positioning" : "Relative Positioning");
+          for(int i =0; i < axCount; i++){
+              char axSym = (i==L)?'L':(i==R)?'R':(i==T)?'T':'G';
+              Serial.print(axSym);
+              Serial.print("--");
+              Serial.println(ServoPos[i]);
+          }
+          break;
+        case 'S':
+          if(serialBuf[1] == '0') absPos = false;
+          else absPos = true;
+          break;
+        case 'D': //default
+          for(int i = 0; i < axCount;i++){
+          ServoPos[i] = 90;
+          }
+          break;
+          
+ 
+          
+        default:
+        error("invalid axis description");
+        break;
+      }
+  }
+