Help with arduino code!

Using Arduino Programming Questions
1

Predoblikovano besediloI'm working on this project :
Joystick Controller for MeArm Robot - Recording Coordinates - Arduino Project Hub

I will us this code for 6Dof Robotic Arm . I have instead of 2 ,3 joysticks and instead 4, 6 servos. Manual everything works fine ,except of recording and playing.
When i record and play some of the servos working and some don´t. What do i need to change in code to work ? With orginal code for 2 joystick and 4 servos record and play works with no problem.
My code [code]


#include <Servo.h>

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

int basePin = 11;       /* Base servo */
int shoulderPin = 10;   /* Shoulder servo */
int elbowPin = 9;       /* Elbow servo */
int gripperPin = 6;     /* Gripper servo */
int foreArmPin = 7;
int wristPin = 8;

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


//int pinRecord = A4;     /* Button record - backward compatibility */
//int pinPlay = A5;       /* Button play  - backward compatibility */
int pinRecord = PD2;     /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */
int pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used for additional joystick) */
int pinLedRecord = PD4;  /* LED - indicates recording (light) or auto play mode (blink one) */

bool useInternalPullUpResistors = false;

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

int startBase = 90;
int startShoulder = 90;
int startElbow = 90;
int startGripper = 0;
int startForeArm = 90;
int startWrist = 90;

int posBase = 90;
int posShoulder = 90;
int posElbow = 90;
int posGripper = 0;
int posForeArm = 90;
int posWrist = 90;

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

int minBase = 0;
int maxBase = 150;
int minShoulder = 0;
int maxShoulder = 150;
int minElbow = 0;
int maxElbow = 150;
int minGripper = 0;
int maxGripper = 150;
int minForeArm = 0;
int maxForeArm = 150;
int minWrist = 0;
int maxWrist = 150;

const int countServo = 6;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool play = false;
bool debug = false;

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

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool ledLight = false;

Servo servoBase;
Servo servoShoulder;
Servo servoElbow;
Servo servoGripper;
Servo servoForeArm;
Servo servoWrist;

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

  if (useInternalPullUpResistors) {
    pinMode(pinRecord, INPUT_PULLUP);
    pinMode(pinPlay, INPUT_PULLUP);
  }
  else
  {
    pinMode(pinRecord, INPUT);
    pinMode(pinPlay, INPUT);
  }

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

  pinMode(pinLedRecord, OUTPUT);

  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);
  servoForeArm.attach(foreArmPin);
  servoWrist.attach(wristPin);

  StartPosition();

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

void loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  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();
      }
    }
  }

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

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

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);
  float ds = map(analogRead(sdirPin), 0, 1023, 5.0, -5.0);
  float dr = map(analogRead(rdirPin), 0, 1023, 5.0, -5.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;
  if (abs(ds) < 1.5) ds = 0;
  if (abs(dr) < 1.5) dr = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;
  posForeArm += ds;
  posWrist += dr;

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

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

    bool endOfData = false;

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

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x3000;
      endOfData = data & 0x4000;

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

        case 0x1000:
          posShoulder = angle;
          break;

        case 0x2000:
          posElbow = angle;
          break;

        case 0x3000:
          posForeArm = angle;
          break;

        case 0x4000:
          posWrist = angle; 
          break;    

        case 0x5000:
          posGripper = angle;
          dg = posGripper - lastGripper;
          break;

        
      }

      playPos++;
    }
  }

  if (posBase > maxBase) posBase = maxBase;
  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
  if (posElbow > maxElbow) posElbow = maxElbow;
  if (posGripper > maxGripper) posGripper = maxGripper;
  if (posForeArm > maxForeArm) posForeArm = maxForeArm;
  if (posWrist > maxWrist) posWrist = maxWrist;

  if (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder) posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;
  if (posForeArm < minForeArm) posForeArm = minForeArm;
  if (posWrist < minWrist) posWrist = minWrist;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoForeArm.write(posForeArm);
  servoWrist.write(posWrist);

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

  servoGripper.write(posGripper);
  if (play && waitGripper)
  {
    delay(1000);
  }

  if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastForeArm != posForeArm) | (lastWrist != posWrist) | (lastGripper != posGripper))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lastBase != posBase)
        {
          buffAdd[buffPos] = posBase;
          buffPos++;
        }

        if (lastShoulder != posShoulder)
        {
          buffAdd[buffPos] = posShoulder | 0x1000;
          buffPos++;
        }

        if (lastElbow != posElbow)
        {
          buffAdd[buffPos] = posElbow | 0x2000;
          buffPos++;
  
        }

        if(lastForeArm != posForeArm)
        {
          buffAdd[buffPos] = posForeArm | 0x3000;
          buffPos++;
          
        }
          
        if(lastWrist != posWrist)
        {
          buffAdd[buffPos] = posWrist | 0x4000;
          buffPos++;  
        }

        if (lastGripper != posGripper)
        {
          buffAdd[buffPos] = posGripper | 0x5000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x6000;

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

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\t");
    Serial.print(printPos);
    Serial.print("\t");
    Serial.print(posBase);
    Serial.print("\t");
    Serial.print(posShoulder);
    Serial.print("\t");
    Serial.print(posElbow);
    Serial.print("\t");
    Serial.print(posForeArm);
    Serial.print("\t");
    Serial.print(posWrist);
    Serial.print("\t");
    Serial.print(posGripper);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lastBase = posBase;
  lastShoulder = posShoulder;
  lastElbow = posElbow;
  lastForeArm = posForeArm;
  lastWrist = posWrist;
  lastGripper = posGripper;

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

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

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

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x3000;
    bool endOfData = data & 0x4000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    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 angleForeArm = servoForeArm.read();
  int angleWrist = servoWrist.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(angleForeArm);
  Serial.print("\t");
  Serial.print(angleWrist);
  Serial.print("\t");
  Serial.print(angleGripper);
  Serial.println("\t");

  posBase = startBase;
  posShoulder = startShoulder;
  posElbow = startElbow;
  posForeArm = startForeArm;
  posWrist = startWrist;
  posGripper = startGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoForeArm.write(posForeArm);
  servoWrist.write(posWrist);
  servoGripper.write(posGripper);
}
[/code]
Thanks.
2

Read the forum guidelines. Post your test code in code tags. Use the IDE autoformat tool (ctrl-t or Tools, Auto Format) to indent the code for readability before posting code.

3

Oh and it might be useful to say WHICH of the servos don't work.

4

That code HAS to be from instructables. :scream:

5

using IO-pins 0 and 1 is a very bad idea because those are the two pins that are used for the serial connection.

modifying code where the modifying goes beyond changing a text on a display or the frequency of a blinking LED in 98% of all cases ges wrong if the modifyier does not understand how the code works.

Attentionmarks in the headline don't help at all. The only thing attentionsmarks in the headline do is indicating the user is in a hurry or is expecting to get exclusivly the quickest help ever possible.
In a userforum where everybody works for free you can't expect that.

The grammar of your english is wrong in some places. It is still understandable but the combination of beeing a beginner in programming and not a native english speaker might lead to misunderstandings.
So my recommendation is to write your questions in your native language and let do google-translate the translation. The grammar of google-translate is not brilliant but this way you ensure to be able to describe everything in detail because you know all the words.

Still you are free to try it quick & dirty just to find it out trying it quick & dirty ends up in consuming more time because others are asking back for details.

best regards Stefan

6

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.