135 lines
3.4 KiB
C++
135 lines
3.4 KiB
C++
#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);
|
|
}
|