Storing multiple servo positions for robot arm

Using Arduino Networking, Protocols, and Devices
1

I'm working on a robot arm with multiple servos on an Adafruit PWM Servo Driver. I have code so I can adjust each of their positions using a potentiometer. Then I note the position of a servo, move to the next one, adjust it, note its position, etc. This allows me to set the arm into a position, tweaking each servo until all of them are just right. The position of each servo is shown on an OLED screen. The servo positions can be used in a function to get the robot to move into a specific position.

What I have now uses the following:
Arduino
Adafruit PWM Servo driver
Potentiometer
Buttons
OLED screen

/***************************************************
  Potentiometer + Adafruit PWM Servo Driver + OLED + Buttons
  
  Adafruit PWM & pot:
  https://forums.adafruit.com/viewtopic.php?f=31&t=119704

  Connections:
  Adafruit PWM to 5V, GND, SDA and SCL pins
  OLED to 5V, GND, SDA and SCL pins
  Potentiometer to A0

  11/9/21: Changed from LCD to OLED using U8g2 library.
  Works great!  Picked best font for clarity.
  11/9/21: Added JCButton library to manage switching servos
 ****************************************************/

#include <Wire.h>

// JC Button library
#include <JC_Button.h>
// JC Button pin assignments
const byte 
        DN_PIN(7),  
        UP_PIN(8);  // connect a button from these pins to ground
                            
Button btnUP(UP_PIN), btnDN(DN_PIN);    // define the buttons

const unsigned long
          REPEAT_FIRST(500),              // ms required before repeating on long press
          REPEAT_INCR(100);               // repeat interval for long press
const int 
          MIN_COUNT(0),
          MAX_COUNT(16);


// OLED - U8g2 library
#include <Arduino.h>
#include <U8x8lib.h>

U8X8_SSD1306_128X64_NONAME_HW_I2C u8x8(/* reset=*/ U8X8_PIN_NONE);

// Adafruit PWM
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

// pot constants
const int potPin1 = A0;    //potentiometer pin

// variables
int val;
int pulseLength;

void setup() {
  btnUP.begin();
  btnDN.begin();

  u8x8.begin();
  u8x8.setPowerSave(0);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  yield();
}

void loop() {

  static int
  count,                          // the number that is adjusted
  lastCount(-1);                  // previous value of count (initialized to ensure it's different when the sketch starts)
  static unsigned long
  rpt(REPEAT_FIRST);              // a variable time that is used to drive the repeats for long presses
  enum states_t {WAIT, INCR, DECR};   // states for the state machine
  static states_t STATE;              // current state machine state

  btnUP.read();                   // read the buttons
  btnDN.read();

  if (count != lastCount)         // print the count if it has changed
  {
    lastCount = count;
    Serial.println(count, DEC);
  }

  switch (STATE)
  {
    case WAIT:                              // wait for a button event
      if (btnUP.wasPressed())
        STATE = INCR;
      else if (btnDN.wasPressed())
        STATE = DECR;
      else if (btnUP.wasReleased())       // reset the long press interval
        rpt = REPEAT_FIRST;
      else if (btnDN.wasReleased())
        rpt = REPEAT_FIRST;
      else if (btnUP.pressedFor(rpt))     // check for long press
      {
        rpt += REPEAT_INCR;             // increment the long press interval
        STATE = INCR;
      }
      else if (btnDN.pressedFor(rpt))
      {
        rpt += REPEAT_INCR;
        STATE = DECR;
      }
      val = analogRead(potPin1);            // reads the value of the potentiometer (value between 0 and 1023)
      pulseLength = map(val, 0, 1023, SERVOMIN, SERVOMAX);      //map potentiometer to SERVOMIN and SERVOMAX
      pwm.setPWM(count, 0, pulseLength);

      //u8x8.setFont(u8x8_font_courB18_2x3_f);  // This is good, fits single-digit servo
      u8x8.setFont(u8x8_font_inr21_2x4_f);  // Good one!


      // Display selected servo
      u8x8.setCursor(0, 0);
      u8x8.print("Servo:");

      if (count < 10) {
      u8x8.setCursor(14, 0);
      u8x8.print(" ");      // Need this to get rid of flickering & stray pixels from numbers over 10
      u8x8.setCursor(12, 0);
      u8x8.print(count);          
      }
      else {
      u8x8.setCursor(12, 0);
      u8x8.print(count);       
      }

      // Display Pulse Length of servo position
      u8x8.setCursor(0, 4);
      u8x8.print("Pul:");
      u8x8.setCursor(8, 4);
      u8x8.print(pulseLength);

      break;

    case INCR:
      ++count;                            // increment the counter
      count = min(count, MAX_COUNT);      // but not more than the specified maximum
      STATE = WAIT;
      break;

    case DECR:
      --count;                            // decrement the counter
      count = max(count, MIN_COUNT);      // but not less than the specified minimum
      STATE = WAIT;
      break;
  }
}

This works fine so far, but the problem is that the servo positions aren't stored. Let's say I move the robot shoulder to a position, then press the button to move to the elbow servo. I adjust that, then realize that the shoulder should be a little different. I press a button to go back to the shoulder servo, but the position of the pot for the elbow is read and the shoulder moves to that angle.

I would like to be able to toggle through all servos, setting their angles, without having to write down the current angle of servo before moving to the next. Then at the end I should be able to toggle through the servos, and see their angles and record them.

Also the idea is to have this as a free-standing board that doesn't need to be connected to a PC to work, that's why I'm using the OLED.

What's the best approach? I have a couple of rotary encoders available. I can see incrementing/decrementing the servo angles that way. Or should I continue with the pot and add an array, adding an extra button to change or store the value?

2

Matrices https://youtube.com/user/asodemann3 see Robotics topics

3

I think that you need some more buttons to control everything. A pair of buttons to select a servo, one more to select a position, and one button to copy the pot value to the currently selected servo. It may be easier to have one "store" (or "follow") button for each servo and a pair of buttons to cycle through positions.

Then have an array of [position, servo, value] where the servo values are stored. When you move from one position to another one then the stored values of that position are copied to the servos. Then select a servo and adjust the pot for it. As long as you press the "store" button the servo follows the pot and the value is stored.

4

I think there is a principle problem with your approach: When setting the servo positions with one pot for all servos, the last set position is stored with the mechanical potentiometer position. When switching to another servo you will always get the position from the previous servo because of the mechanical position of the pot.
Consider switching to a rotary encoder to change the servo postion.

5

I agree. After a walk I realized I should set up an array, and initialize it to values in the center of the servo travel at boot up. Then the encoder can change a value in the array, and the new value would be copied to the servo. Go to the next servo/value in the array and change it if needed. Go back to adjust earlier ones. When you're done you can flip through all the servos and record the new values.

For revising older settings you have to look up the settings, turn it on, enter them (which should be quick) and then adjust as needed. Update the values for the new settings, which will be kept in a spreadsheet.

6

I solved the problem, the code is here:

Thanks for the feedback.

7

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