Adafruit 12bit PWM ServoShield - controling servos with pushbuttons problem

Projects General Guidance
1

Hi! It's my first post on this forum, so it is my duty to greet everyone thats willing to help people like me overcome trivial, and those less trivial problems with Arduino and so on!

The PR part is done, so it's time to describe my little problem :slight_smile:

My problem is associated with programming of the Adafruit 12bit PWM Servoshield.

One pushbutton is used to select the motors, the rest to control servo position.

Motors are arranged in this order:
( 1 )
( 2,3 )
( 4,5 )
( 6 )
( 7 )
( 8 )

The code i wrote was supposed to be as simple as possible, but something went wrong.

All motors are moving at once trying to reach maximum position, and only one pushbutton is working. If pressed, all motors are going to minimum. If not pressed, all servos are trying to reach maximum again.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Servo.h>
#include <LiquidCrystal.h>

int button1 = 6; //defines button (button1), and the pin (1)
int button2 = 7; //defines button (button2), and the pin (2)
int button3 = 8; //defines button (button3), and the pin (3)

byte buttonPin = 2;
byte buttonPresses = 0;
byte lastPressCount = 0;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

void setup() {
  pinMode(buttonPin, INPUT);
  digitalWrite(buttonPin, LOW);
  Serial.begin(9600);

  Serial.println("Initializing program...");

  pwm.begin();

  pwm.setPWMFreq(60);

  yield();

#define SERVOMIN  150
#define SERVOMAX  455

  pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);

  LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
  lcd.begin(16, 2);

}

void setServoPulse(uint8_t n, double pulse) {
  double pulselength;

  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  Serial.print(pulselength); Serial.println(" us per period");
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit");
  pulse *= 1000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void loop()
{
  LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
  lcd.begin(16, 2);
  {

    if (digitalRead(buttonPin) == LOW)
    {
      buttonPresses++;
      delay(250);
    }
    if (buttonPresses == 7)buttonPresses = 0;
    if (lastPressCount != buttonPresses)
    {
      Serial.print("Press counter= ");
      Serial.println(buttonPresses, DEC);
      {
        if (buttonPresses == 1)
          lcd.print("Base rotation");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(15, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(15, 0, SERVOMAX);
          }
        }
        if (buttonPresses == 2)
          lcd.print("Control JOINT 1");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(14, 0, SERVOMIN);
            pwm.setPWM(13, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(14, 0, SERVOMAX);
            pwm.setPWM(13, 0, SERVOMAX);
          }
        }
        if (buttonPresses == 3)
          lcd.print("Control JOINT 2");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(11, 0, SERVOMIN);
            pwm.setPWM(10, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(11, 0, SERVOMAX);
            pwm.setPWM(10, 0, SERVOMAX);
          }
        }
        if (buttonPresses == 4)
          lcd.print("Control JOINT 3");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(7, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(7, 0, SERVOMAX);
          }
        }
        if (buttonPresses == 5)
          lcd.print("Gripper rotation");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(5, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(5, 0, SERVOMAX);
          }
        }
        if (buttonPresses == 6)
          lcd.print("Gripper control");
        {
          if (digitalRead(button2) == LOW)
          {
            pwm.setPWM(3, 0, SERVOMIN);
          }
          if (digitalRead(button3) == LOW)
          {
            pwm.setPWM(3, 0, SERVOMAX);
          }
        }
        lastPressCount = buttonPresses;
      }
    }

  }
}

The general assumptions are:

  • Use [button1] to select arranged motors, and [button2] and [button3] to control the position
  • Control position up/down only when button is pressed (not min->max or max->min)
  • Display current position on lcd (i had no idea how do this, so its not included anywhere in the code)

I hope that someone will point out my mistakes and/or correct the code, because im out of ideas right now. :frowning:

2

Can you control a single servo, using setServoPulse or setPWM?

Learn how do debounce buttons, the Debounce and StateChangeDetection examples will be very helpful.

If you want to make the deselected servos keep their positions, use an array containing the current position of every servo. Then use buttons to increment or decrement the position value for the selected motor, and make that motor move to the new position.

3

Thanks for response. After few hours of googling and putting together a lot of stuff, I ended up with this:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <LiquidCrystal.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define BAZA 15
#define RAMIE10 14
#define RAMIE11 13
#define RAMIE20 11
#define RAMIE21 10
#define RAMIE3 7
#define KISC 5
#define CHWYTAK 3

float baza_init = 1440;
float ramie10_init = 1675;
float ramie11_init = 1675;
float ramie20_init = 1440;
float ramie21_init = 1440;
float ramie3_init = 1440;
float kisc_init = 1440;
float chwytak_init = 2155;

float baza_current = baza_init;
float ramie10_current = ramie10_init;
float ramie11_current = ramie11_init;
float ramie20_current = ramie20_init;
float ramie21_current = ramie21_init;
float ramie3_current = ramie3_init;
float kisc_current = kisc_init;
float chwytak_current = chwytak_init;

float SERVOFREQ = 50;
float pulseconstant;
boolean limit_reached = false;

const int buttonPin2 = 6;
const int buttonPin3 = 8;
const int buttonPin = 7;

int mode = 0; 
int val = 0; 
int buttonState = 0;
int buttonState2 = 0;
int buttonState3 = 0;
int modeState = 0; 
boolean debug = 1; // 1 = Print Serial Enabled / 0 = disabled

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);


void setup() {
  Serial.begin(9600);
  
  pulseconstant = (1000000 / SERVOFREQ) / 4096;

  pwm.begin();
  pwm.setPWMFreq(SERVOFREQ);

  servoWrite(BAZA, baza_init);
  servoWrite(RAMIE10, ramie10_init);
  servoWrite(RAMIE11, ramie11_init);
  servoWrite(RAMIE20, ramie20_init);
  servoWrite(RAMIE21, ramie21_init);
  servoWrite(RAMIE3, ramie3_init);
  servoWrite(KISC, kisc_init);
  servoWrite(CHWYTAK, chwytak_init);
}

void loop() {
  buttonState2 = digitalRead(buttonPin2);
  buttonState3 = digitalRead(buttonPin3);

  val = digitalRead(buttonPin);

 
  if (val != buttonState && val == LOW) {
    mode++;
  }

  buttonState = val;

  if (modeState != mode) {

    // If no keys have been pressed yet don't execute
    // the switch code below
    // if (mode != 0) {

    switch ( mode ) {

      case 2:
        {
 
          Serial.print("Ramie1");
          lcd.print("Sterowanie Ramieniem 1");
          if (ramie10_current - 10 <= 525 && buttonState3 == LOW) {
            limit_reached = true;
            return;
          } else if (ramie10_current + 10 >= 2355 && buttonState2 == LOW) {
            limit_reached = true;
            return;
          } else {

            if (buttonState2 == LOW) {
              ramie10_current += 10;
            }
            if (buttonState3 == LOW) {
              ramie10_current -= 10;
            }
          }
        }
        showState();
        break;

      case 3:
   {
          // check to stop the servo
          Serial.println("Ramie2");
          lcd.print("Sterowanie Ramieniem 2");
          if (ramie20_current - 10 <= 525 && buttonState3 == LOW) {
            limit_reached = true;
            return;
          } else if (ramie20_current + 10 >= 2355 && buttonState2 == LOW) {
            limit_reached = true;
            return;
          } else {
            if (buttonState2 == LOW) {
              ramie20_current += 10;
            }
            if (buttonState3 == LOW) {
              ramie20_current -= 10;
            }
          }
        }
  
        showState();
        break;

      case 4:
 {
          Serial.print("Ramie3");
          lcd.print("Sterowanie Ramieniem 3");
          if (ramie3_current - 10 <= 525 && buttonState3 == LOW) {
            limit_reached = true;
            return;
          } else if (ramie3_current + 10 >= 2355 && buttonState2 == LOW) {
            limit_reached = true;
            return;
          } else {
            // handle movement
            if (buttonState2 == LOW) {
              ramie3_current += 10;
            }
            if (buttonState3 == LOW) {
              ramie3_current -= 10;
            }
          }
        }

        showState();
        break;

      case 5:
{
          Serial.print("Obrot");
          lcd.print("Obrot chwytaka");
          if (kisc_current - 10 <= 525 && buttonState3 == LOW) {
            limit_reached = true;
            return;
          } else if (kisc_current + 10 >= 2355 && buttonState2 == LOW) {
            limit_reached = true;
            return;
          } else {
            if (buttonState2 == LOW) {
              kisc_current += 10;
            }
            if (buttonState3 == LOW) {
              kisc_current -= 10;
            }
          }
        }

        showState();
        break;

      case 6:
{
          Serial.print("Chwytak");
          lcd.print("Sterowanie Chwytakiem");
          if (chwytak_current - 10 <= 525 && buttonState3 == LOW) {
            limit_reached = true;
            return;
          } else if (chwytak_current + 10 >= 2355 && buttonState2 == LOW) {
            limit_reached = true;
            return;
          } else {
            if (buttonState2 == LOW) {
              chwytak_current += 10;
            }
            if (buttonState3 == LOW) {
              chwytak_current -= 10;
            }
          }
        }

        showState();
        break;

      default:
        mode = 1;
{
          Serial.println("Obrot bazy");
          lcd.print("Obrot Bazy");
          if (baza_current - 10 <= 525 && buttonState3 == HIGH) {
            limit_reached = true;
            return;
          } else if (baza_current + 10 >= 2355 && buttonState2 == HIGH) {
            limit_reached = true;
            return;
          } else {
            if (buttonState2 == LOW) {
              baza_current += 10;
            }
            if (buttonState3 == LOW) {
              baza_current -= 10;
            }
          }
        }

        showState();
        break;

    }

  } 

  modeState = mode; 
  delay(10); 

}
void showState() {
  if (debug) {
    Serial.print("Mode: ");
    Serial.println(mode);
  }
}
void servoWrite(uint8_t n, float pulse) {
  float pulsetick = pulse / pulseconstant;
  pwm.setPWM(n, 0, pulsetick);
}

This time all motors go to *_init position given on the begining of the program and keep this value. Mode switching works as well (or maybe only serial display says so), but the case itself isn't working. There is no answer to +/- buttons (buttonPin2,buttonPin3).

Is there a problem with cases itself or the buttonState command?

4

Perhaps it helps if you debounce at least buttonPin properly.