/*
  Move a 6 DOF robot arm with potentiometers 
  one button to store position
  one button to run to all positions

  https://www.blafusel.de/
  VI/2025
*/

#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define BTNSAVE 2
#define BTNRUN 3
#define ARRAYSIZE 10
#define SERVOMIN  130   // value for servo left/right position
#define SERVOMAX  500   // value for servo other position
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
uint16_t PWMStep = (SERVOMAX - SERVOMIN) / 180;
double PotiStep = 1024 / 180.0;

uint8_t Positions[] = {90, 100, 90, 80, 95, 120};   // Start positions middle (claw  not < ~120), try out
uint8_t AnalogPin[] = {A0, A1, A2, A3, A6, A7};      // The Analog In-Pins 

uint16_t TeachInArray[6][ARRAYSIZE];
uint16_t TeachInPos = 0;
volatile uint8_t IRQButton=0;
volatile uint8_t IRQRun=0;

void IRQSavePosBtn ()
{
  IRQButton = 1;
}

void IRQRunBtn ()
{
  IRQRun = 1;
}

void setup() 
{
  Serial.begin(19200);
  pinMode(BTNSAVE, INPUT_PULLUP);
  pinMode(BTNRUN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BTNSAVE), IRQSavePosBtn, FALLING);
  attachInterrupt(digitalPinToInterrupt(BTNRUN), IRQRunBtn, FALLING);

  pwm.begin();
  pwm.setOscillatorFrequency(25000000);   // try out for best results
  pwm.setPWMFreq(SERVO_FREQ);  
  delay(10);

  for (uint8_t i = 0; i < 6; i++)
    pwm.setPWM(i, 0, SERVOMIN + Positions[i] * PWMStep);
}

void loop() 
{
  uint8_t i, u;
  uint16_t sensorValue;
  uint16_t NewPos;

  for (i=0; i < 6; i++)   // read all 6 poti values and move arm
  {
    sensorValue = analogRead(AnalogPin[i]);
    NewPos = round (SERVOMIN + (sensorValue / PotiStep * PWMStep));
    pwm.setPWM(i, 0, NewPos);
    Serial.print (NewPos); Serial.print ("\t");
  }
  Serial.println();

  if (IRQRun == 1)      // Run once (after it return to poti positions)
  {
    Serial.println ("RUN ===========================");
    for (u=0; u < TeachInPos; u++)    // for all stored positions
    {
      Serial.print ("Step ");
      Serial.println (u);

      for (i=0; i < 6; i++)   // 6 servos in each step
      {
        pwm.setPWM(i, 0, TeachInArray[u][i]);
      }
      delay (1500);  // wait before next step
    }
    Serial.println ("==============================");
    delay (100);    //debounce
    IRQRun = 0;
  }

  if (IRQButton == 1)      // Save Pos
  {
    if (TeachInPos < ARRAYSIZE)   // space?
    {                  
      Serial.println ("SAFE =========================");
      for (i=0; i < 6; i++)
      {
        sensorValue = analogRead(AnalogPin[i]);
        NewPos = round (SERVOMIN + (sensorValue / PotiStep * PWMStep));
        TeachInArray[TeachInPos][i] = NewPos;
      }
      TeachInPos++;
      for (u=0; u < TeachInPos; u++)
      {
        for (i=0; i < 6; i++)
        {
          Serial.print (TeachInArray[u][i]);
          Serial.print ("\t");
        }
        Serial.println();
      }
      Serial.println ("==============================");
      delay (100);    //debounce
      IRQButton = 0;
    }
  }
}

