Bi Ped robot and panning servo question (blocking function?)

Projects Programming
1

If you've seen any of my previous posts, I have 2 robots that have the same objectives. One is on tracks and the other is a Bi Ped walking robot. I've had them both for about 3 years and continuously make them both more accurate, advanced and efficient. They typically leapfrog past each other in efficiency and tasks untill I get the other to catch up.

The Bi Ped robot in question has 6 servos for movement and one servo for panning. The pan servo has a camera to look for certain colors and an IR distance sensor to avoid objects. The robot is very accurate and does what it is supposed to. But I'm looking to make it better. Currently the panning servo only pans in between steps, not while the robot is walking. I'm trying to make it so the pan servo will pan continuously while the robot is walking and scanning for objects. I'm afraid that the Walk() is a blocking function that is not allowing the pan servo to pan while the robot is walking. Below is my current code for the appropriate loops.

int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;

void ScanForBlocks()
{
  if (millis() - lastMove > 8)
  {
    lastMove = millis();
    panLoop.m_pos += scanIncrement;
    if ((panLoop.m_pos >= RCS_MAX_POS) || (panLoop.m_pos <= RCS_MIN_POS))
    {
      tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
      scanIncrement = -scanIncrement;

      if (scanIncrement != 0)
      {
        Walk(15);
        Walk(15);
      }
    }
    pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
  }
}

Above you can see that once the servo pans to either side the robot takes 2 steps then will scan again.

Here is the loop. All of these functions work as expected.

void loop()
{
  read_Ir();
  ScanForBlocks();
  camera();

  if ((ir > Middle) && (millis() - lastBlockTime < 500))
  {
    Walk(5);
    r2D2();
    {
      while ((ir > Middle) && (millis() - lastBlockTime < 600000))
      {
        Rest();
        read_Ir();
      }
    }
  }
  else if ((panLoop.m_pos < RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Right(200);
  }
  else if ((panLoop.m_pos > RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Left(200);
  }
}

Next I'll post what I've tried and the results.

2

It seems that the problem is in the code you didn't post (the Walk() function.)

It also seems that it's a poor design that the ScanForBlocks() function is also capable of walking. All the places that Walk() is called should be within the same module or function.

3

MorganS:
It seems that the problem is in the code you didn't post (the Walk() function.)

It also seems that it's a poor design that the ScanForBlocks() function is also capable of walking. All the places that Walk() is called should be within the same module or function.

The Walk and all the group moves are in a .h folder. Here is Walk()

void Walk(byte angle)
{
  angle = angle*Range;
  if(stepN)
  {
    angle = 90 + angle;
    GroupMove(last_angle, last_angle, 75, last_angle, last_angle, 55, Speed);
    GroupMove(angle, angle, 75, angle, angle, 55, Speed);
    GroupMove(angle, angle, 90, angle, angle, 90, Speed);
    stepN = !stepN;
  }
  else
  {
    angle = 90 - angle;
    GroupMove(last_angle, last_angle, 120, last_angle, last_angle, 105, Speed);
    GroupMove(angle, angle, 125, angle, angle, 105, Speed);
    GroupMove(angle, angle, 90, angle, angle, 90, Speed);
    stepN = !stepN;
  }
  last_angle = angle;
}

Here is how I moved Walk() out of ScanForBlocks and put it in the loop. In this code the robot walks continuously, but the pan servo moves so very slightly in-between steps that it's useless. The pan servo does not move at all during the Walk() group move.

void loop()
{
  read_Ir();
  ScanForBlocks();
  camera();

  if (ir < Far)
  {
    Walk(15);
  }
  else if ((ir > Middle) && (millis() - lastBlockTime < 500))
  {
    Walk(5);
    r2D2();
    {
      while ((ir > Middle) && (millis() - lastBlockTime < 600000))
      {
        Rest();
        read_Ir();
      }
    }
  }
  else if ((panLoop.m_pos < RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Right(200);
  }
  else if ((panLoop.m_pos > RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Left(200);
  }
}
4

I got a chance to work on it a little. I moved the Walk() out of the .h folder and into the sketch. I also tried adding ScanForBlocks() into Walk() as an experiment. All results are the same, I can't seem to get the robot to walk and move the panning servo at the same time.

6

Delta_G:
The smarter thing would be to make all those functions non-blocking. That would not only make solving this issue easier, but it will probably help with future improvements.

Exactly! That's what I'm trying to do. How do I make them non blocking? What is making them block other functions from running?

7

Have you have posted the GroupMove() function ? If so, then I have missed it.

9

UKHeliBob:
Have you have posted the GroupMove() function ? If so, then I have missed it.

Sorry, here is the beginning of the group move header file.

#include <Arduino.h>

#define period 10 //time between each servo adjustment in Microseconds

extern Servo ServoTable[6];
extern int Offsets[6];

float RHP = 90;
float RKP = 90;
float RAP = 90;
float LHP = 90;
float LKP = 90;
float LAP = 90;

float Range = 1;
boolean stepN;
int last_angle = 90;
int Speed = 290;

void GroupMove(byte p1, byte p2, byte p3, byte p4, byte p5, byte p6, int Speed)
{
  float ticks = Speed / period;
  float RHS = (p1 - RHP) / ticks;
  float RKS = (p2 - RKP) / ticks;
  float RAS = (p3 - RAP) / ticks;
  float LHS = (p4 - LHP) / ticks;
  float LKS = (p5 - LKP) / ticks;
  float LAS = (p6 - LAP) / ticks;
  for(int x = 0; x < ticks; x++)
  {
    RHP = RHP + RHS;
    RKP = RKP + RKS;
    RAP = RAP + RAS;
    LHP = LHP + LHS;
    LKP = LKP + LKS;
    LAP = LAP + LAS;
    ServoTable[0].write(RHP + Offsets[0]);
    ServoTable[1].write(RKP + Offsets[1]);
    ServoTable[2].write(RAP + Offsets[2]);
    ServoTable[3].write(LHP + Offsets[3]);
    ServoTable[4].write(LKP + Offsets[4]);
    ServoTable[5].write(LAP + Offsets[5]);
    delay(period);
  }
}
10 
#include <EEPROM.h>
#include "WriteReadAnything.h"
#include <Servo.h>
#include "Move_Data.h"
#include <SPI.h>
#include <Pixy.h>
#include "musical_notes.h"

#define RightHipPin 9
#define RightKneePin 8
#define RightAnklePin 7
#define LeftHipPin 4
#define LeftKneePin 3
#define LeftAnklePin 2

#define X_CENTER    160L
#define Y_CENTER    100L
#define RCS_MIN_POS     0L
#define RCS_MAX_POS     1000L
#define RCS_CENTER_POS  ((RCS_MAX_POS-RCS_MIN_POS)/2)

#define Far 150
#define Middle 350
#define Close 500

int irtemp[10];
byte detpos;
int ir;
boolean dirflag = false;
byte pos = 90;
byte followError;
int speakerPin = 5;

//-----------------------------------------------------------------------------
// Servo Loop Class
// A Proportional/Derivative feedback
// loop for pan/tilt servo tracking of blocks.
// (Based on Pixy CMUcam5 example code)
//-----------------------------------------------------------------------------

class ServoLoop
{
  public:
    ServoLoop(int32_t proportionalGain, int32_t derivativeGain);

    void update(int32_t error);

    int32_t m_pos;
    int32_t m_prevError;
    int32_t m_proportionalGain;
    int32_t m_derivativeGain;
};

//-----------------------------------------------------------------------------
// ServoLoop Constructor
//-----------------------------------------------------------------------------

ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)
{
  m_pos = RCS_CENTER_POS;
  m_proportionalGain = proportionalGain;
  m_derivativeGain = derivativeGain;
  m_prevError = 0x80000000L;
}

// ServoLoop Update
// Calculates new output based on the measured
// error and the current state.

void ServoLoop::update(int32_t error)
{
  long int velocity;
  char buf[32];
  if (m_prevError != 0x80000000)
  {
    velocity = (error * m_proportionalGain + (error - m_prevError) * m_derivativeGain) >> 10;

    m_pos -= velocity;
    if (m_pos > RCS_MAX_POS)
    {
      m_pos = RCS_MAX_POS;
    }
    else if (m_pos < RCS_MIN_POS)
    {
      m_pos = RCS_MIN_POS;
    }
  }
  m_prevError = error;
}

//-----------------------------------------------------------------------------
// End Servo Loop Class
//-----------------------------------------------------------------------------

Pixy pixy;  // Declare the camera object

ServoLoop panLoop(550, 400);  // Servo loop for pan
ServoLoop tiltLoop(150, 200); // Servo loop for tilt

//-----------------------------------------------------------------------------
// Setup - runs once at startup
//-----------------------------------------------------------------------------

int Offsets[6] = {0, 0, 0, 0, 0, 0};

Servo ServoTable[6];

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
  pixy.init();
  ServoTable[0].attach(RightHipPin);
  ServoTable[1].attach(RightKneePin);
  ServoTable[2].attach(RightAnklePin);
  ServoTable[3].attach(LeftHipPin);
  ServoTable[4].attach(LeftKneePin);
  ServoTable[5].attach(LeftAnklePin);
  pinMode(7, INPUT);  //Activate the onboard buttons for our use in the program
  pinMode(8, INPUT);  //
  pinMode(9, INPUT);  //
  pinMode(speakerPin, OUTPUT);
  if (digitalRead(7) == LOW)  //if we hold down the A button enter config mode
  {
    Load_Offsets();
    Config_Offsets();
  }
  else                       //Otherwise Load the current offsets
    Load_Offsets();
}

void Load_Offsets()
{
  EEPROM_readAnything(0, Offsets);
}

void Config_Offsets()
{
  tone(5, 1000, 1000);
  delay(2000);

  for (int x = 0; x < 6; x++)
  {
    while (digitalRead(7))
    {
      if (digitalRead(8) == LOW)
      {
        Offsets[x] = Offsets[x] - 1;
        delay(100);
      }
      else if (digitalRead(9) == LOW)
      {
        Offsets[x] = Offsets[x] + 1;
        delay(100);
      }
      ServoTable[x].write(90 + Offsets[x]);
    }
    delay(500);

  }
  EEPROM_writeAnything(0, Offsets);
  for (int x = 0; x < 5; x++)
  {
    tone(5, 100 * x + 500, 100);
    delay(100);
  }
}

uint32_t lastBlockTime = 0;

//-----------------------------------------------------------------------------
// IR sensor constructor
//-----------------------------------------------------------------------------

int read_Ir()
{
  int A = analogRead(3);
  delay(1);
  int B = analogRead(3);
  int number = 0;
  for (int x = 0; x < 10; x++)
  {
    A = analogRead(3);
    B = analogRead(3);
    while (abs(A - B) > 5)
    {
      A = B;
      B = analogRead(3);
    }
    irtemp[x] = (A + B) / 2;
  }
  ir = 0;
  for (int x = 0; x < 9; x++)
    ir = ir + irtemp[x];
  ir = ir / 10;

  if (ir > Far)
    detpos = pos;
}

//-----------------------------------------------------------------------------
// Camera constructor
//-----------------------------------------------------------------------------

void camera()
{
  uint16_t blocks;
  blocks = pixy.getBlocks();

  // If we have blocks in sight, track and follow them
  if (blocks)
  {
    int trackedBlock = TrackBlock(blocks);
    FollowBlock(trackedBlock);
    lastBlockTime = millis();
  }
  else if (millis() - lastBlockTime > 100)
  {
    ScanForBlocks();
  }
}

int oldX, oldY, oldSignature;

//-----------------------------------------------------------------------------
// Track blocks via the Pixy pan/tilt mech
// (based in part on Pixy CMUcam5 pantilt example and Pixy Pet)
//-----------------------------------------------------------------------------

int TrackBlock(int blockCount)
{
  int trackedBlock = 0;
  long maxSize = 0;

  Serial.print("blocks =");
  Serial.println(blockCount);

  for (int i = 0; i < blockCount; i++)
  {
    if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature))
    {
      long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
      if (newSize > maxSize)
      {
        trackedBlock = i;
        maxSize = newSize;
      }
    }
  }

  int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
  int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;

  panLoop.update(panError);
  tiltLoop.update(tiltError);

  pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);

  oldX = pixy.blocks[trackedBlock].x;
  oldY = pixy.blocks[trackedBlock].y;
  oldSignature = pixy.blocks[trackedBlock].signature;
  return trackedBlock;
}

//-----------------------------------------------------------------------------
// Follow and walk towards blocks
// This code makes the robot walk and turn
// to follow the tilt tracking of the head.
// (based in part on Pixy Pet)
//-----------------------------------------------------------------------------

int32_t size = 400;
void FollowBlock(int trackedBlock)
{
  int32_t followError = RCS_CENTER_POS - panLoop.m_pos;  // How far off-center are we looking now?
  // Size is the area of the object.
  // We keep a running average of the last 8.
  size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
  size -= size >> 3;

  if (followError > 285)
  {
    Turn_Left(200);
  }
  else if (followError < -285)
  {
    Turn_Right(200);
  }
  else if (followError >= -284 && followError <= 284)
  {
    Walk(15);
  }
  pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
}

//-----------------------------------------------------------------------------
// Random search for blocks
// (based in part on Pixy Pet)
//-----------------------------------------------------------------------------

int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;

void ScanForBlocks()
{
  if (millis() - lastMove > 8)
  {
    lastMove = millis();
    panLoop.m_pos += scanIncrement;
    if ((panLoop.m_pos >= RCS_MAX_POS) || (panLoop.m_pos <= RCS_MIN_POS))
    {
      tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
      scanIncrement = -scanIncrement;
    }
    pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
  }
}

//-----------------------------------------------------------------------------
// Wonder around looking for more red objects
// Avoid other objects
//-----------------------------------------------------------------------------

void loop()
{
  read_Ir();
  ScanForBlocks();
  camera();

  if (ir < Far)
  {
    Walk(15);
  }
  else if ((ir > Middle) && (millis() - lastBlockTime < 500))
  {
    Walk(5);
    r2D2();
    {
      while ((ir > Middle) && (millis() - lastBlockTime < 600000))
      {
        Rest();
        read_Ir();
      }
    }
  }
  else if ((panLoop.m_pos < RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Right(200);
  }
  else if ((panLoop.m_pos > RCS_CENTER_POS) && ir > Far && (millis() - lastBlockTime > 500))
  {
    laugh();
    Turn_Left(200);
  }
}
11

Delta_G:
How the hell are we supposed to know if you're only going to show us little bits and pieces of the code? You're the only one that can see it. You tell me why it blocks.

The post above is all of the sketch that has to do with movement. I had to cut out the beeps the robot makes to fit in the 9000 character limit of posts.