Help with servo motor controlling

poypoy007 · March 17, 2024, 9:27pm

Hi, so I have 4 servo motors and 2 joysticks. 1 joystick controls 2 motors. I want to make the servos rotate faster, I dont remember where I took the code but it works well so far. I just cant find how to make them rotate faster. I messed around with the code to do it but I only managed to make one of them faster. I just need to make em rotate faster. Also I feel like all this could be done with a shorter code so I would really appreciate your suggestions

EDIT: I think the code is actually to make some sort of robot which is why most of the lines arent useful. I would clear them but Im 1000% I would screw smth up


#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 xdirPin = 0; /* Base - joystick1*/
int ydirPin = 1; /* Shoulder - joystick1 */
int zdirPin = 3; /* Elbow - joystick2 */
int gdirPin = 2; /* Gripper - joystick2 */

//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 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 = 360;
int minShoulder = 0;
int maxShoulder = 360;
int minElbow = 0;
int maxElbow = 360;
int minGripper = 0;
int maxGripper = 360;

const int countServo = 4;
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;

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(pinLedRecord, OUTPUT);

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

  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);

  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) {
    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:
          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 (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder) posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);

  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) | (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 (lastGripper != posGripper) {
          buffAdd[buffPos] = posGripper | 0x3000;
          buffPos++;
        }

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

        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(posGripper);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

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

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

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

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

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

xfpd · March 18, 2024, 5:40am

The fastest you can rotate a servo is to send it to a position without a loop, for example

#include<Servo.h>
Servo myServo;
int servoPin = ??;
void setup() {
  myServo.attach(servoPin);
}
void loop() {
  myServo.write(0);
  delay(500);
  myServo.write(180);
  delay(500);
}

ruilviana · March 18, 2024, 12:25pm

Which Arduino are you using?

poypoy007 · March 19, 2024, 1:20pm

Oh yea I forgot to say it. Its an Arduino UNO

ruilviana · March 19, 2024, 1:26pm

These are not Arduino pins

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; /

poypoy007 · March 19, 2024, 1:28pm

That could work but I want to rotate them with the joystick. Like when I push the joystick to the x axis, one of the motors connected to it moves to the same direction I moved the joystick to and vice versa with the y axis. I can do this with my current code. I just need to make the motors rotate faster

poypoy007 · March 19, 2024, 1:31pm

Yeah like I said I took the code from somewhere but cant remember where. I would clean the useless codes but I probably would screw something up. I will try to do that later tho

jim-p · March 19, 2024, 1:47pm

@poypoy007
The Knob.ino example show you how to do it for one servo and it's just 10 lines of code.
Find it under Files->Examples->Servo
You should be able to easily add another joystick a more servos.

xfpd · March 19, 2024, 2:31pm

If the servo library is not right for you, adjust the pulse width with the joystick:

Files for WOKWI.COM

sketch.ino

/*
  Servo PWM without library

  Most 180 degree servos use a pulse train with 5% to 10% duty cycle at 50 kHz (50000 pulses per second)
  1ms to 2 ms (1000us to 2000us) ON, 19ms to 18ms OFF (18000us to 19000us) over 20ms (20000us)
    _                     _                    x
  _|1|_______________19.0| |___________________x   0 degrees
    ___                   ___                  x
  _|1.5|_____________18.5|   |_________________x  90 degrees
    ____                  ____                 x
  _|2.0 |____________18.0|    |________________x 180 degrees

  - start timer
  - write PWM HIGH = begin EVENT 1
  - check timer for 1ms to 2ms (the angle you need, adjusted for your servo)
  - write PWM LOW = end EVENT 1
  - wait 19ms to 18ms (note this is the 20ms remainder of the 1ms to 2ms pulse)
  - any number of EVENTs can be started and ended at any time
*/

unsigned long totalTime = 20000; // PWM time a 20ms pulse cycle
unsigned long currentMicros, previousMicros = 0; // timers

#define DEBUG 1 // 1 = print values

#ifdef DEBUG
int oldTime;
#endif

#define POTENTIOMETERpin A0 // potentiometer pin
const byte servoPin[] = {9, 10}; // servo pin

void setup() {
  if (DEBUG)
    Serial.begin(115200); // for debug

  pinMode(servoPin[0], OUTPUT); // servo data pin
  pinMode(servoPin[1], OUTPUT);
  digitalWrite(servoPin[0], HIGH); // begin "ON" pulse
  digitalWrite(servoPin[1], HIGH);
}

void loop() {
  bool timeONFlag, timeOFFFlag; // indicate xTime has finished
  int potentiometerValue; // potentiometer reading

  potentiometerValue = analogRead(POTENTIOMETERpin); // read the potentiometer
  // Map 450us to 2550us for simulator. Use 1000us to 2000us (1ms to 2ms) for average servo
  unsigned long timeON = map(potentiometerValue, 0, 1023, 440, 2550);
  unsigned long timeOFF = totalTime - timeON; // calculate timeOFF LOW signal

  if (DEBUG) {
    if (oldTime != timeON) {
      Serial.print("Pulse width: ");
      Serial.println(timeON);
      oldTime = timeON;
    }
  }
  currentMicros = micros(); // start a new pulse train timer

  // EVENT 1 - PWM OFF - occurs between 1ms and 2ms (1000us and 2000us) after start pulse to end HIGH pulse
  if (currentMicros - previousMicros >= timeON) { // bypass this for condition 1000us to 2000us
    if (timeONFlag == 0) { // test if timeON has previously been tested
      digitalWrite(servoPin[0], LOW); // set pulse OFF for timeOFF
      digitalWrite(servoPin[1], LOW);
      timeONFlag = 1; // timeON is finished, do not enter this condition until end of timeOFF
      // previousMicros = currentMicros; // previousMicros is new "zero" for timeOFF
    }
  }

  // EVENT 2 - PWM ON - occurs every 20ms (20000us) to start a new pulse
  if (currentMicros - previousMicros >= timeOFF) { // bypass this for condition 18000us to 19000us
    timeONFlag = 0; // timeOFF is finished at 20ms. Reset timeONFlag
    digitalWrite(servoPin[0], HIGH); // set pulse ON to begin timeON
    digitalWrite(servoPin[1], HIGH); // set pulse ON to begin timeON
    previousMicros = currentMicros; // previousMicros is new reference time "zero"
  }
}

diagram.json

{
  "version": 1,
  "author": "Anonymous maker",
  "editor": "wokwi",
  "parts": [
    { "type": "wokwi-arduino-nano", "id": "nano", "top": -14.4, "left": -10.1, "attrs": {} },
    {
      "type": "wokwi-servo",
      "id": "servo2",
      "top": -183.4,
      "left": -94.2,
      "rotate": 270,
      "attrs": {}
    },
    { "type": "wokwi-potentiometer", "id": "pot1", "top": -193.3, "left": 134.2, "attrs": {} },
    {
      "type": "wokwi-servo",
      "id": "servo1",
      "top": -183.4,
      "left": -7.8,
      "rotate": 270,
      "attrs": {}
    }
  ],
  "connections": [
    [ "nano:GND.1", "servo2:GND", "black", [ "v9.6", "h-153.6" ] ],
    [ "nano:5V", "servo2:V+", "red", [ "v19.2", "h-144", "v-86.4", "h19.1" ] ],
    [ "nano:A0", "pot1:SIG", "green", [ "v28.8", "h134.8" ] ],
    [ "pot1:GND", "nano:GND.2", "black", [ "v105.6", "h-48" ] ],
    [ "pot1:VCC", "nano:5V", "red", [ "v192", "h-68" ] ],
    [ "nano:10", "servo2:PWM", "green", [ "v-20.27", "h-24.9" ] ],
    [ "servo1:PWM", "nano:9", "green", [ "v9.6", "h-47.8" ] ],
    [ "nano:GND.2", "servo1:GND", "black", [ "v-10.67", "h-44.1" ] ],
    [ "servo2:V+", "servo1:V+", "red", [ "v19.2", "h86.4" ] ]
  ],
  "dependencies": {}
}

ruilviana · March 19, 2024, 2:36pm

@poypoy007
See if meet your need.

PS: Read last code line...

poypoy007 · March 19, 2024, 6:48pm

This works but it slowly moves every servo motor to the left even when Im not touching anything.(also crashed my laptop but it crashes when opening minecraft so its nothing to worry about ig)

poypoy007 · March 19, 2024, 7:42pm

actually that works almost perfectly. I just need to switch the small servos with big ones and rename them in the code and it will be complete. Thanks

system · September 15, 2024, 7:43pm

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

Topic		Replies	Views
Servo with joystick x and y question General Guidance	19	249	May 18, 2025
Control 5 servos 3 joysticks recording coordinates arduino uno Programming	33	287	May 18, 2025
Servo Motor Joint Drive Programming	52	451	June 30, 2025
Slow Down servos compared to mimic controller Programming	14	132	July 5, 2025
Basic programming Continuous servo Programming	83	402	December 12, 2025

Help with servo motor controlling

Related topics