PS3BT 3 servo hexapod

Projects General Guidance
1

Hi all,

I'm starting a new build, a 3 servo hexapod. I have arduino uno, usb shield, ps3 controller, BT dongle, mini servos etc.

I have the ps3 controller successfully talking to the uno and can run the PS3BT sketch no problem.

I can run the servos in a forward loop with or without the controller connected.

My question is, How do I stop it from moving?

Here is my code. I don't think the controller is doing anything at the moment?. I have commented out the other movements for now, until I can get the basics working.

#include <PS3BT.h>                    //Include the necessary libraries. 
#include <Servo.h> 

USB Usb; 
BTD Btd(&Usb); 
PS3BT PS3(&Btd);

Servo servoLeft;          // Define left servo
Servo servoRight;        // Define right servo
Servo servoCenter;      //Define center servo  

void setup() { 
    Serial.begin(115200);                     
  if (Usb.Init() == -1) {                     
    Serial.print(F("\r\nOSC did not start")); 
    while(1); //halt 
  } 
  Serial.print(F("\r\nPS3 Bluetooth Library Started")); 
  
  servoLeft.attach(3);  // Set left servo to digital pin 3
  servoRight.attach(5);
  servoCenter.attach(6);  // Set right servo to digital pin 6
 

} 

void loop() {   // Loop through motion tests

Usb.Task(); 

  if(PS3.PS3Connected || PS3.PS3NavigationConnected) {
 
    
     if(PS3.getAnalogHat(RightHatX) >145); 
  {forward();}                                           // Example: move forward
    
     if(PS3.getAnalogHat(RightHatX) <=135);
   {stopRobot();}
 }
}

// Motion routines for forward
void forward() {
  servoCenter.write(50);
  delay(250);
  servoRight.write(160);
  servoLeft.write(10);
  delay(250);
  servoCenter.write(120);
  delay(250);
  servoRight.write(10);
  servoLeft.write(160);
  delay(250);
  
} 


void stopRobot() {
  servoLeft.write(90);
  servoRight.write(90);
  servoCenter.write(90);
}
2

The first thing to do is get rid of the semi-colon at the end of these lines:-

if(PS3.getAnalogHat(RightHatX) >145);
if(PS3.getAnalogHat(RightHatX) <=135);

and the ones at the end of the similar lines that are commented out, if you plan to uncomment and use them.

It would have been much easier to read if you'd deleted the commented out lines when you posted the code, so that it looked like this:-
(And I removed the offending semicolons.)

#include <PS3BT.h>                    //Include the necessary libraries.
#include <Servo.h>

USB Usb;
BTD Btd(&Usb);
PS3BT PS3(&Btd);

Servo servoLeft;          // Define left servo
Servo servoRight;
Servo servoCenter;        // Define right servo

void setup()
{
    Serial.begin(115200);
    if (Usb.Init() == -1)
    {
        Serial.print(F("\r\nOSC did not start"));
        while (1); //halt
    }
    Serial.print(F("\r\nPS3 Bluetooth Library Started"));

    servoLeft.attach(3);  // Set left servo to digital pin 10
    servoRight.attach(5);
    servoCenter.attach(6);  // Set right servo to digital pin 9
}

void loop()     // Loop through motion tests
{
    Usb.Task();

    if (PS3.PS3Connected || PS3.PS3NavigationConnected)
    {
        if (PS3.getAnalogHat(RightHatX) > 145)
        {
            forward();   // Example: move forward
        }

        if (PS3.getAnalogHat(RightHatX) <= 135)
        {
            stopRobot();
        }
    }
}

// Motion routines for forward
void forward()
{
    servoCenter.write(50);
    delay(250);
    servoRight.write(160);
    servoLeft.write(10);
    delay(250);
    servoCenter.write(120);
    delay(250);
    servoRight.write(10);
    servoLeft.write(160);
    delay(250);
}

void stopRobot()
{
    servoLeft.write(90);
    servoRight.write(90);
    servoCenter.write(90);
}

Also, it can be very confusing when the comments don't describe what's really going on:-

servoLeft.attach(3);  // Set left servo to digital pin 10
    servoRight.attach(5);
    servoCenter.attach(6);  // Set right servo to digital pin 9

Edit: I initially forgot to actually remove the semicolons that I mentioned, but it's fixed now.

3

The problem is/are the semicolons after the conditions of your if-statements.

gyrojeremy:

  if(PS3.getAnalogHat(RightHatX) >145);

{forward();}// Example: move forward

if(PS3.getAnalogHat(RightHatX) <=135);
{stopRobot();}

Right now, it effectively says:

if(PS3.getAnalogHat(RightHatX) >145)
{
  // do nothing
}
{
  forward(); // <-- always do this
}

if(PS3.getAnalogHat(RightHatX) <=135)
{
  // do nothing
}
{
  stopRobot(); // always do this
}

So every loop, it goes through all the movements, regardless of the controls.

4

Cool thanks for that. I cleaned up the code in the OP.
The forward is working on command and the stop is to but I have to hold the stick right on 135 for it to stop. I tried writing it like this
if(PS3.getAnalogHat(RightHatX) <=135, >=115) but then it wont run forward ether.

How do I write it so that when the RightHatX is between 115 and 135 it stops?

5

gyrojeremy:
Cool thanks for that. I cleaned up the code in the OP.
The forward is working on command and the stop is to but I have to hold the stick right on 135 for it to stop. I tried writing it like this

if(PS3.getAnalogHat(RightHatX) <=135, >=115)

but then it wont run forward ether.

How do I write it so that when the RightHatX is between 115 and 135 it stops?

I don't know the return type of getAnalogHat(), but if I assume it's an int:-

int hatX = PS3.getAnalogHat(RightHatX);
if(hatX > 115 && hatX < 135)
{
    stopRobot();
}

Edit: I should add, when only one statement follows the 'if()', you don't actually need brackets, although many people prefer to use them anyway:-

int hatX = PS3.getAnalogHat(RightHatX);
if(hatX > 115 && hatX < 135)
    stopRobot();
6

Thank you Steve/Jobi. That worked. Think I just need to tweak all the numbers now & build it of course.

7

gyrojeremy:
Thank you Steve. That worked. Think I just need to tweak all the numbers now & build it of course.

No worries. Have fun. :slight_smile:

8

Update.
So I found when running the stopRobot(); code it got quite buggy. However , now that I've had a chance to mess around with it it will actually stop by itself after the RightHat returns to the middle.
But it takes about 5-7 seconds to stop after the stick is centered.

Is that because of the ps3bt library or that way my code is set out?

9

gyrojeremy:
Is that because of the ps3bt library or that way my code is set out?

It's because of the way you have delays in your move code. You really need to use some sort of timing variable so you know when it's time to advance the servos.

10

It's because of the way you have delays in your move code.

I was afraid of that. I'll try to rap my head around millis. I guess the Blink without delay sketch would have everything I need. Hopefully it all fits on the uno with the ps3bt library.

11

gyrojeremy:
I was afraid of that. I'll try to rap my head around millis. I guess the Blink without delay sketch would have everything I need. Hopefully it all fits on the uno with the ps3bt library.

"BlinkWithoutDelay" is a good basic introduction to 'millis()'-based timing. Robin2 has also written a good tutorial thread on doing multiple things at once. A bit more complex, but well worth reading and getting your head around the concepts:-
Demonstration code for several things at the same time

12

The joystick servo code you used in your robot arm uses "micros()" to keep track of when to perform various tasks including updating the position of the servos.

I millisecond is kind of long time in the life of a servo refresh cycle. I think keeping track of time in microseconds will provide smoother servo motion than keeping track of time with milliseconds.

13

Ok, this is what I've come up with.

#include <PS3BT.h>                    //Include the necessary libraries. 
#include <Servo.h> 

USB Usb; 
BTD Btd(&Usb); 
PS3BT PS3(&Btd);

const int servoRpin = 3;
const int servoLpin = 5;
const int servoCpin = 6;
const int servoMinDegreesR = 60; // the limits to servo movement
const int servoMaxDegreesR = 130;
const int servoMinDegreesL = 60; // the limits to servo movement
const int servoMaxDegreesL = 130;
const int servoMinDegreesC = 60; // the limits to servo movement
const int servoMaxDegreesC = 105;

Servo servoR;  // create servo object to control a servo
Servo servoL;
Servo servoC;

int servoPosition = 90;     // the current angle of the servo - starting at 90.
int servoFastInterval = 6000;
int servoInterval = servoFastInterval; // initial microsecs between servo moves
int servoDegrees = 1;

unsigned long currentMicros = 0;
unsigned long previousServoMicros = 0;

void setup() {
  
    Serial.begin(115200);                     
  if (Usb.Init() == -1) {                     
    Serial.print(F("\r\nOSC did not start")); 
    while(1); //halt 
  } 
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));

  servoR.writeMicroseconds(servoPosition); // sets the initial position
  servoR.attach(servoRpin);
  servoL.writeMicroseconds(servoPosition); // sets the initial position
  servoL.attach(servoLpin);
  servoC.writeMicroseconds(servoPosition); // sets the initial position
  servoC.attach(servoCpin);
 
}

void loop() {

      // Notice that none of the action happens in loop() apart from reading micros()
      //   it just calls the functions that have the action code

  currentMicros = micros();   // capture the latest value of micros()
                              
 Usb.Task(); 

  if(PS3.PS3Connected || PS3.PS3NavigationConnected) {
    
      
     if(PS3.getAnalogHat(RightHatY) <100)
  forward(); 
  
  
  if(PS3.getButtonClick(PS)) 
      PS3.disconnect();
 
  }
}

void forward() {

      // this is similar to the servo sweep example except that it uses millis() rather than delay()

      // nothing happens unless the interval has expired
      // the value of currentMillis was set in loop()
  
  if (micros() - previousServoMicros >= servoInterval) {
        // its time for another move
    previousServoMicros += servoInterval;
    
    servoPosition = servoPosition + servoDegrees; // servoDegrees might be negative

  if ((servoPosition >= servoMaxDegreesL) || (servoPosition <= servoMinDegreesL))  {
          // if the servo is at either extreme change the sign of the degrees to make it move the other way
      servoDegrees = - servoDegrees; // reverse direction
          // and update the position to ensure it is within range
      servoPosition = servoPosition + servoDegrees; 
    }
    
        // make the servo move to the next position
    servoL.writeMicroseconds(servoPosition);
        // and record the time when the move happened

  }
  
}

Before I change it to Micros, I could get servoL to sweep on command (Using Millis) . Now with Micros in there, as soon as it powers up the servo moves to 180° and stays there with no response to ps3 controller commands. Am I missing something when I change to Micros?

14

I see a couple problems in the code.

First off did you change the value of "servoFastInterval" after switching to micros? 6000us (6ms) is a shorter control cycle than most servos require. I'm not sure how often the Arduino refreshes servos but my guess is it's every 20ms (20,000us). I'd use 20000 as the value for "servoFastInterval" and modify the values of "servoDegrees" to compensate for the longer update period.

Speaking of "servoDegrees", it looks like you mix microseconds and degrees. You use "writeMicroseconds" but it looks like you calculate servo positions based on angles.

If you're using degrees as position values then you need to use "write" to set the servo position.

15

yes I did change to 6000us. It was 6ms to start with. So I changed it all back and only changed these ones. currentMicros = micros(); and int servoFastInterval = 20000; and this one to speed it up int servoDegrees = 4; because 20000us is very slow. I tried 10000us but didn't seen to make any difference.

So it's running ok now but it has a weird jitter when the RightHatY is first applied, like the servo is trying to find 90° first before it starts to sweep from 60° to 130° and back. It was there in millis as well and got worse as more servos were introduced.

Which brings me to my next question. Is the forward loop a good way to write it?
When I added the other 2 servos it started getting pretty long and there was no way to time when the servos should move which made it walk funny?

16

Can someone please explain how this code works (in simple mans terms)?
Am I on the right track to assume that this part tells the servoL to move?

  if (currentMicros - previousServoMicros >= servoInterval) 
  {      previousServoMicros += servoInterval;
         servoPositionL = servoPositionL + servoDegrees;

And this part tells the servoL where to move to?

   if ((servoPositionL >= servoMaxDegreesL) || (servoPositionL <= servoMinDegreesL))  
  {      servoDegrees = - servoDegrees; 
         servoPositionL = servoPositionL + servoDegrees; 
    }
17

gyrojeremy:
Can someone please explain how this code works (in simple mans terms)?
Am I on the right track to assume that this part tells the servoL to move?

  if (currentMicros - previousServoMicros >= servoInterval)

{      previousServoMicros += servoInterval;
        servoPositionL = servoPositionL + servoDegrees;




And this part tells the servoL where to move to?

if ((servoPositionL >= servoMaxDegreesL) || (servoPositionL <= servoMinDegreesL))  
 {      servoDegrees = - servoDegrees;
        servoPositionL = servoPositionL + servoDegrees;
   }

if (currentMicros - previousServoMicros >= servoInterval)

Check to see it the interval of time defined by "servoInterval" has passed. The variables should be unsigned longs in order for the math to work correctly when the micros() macro rolls over.

previousServoMicros += servoInterval;

Add the interval to the "previousServoMicros" so it's ready for the next interval. You could use:

previousServoMicros = micros();

But the above code would not be as accurate as the code you're using. This second option would be a little off each time it was set.

if ((servoPositionL >= servoMaxDegreesL) || (servoPositionL <= servoMinDegreesL))

The "||" indicates "OR". If the position is greater the max limit or less than the min limit, you want the servo to change direction. To change direction you change the sign of the value you've been adding to the servo.

servoDegrees = - servoDegrees;

This could also be written as:

servoDegrees = -1 * servoDegrees;

or

servoDegrees *= -1;

The code:

servoPositionL = servoPositionL + servoDegrees;

increments (or decrements) the servo position by "servoDegrees". The above code could also be written as:

servoPositionL += servoDegrees;

This is a nice non-blocking way to control servos in a linear manner.

I personally would make all these variables into arrays and use equations/algorithms like:

if (currentMicros - previousServoMicros >= servoInterval) 
    {
      previousServoMicros += servoInterval;
      for (int i = 0; i < SERVOS_IN_USE; i++)
      {
        if ((servoPosition[i] >= servoMaxDegrees[i]) || (servoPosition[i] <= servoMinDegrees[i]))  
        {
          servoDegrees[i] = - servoDegrees[i]; 
        }
        servoPosition[i] += servoDegrees[i];
      }
    }

This would allow you to move all the servos at the same time rather than one after the other.

I'd change the name of "servoDegrees[]" to something more meaningful like "positionIncrement[]".

If one servo wasn't supposed to move for some amount of time, you could set "positionIncrement[]" to zero.

Better yet, these increment values could be calculated based on input from the controller. You could have proportional control over the speed of the servos/robot.

I'd also use microseconds for all servo position calculations (unless you need to do trig and then use radians).

You can set the various parameters like "servoMinDegrees[]" as arrays at the top of the program like you did for your robot arm. I'd also change names like "servoMinDegrees[]" to "servoMinPosition[]".

If you need to use degrees, come up with a proper conversion algorithm rather than pretending the units used by "write" are degrees (they VERY rarely are). Degrees also give lousy resolution.

18

Cool thanks Duane. I'll try to model something off the robot arm code and what you've given me here.

19

Ok this is my third attempt at this post because every time I write something I think of something else to try which changes the results. So here is my code so far

#include <Servo.h>
#include <PS3USB.h> 


USB Usb; 
PS3USB PS3(&Usb); 
            
const byte servoInUse = 3;
const byte servoPin[] = {3, 5, 6};

const long minPulse[] = {900, 900, 900};
const long maxPulse[] = {1800, 1800, 1800};

unsigned long servoRefreshHz = 1000;
unsigned long currentMicros = 0;
unsigned long previousServoMicros = 0;

long servoPulseRange[servoInUse]; 
long servoPosition[servoInUse];

int positionIncrement = 2;

Servo myServo[servoInUse];


void setup()
{
    Serial.begin(115200); 
while (!Serial);    
  if (Usb.Init() == -1) 
  {                     
    Serial.print(F("\r\nOSC did not start")); 
    while(1); //halt 
  } 
  Serial.print(F("\r\nPS3 Bluetooth Library Started"));
  
  for (int i = 0; i < servoInUse; i++)
  {
    servoPulseRange[i] = maxPulse[i] - minPulse[i];
    servoPosition[i] = minPulse[i] + (servoPulseRange[i] / 2);
    
    myServo[i].writeMicroseconds(servoPosition[i]);
    myServo[i].attach(servoPin[i]);
 
  }
}

void loop()
{ 
  
   currentMicros = micros();  
   
   Usb.Task();
  if(PS3.PS3Connected || PS3.PS3NavigationConnected) 
  
  if(PS3.getAnalogHat(RightHatY) <117)
   leftServo();
  
 }

void leftServo()
{
   
  for (int i = 0; i < servoInUse; i++)
  {
   if (currentMicros - previousServoMicros >= servoRefreshHz) 
  {      previousServoMicros += servoRefreshHz;
         servoPosition[i] = servoPosition[i] + positionIncrement;
         
   if ((servoPosition[i] >= maxPulse[i]) || (servoPosition[i] <= minPulse[i]))  
  {      positionIncrement = - positionIncrement; 
         servoPosition[i] += positionIncrement;
  } 
    myServo[i].writeMicroseconds(servoPosition[i]);
}}}

It's still a work in progress, albeit slow at my level. I'm starting to understand it but it's still very difficult to write a sketch from scratch. So I resort to piecing together bit of code from multiple sources.
I managed to put the variables into arrays (I think). I can't get the servoRefreshHz or positionIncrement working like they're suppose to. I can have it like in the code above or set to these to get any sort of speed out of it.

unsigned long servoRefreshHz = 20000
int positionIncrement = 50;

I don't think that's right. Shouldn't be more like this?

unsigned long servoRefreshHz = 20000
int positionIncrement = 4;

Ether way it won't fix the elephant in the room. Only 1 servo sweeps out of the 3. They all have a little jitter (which I have a theory about) when the RightHatY is first applied but then only the first servo sweeps?

My theory on the jitter...
Jitter is the wrong name for it, it's more like rapid sweeps. I'm not sure how to word it but I'll do my best. The longer the controller is left idle, the longer the jitter is before it starts sweeping. It's like the micros counter (or the loop) is still counting loops while no joystick inputs are being applied. Then as soon as the RightHatY is applied the servos madly try to catch up their loops to where they last stopped before they can resume the sweep.
So, I think I need a way to reset/restart the counter at the start of void function or write the function in a way that it takes the most resent micros and starts the loop with that. But I thought that's what this was doing?

currentMicros = micros();

Or it needs to stop counting loops once there is no joystick inputs???

Edit: Sorted out the jitter by adding this line after writeMicroseconds

previousServoMicros = currentMicros;
20

I'm don't think you're using the correct terms for your refresh rate.

Here's a couple constants I often use in servo programs.

const unsigned long SERVO_REFRESH_HZ = 50;

const unsigned long SERVO_REFRESH_PERIOD_US = 1000000 / SERVO_REFRESH_HZ;

You want to set your "previousServoMicros" as part of the setup function.

  previousServoMicros = micros();

I think the over all structure of your program needs to be changed.

I think you should check to see if it's time to change servo positions, and only then read input from your controller and then take action based on which phase of the gait is active.

Can you describe the desired gait?

You have three servos right? What do you want these servo to be doing in order to make the robot walk?

How would you like your robot to be positioned when it's first powered on?