initial commit

Arduino/main.ino

@@ -0,0 +1,134 @@
+#include <Servo.h>
+
+//amount of values to collect for everaging
+#define AMOUNT_OF_VALUES 5
+//minimum angle of the servo movement
+#define MINIMUM_POSITION 180
+//maximum angle of the servo movement
+#define MAXIMUM_POSITION 0
+//minimum value of ADC
+#define ADC_MIN_VALUE 0
+//maximum value of the adc, usually 2^10 for arduinos
+#define ADC_MAX_VALUE 1024
+//minimum output value of the opamp circuit
+#define SENSOR_MIN_VALUE 0
+//maximum output value of the opamp circuit. 
+//Arduinos have a 0-5V ADC but the current circuit maps to 0-1V for ESP32 chips
+#define SENSOR_MAX_VALUE 256
+//how long to wait between iterations in the main loop
+#define DELAY 200
+
+//pins for the servos
+#define SERVO_DAUMEN 2
+#define SERVO_ZEIGEFINGER 3
+#define SERVO_MITTELFINGER 5
+#define SERVO_RINGFINGER 6
+#define SERVO_KLEINERFINGER 4
+
+//pins for the sensors
+#define SENSOR_DAUMEN A1
+#define SENSOR_ZEIGEFINGER A2
+#define SENSOR_MITTELFINGER A4
+#define SENSOR_RINGFINGER A5
+#define SENSOR_KLEINERFINGER A3
+
+Servo servos[5];
+
+/*
+
+|J1- Color   - Use   - Pin    - Comments
+|------------|-------|--------|-----------
+|1 - Blk     - GND   - GND    -
+|2 - Wht     - OUT4  - Pin A2 - Zeigefinger
+|3 - NC      -       -        -
+|4 - Lgt Brn - OUT5  - Pin A1 - Daumen
+|5 - Red     - VCC   - 3.3V   -
+|6 - Gry     - OUT3  - Pin A4 - Mittelfinger
+|7 - Ppl     - OUT2  - Pin A5 - Ringfinger
+|8 - Cyn     - OUT1  - Pin A3 - Kleiner Finger
+ */
+ 
+int sensorPins[5] = {SENSOR_DAUMEN, SENSOR_ZEIGEFINGER, SENSOR_MITTELFINGER, SENSOR_RINGFINGER, SENSOR_KLEINERFINGER};
+int servoPins[5] = {SERVO_DAUMEN, SERVO_ZEIGEFINGER, SERVO_MITTELFINGER, SERVO_RINGFINGER, SERVO_KLEINERFINGER};
+
+int sensorsRaw[5][AMOUNT_OF_VALUES];
+double sensorsAvg[5] = {0,0,0,0,0};
+int servoValues[5] = {0,0,0,0,0};
+
+void setup() {
+
+  Serial.begin(9600);
+
+  for(int i = 0; i<5; i++) {
+    servos[i].attach(servoPins[i]);
+  }
+  
+}
+
+/*
+ * Read the sensors after each other for a total of AMOUNT_OF_VALUES times
+ */
+void readSensors() {
+
+  for(int i = 0; i<AMOUNT_OF_VALUES; i++) {
+    for(int j = 0; j<5; j++) {
+      sensorsRaw[j][i] = analogRead(sensorPins[j]);
+    }
+  }
+}
+
+/*
+ * Average the AMOUNT_OF_VALUE readings from the sensor
+ */
+void calculateAvg(){
+  for(int i = 0; i<5; i++) {
+    for(int j = 0; j<AMOUNT_OF_VALUES; j++) {
+      sensorsAvg[i] += sensorsRaw[i][j];
+    }
+    //calculate average for sensor i
+    sensorsAvg[i] /= AMOUNT_OF_VALUES;
+  }
+}
+
+/*
+ * Move servos based on the average value
+ */
+void moveServos(){
+  for(int i = 0; i<5; i++) {
+    //make sure the average value is within the bounds of the ADC
+    int constrained_value = constrain(sensorsAvg[i], ADC_MIN_VALUE, ADC_MAX_VALUE);
+    //linear mapping from sensor values to movement position
+    servoValues[i] = map(constrained_value, SENSOR_MIN_VALUE, SENSOR_MAX_VALUE, MINIMUM_POSITION, MAXIMUM_POSITION);
+    //move servo i
+    servos[i].write(servoValues[i]);
+  }
+}
+
+/*
+ * Debug over serial port, use Serial Plotter in Arduino IDE!
+ */
+void printDebug() {
+  Serial.print("D_Avg:");
+  Serial.print(sensorsAvg[0]);
+  Serial.print(",");
+  Serial.print("ZF_Avg:");
+  Serial.print(sensorsAvg[1]);
+  Serial.print(",");
+  Serial.print("MF_Avg:");
+  Serial.print(sensorsAvg[2]);
+  Serial.print(",");
+  Serial.print("RF_Avg:");
+  Serial.print(sensorsAvg[3]);
+  Serial.print(",");
+  Serial.print("KF_Avg:");
+  Serial.println(sensorsAvg[4]);
+}
+
+void loop() {
+  readSensors();
+  calculateAvg();
+  moveServos();
+  printDebug();
+  
+  delay(DELAY);
+}