MeArm 1.6.1 Robot Joystick Board Recording Movements (IR)

/* meArm analog joysticks version 1.6.1.4 - UtilStudio.com Jan 2019
 Uses two analogue joysticks and four servos.
 
 Version 1.6.1.x is targeted to MeArm Joystick board version 1.6.1.
 
 First joystick moves gripper forwards, backwards, left and right,
 button start/stop recording positions.
 
 Second joystick moves gripper up, down, and closes and opens,
 button start/stop playing recorded positions.
 Press button for 2 seconds to autoplay.
 
 Please send a short message as a feedback, if you uses this code. 
 Thank you. ;-)
 
 
 Pins:
 Arduino  Stick1 Stick2 Base   Shoulder Elbow  Gripper Record/    Infrared
 GND      GND    GND    Brown  Brown    Brown  Brown   Auto play  sensor
 5V       VCC    VCC    Red    Red      Red    Red     LED
 A0       HOR
 A1       VER
 PD2      BUTT
 A2              HOR
 A3              VER
 PD4             BUTT
 11                     Yellow
 10                            Yellow
 9                                      Yellow
 5                                             Yellow
 PD3                                                   X
 12                                                               X
 */

//#define infraredSensor true; // Uncomment this line, if you want to use Infrared controller. 

/* 
 And replace codes of infrared controller with your own codes in function CheckIrController.
 Set debug to true and use a Serial monitor of your Arduino IDE to show received codes of your remote controller.
 */

#include <Servo.h>

#if defined infraredSensor
#include <IRremote.h> 
#endif                      

bool debug = false;

bool repeatePlaying = false;   /* Repeatedly is running recorded cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

const int basePin = 11;       /* Base servo */
const int shoulderPin = 10;   /* Shoulder servo */
const int elbowPin = 9;       /* Elbow servo */
const int gripperPin = 5;     /* Gripper servo - changed */

int xdirPin = 0;        /* Base - joystick1*/
int ydirPin = 1;        /* Shoulder - joystick1 */
int zdirPin = 2;        /* Elbow - joystick2 */
int gdirPin = 3;        /* Gripper - joystick2 */

int pinRecord = PD2;     /* Button record */
int pinPlay = PD4;       /* Button play - changed */
int pinLedRecord = PD3;  /* LED - indicates recording (light) or auto play mode (blink ones) - changed */

int RECV_PIN = 12;      /* Infrared sensor */

const int buffSize = 412; /* Size of recording buffer */

int startBase = 90;
int startShoulder = 42;
int startElbow = 50;
int startGripper = 30;

int posBase = 90;
int posShoulder = 90;
int posElbow = 90;
int posGripper = 90;

int lastBase = 90;
int lastShoulder = 90;
int lastElbow = 90;
int lastGripper = 90;

int minBase = 0;
int maxBase = 150;
int minShoulder = 25;
int maxShoulder = 135;
int minElbow = 10;
int maxElbow = 118;
int minGripper = 10;
int maxGripper = 61;

const int countServo = 4;
byte currentPos[countServo] = {
  0, 0, 0, 0};
byte lastPos[countServo] = {
  255, 255, 255, 255};
byte minPos[countServo] = {
  minBase, minShoulder, minElbow, minGripper};
byte maxPos[countServo] = {
  maxBase, maxShoulder, maxElbow, maxGripper};
int servoPin[countServo] = {
  basePin, shoulderPin, elbowPin, gripperPin};


uint16_t buff[buffSize];
uint16_t buffAdd[countServo];
int recPos = 0;
int playPos = 0;
int blockPlayPos = 0;

int buffPos = 0;

struct tPlayBuff
{
  byte ServoNumber;
  byte Angle;
};

tPlayBuff playBuff[countServo];
int playBuffCount = 0;

byte cyclesRecord = 0;
byte cyclesPlay = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;

String command = "Manual";
int printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

int servoTime = 0;

float dx = 0;
float dy = 0;
float dz = 0;
float dg = 0;

Servo servoBase;
Servo servoShoulder;
Servo servoElbow;
Servo servoGripper;

#if defined infraredSensor
IRrecv irrecv(RECV_PIN);

decode_results results;

// Please, replace codes of infrared controller with your own codes 
// in function CheckIrController
void CheckIrController()
{
  if (irrecv.decode(&results))
  {
    switch (results.value)
    {
    case 0xBE15326E:
    case 0xDB9D3097:
      dx = 5;
      break;
    case 0x9912B99A:
    case 0x9FA96F:
      dx = -5;
      break;
    case 0xE5139CA7:
    case 0xAC516266:
      dy = 5;
      break;
    case 0xE4139B12:
    case 0xAD5163FB:
      dy = -5;
      break;
    case 0x21:
    case 0x821:
      dz = -5;
      break;
    case 0x20:
    case 0x820:
      dz = 5;
      break;
    case 0x11:
    case 0x811:
      dg = -5;
      break;
    case 0x10:
    case 0x810:
      dg = 5;
      break;
    case 0x1C102884:
    case 0x4B995E59:
      buttonRecord = !buttonRecord;
      break;
    case 0xC460AC26:
    case 0x6626F67F:
      buttonPlay = !buttonPlay;
      break;
    case 0xDF50BC53:
    case 0xF04EE0E6:
      repeatePlaying = !repeatePlaying;

      if (repeatePlaying)
      {
        record = false;
        play = true;  
      }
      else
      {
        play = false;
      }
      break;
    }
  }
}
#endif

void setup() {
  Serial.begin(9600);

  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin, INPUT);
  pinMode(gdirPin, INPUT);

  pinMode(pinRecord, INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord, OUTPUT);

  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);

  StartPosition();

#if defined infraredSensor
  irrecv.enableIRIn(); // Start the receiver
#endif  

  digitalWrite(pinLedRecord, HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void loop() {  
  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  dx = 0;
  dy = 0;
  dz = 0;
  dg = 0;


#if defined infraredSensor
  CheckIrController();

  if (debug)
  {
    Serial.println(results.value, HEX);
  }

  irrecv.resume(); // Receive the next value
#endif  

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatePlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if (buttonPlay == LOW)
    {
      play = !play;
      repeatePlaying = false;

      if (play)
      {
        //  StartPosition();
      }
    }
    buttonPlayLast = buttonPlay;
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play = false;
        repeatePlaying = false;
        recPos = 0;
        cyclesRecord = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
    buttonRecordLast = buttonRecord;
  }

  if (repeatePlaying)
  {
    play = true;
  }

  if (dx == 0) dx = map(analogRead(xdirPin), 0, 1023, 3.5, -3.5);
  if (dy == 0) dy = map(analogRead(ydirPin), 0, 1023, 4.0, -4.0);
  if (dz == 0) dz = map(analogRead(zdirPin), 0, 1023, 4.0, -4.0);
  if (dg == 0) dg = map(analogRead(gdirPin), 0, 1023, -4.0, 4.0);

  if (abs(dx) < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;

  if ((play) | (cyclesPlay > 0))
  {
    if (cyclesPlay > 0)
    {
      cyclesPlay--;

      if (cyclesPlay > 0)
      {
        if (play)
        {
          playPos = blockPlayPos;
        }
        else
        {
          play = false;
        }
      }
    }

    if (play)
    {
      if (playPos >= recPos)
      {
        playPos = 0;

        if (repeatePlaying)
        {
          if (debug)
          {
            Serial.println("Auto start ");
          }

          delay(delayBetweenCycles);
          // StartPosition();
        }
        else
        {
          play = false;
          repeatePlaying = false;
        }
      }

      blockPlayPos = playPos;
      playBuffCount = 0;

      bool endOfData = false;

      while (!endOfData)
      {
        if (playPos >= buffSize - 1) break;
        if (playPos >= recPos) break;

        uint16_t data = buff[playPos];
        byte angle = data & 0xFF;
        uint16_t servoNumber = data & 0xf00;
        endOfData = data & 0x8000;
        uint16_t repeatCycles = data & 0x7000;
        repeatCycles = repeatCycles >> 12;

        if (play & cyclesPlay <= 0)
        {
          cyclesPlay = repeatCycles;
        }

        servoNumber = servoNumber >> 8;

        playBuff[playBuffCount].ServoNumber = servoNumber;
        playBuff[playBuffCount].Angle = angle;

        playBuffCount++;

        switch (servoNumber)
        {
        case 0:
          posBase = angle;
          break;

        case 1:
          posShoulder = angle;
          break;

        case 2:
          posElbow = angle;
          break;

        case 3:
          posGripper = angle;
          dg = posGripper - lastGripper;
          break;
        }

        //        if (debug)
        //        {
        //          Serial.print("Play data ");
        //          Serial.print(data, BIN);
        //          Serial.print(" servo ");
        //          Serial.print(servoNumber);
        //          Serial.print(" angle ");
        //          Serial.print(angle);
        //          Serial.print(" end=");
        //          Serial.println(endOfData);
        //        }
        //

        playPos++;

        if (playPos >= recPos)
        {
          play = false;

          break;
        }
      }
    }
  }
  else
  {
    cyclesPlay = 0;
  }

  if (posBase > maxBase) posBase = maxBase;
  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
  if (posElbow > maxElbow) posElbow = maxElbow;
  if (posGripper > maxGripper) posGripper = maxGripper;

  if (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder) posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;

  bool waitGripper = false;
  if (dg < 0) {
    posGripper = minGripper;
    waitGripper = true;
  }
  else if (dg > 0) {
    posGripper = maxGripper;
    waitGripper = true;
  }

  if (play && waitGripper)
  {
    delay(1000);
  }

  currentPos[0] = posBase;
  currentPos[1] = posShoulder;
  currentPos[2] = posElbow;
  currentPos[3] = posGripper;

  bool positionChanged = false;

  if ((play) | (cyclesPlay > 0))
  {
    for (int i = 0; i < playBuffCount; i++)
    {
      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle, servoTime);
    }
  }
  else {
    for (int i = 0; i < countServo; i++)
    {
      if (currentPos[i] > maxPos[i]) currentPos[i] = maxPos[i];
      if (currentPos[i] < minPos[i]) currentPos[i] = minPos[i];

      if (currentPos[i] != lastPos[i])
      {
        positionChanged = true;
        MoveServo(servoPin[i], currentPos[i], servoTime);
      }
    }
  }

  if (positionChanged)
  {
    if (record)
    {
      buffPos = 0;
      cyclesRecord = 0;

      for (int i = 0; i < countServo; i++)
      {
        if (lastPos[i] != currentPos[i])
        {
          uint16_t value = (currentPos[i] | (0x100 * i)) & 0x0FFF;
          buffAdd[buffPos] = value;
          buffPos++;
        }
      }
      buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

      AddToBuff();
    }
  }
  else
  {
    if (record)
    {
      if (recPos > 0)
      {
        cyclesRecord ++;

        bool added = false;
        bool first = true;

        for (int i = recPos - 1; i >= 0; i--)
        {
          bool endOfData = buff[i] & 0x8000;

          if (!first && endOfData) break;

          if (cyclesRecord > 7)
          {
            cyclesRecord = 0;

            AddToBuff();
            added = true;
          }

          if (!added) {
            uint16_t val = cyclesRecord & 7;
            val = val << 12;
            val = val & 0x7000;

            buff[i] = buff[i] & 0x8FFF;
            buff[i] = buff[i] | val;

            //            if (debug)
            //            {
            //              Serial.print(" i=");
            //              Serial.print(i);
            //              Serial.print(" Buff[i]=");
            //              Serial.print(buff[i], BIN);
            //              Serial.print(" val=");
            //              Serial.println(val, BIN);
            //            }

            first = false;
          }
        }
      }
    }
  }

  lastBase = posBase;
  lastShoulder = posShoulder;
  lastElbow = posElbow;
  lastGripper = posGripper;

  for (int i = 0; i < countServo; i++)
  {
    lastPos[i] = currentPos[i];
  }

  if ( repeatePlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight = true;
    }
  };

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void AddToBuff()
{
  for (int i = 0; i < buffPos; i++)
  {
    buff[recPos + i] = buffAdd[i];
  }

  recPos += buffPos;

  if (recPos >= buffSize - countServo)
  {
    record = false;
  }
}

void MoveServo(uint8_t idServo, int angle, int timeMs)
{
  switch (idServo)
  {
  case basePin:
    servoBase.write(angle);
    break;
  case shoulderPin:
    servoShoulder.write(angle);
    break;
  case elbowPin:
    servoElbow.write(angle);
    break;
  case gripperPin:
    servoGripper.write(angle);
    break;
  }

  if (debug)
  {
    if (record) Serial.print(" Record ");
    if (play) Serial.print(" Play ");
    if (repeatePlaying) Serial.print(" Auto ");

    Serial.print(" Servo ");
    Serial.print(idServo);
    Serial.print(" Angle ");
    Serial.println(angle);
  }
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    uint16_t data = buff[i];
    byte angle = data & 0xFF;
    uint16_t servoNumber = data & 0xF00;
    servoNumber = servoNumber >> 8;
    bool endOfData = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber, HEX);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angleBase = servoBase.read();
  int angleShoulder = servoShoulder.read();
  int angleElbow = servoElbow.read();
  int angleGripper = servoGripper.read();

  Serial.print(angleBase);
  Serial.print("\t");
  Serial.print(angleShoulder);
  Serial.print("\t");
  Serial.print(angleElbow);
  Serial.print("\t");
  Serial.print(angleGripper);
  Serial.println("\t");

  posBase = startBase;
  posShoulder = startShoulder;
  posElbow = startElbow;
  posGripper = startGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoGripper.write(posGripper);
}