I’m totally flummoxed.
I am working on a basic servo extension to coordinate multiple servos; right now they are just one LEFT and one RIGHT.
I have a bug somewhere but I cannot find it. I’m at that 2nd set of eyes point, so I appreciate any help.
Basically, I am trying to get multiple servos to respond to simple commands in unison. Right-handed servos will sweep opposite to Left-handed in an “syncronized” manor. I hope you get it. My problem, and if you take the time to flash the code, you can see from the (I’m using Photon here, 0.6.1) Serial output that I cannot get just the left servo to move. Both move… Right moves… Left won’t move. I know it is something grossly simple that I have stared at for just too damned long.
I’m hoping that fresh eyes prevail!
example.ino
#include "Brazos.h"
Brazos right(RIGHT_HAND);
Brazos left(LEFT_HAND);
uint32_t rightPosition = 0, leftPosition = 0;
void setup(void)
{
Serial.begin(9600);
Particle.variable("rightPos", rightPosition);
Particle.variable("leftPos", leftPosition);
Particle.function("MoveHands", moveHands);
right.begin(D2, A2);
left.begin(D3, A3);
right.write(0);
left.write(180);
delay(3000);
Brazos::moveAll(HAND_UP);
Serial.printf("There %s %d %s attached\n", Brazos::getInstanceCount() == 1? "is" : "are", Brazos::getInstanceCount(), Brazos::getInstanceCount() == 1? "servo" : "servos");
}
void loop(void)
{
Brazos::update(millis());
rightPosition = right.read();
leftPosition = left.read();
}
int moveHands(const String command)
{
char myCommand[256] = "";
command.toCharArray(myCommand, sizeof(myCommand));
Brazos* selectedHand;
if(strstr(myCommand, "right"))
{
selectedHand = &right;
Serial.println("selected Right");
}
else if(strstr(myCommand, "left"))
{
selectedHand = &left;
Serial.println("selected Left");
}
else
{
selectedHand = NULL;
Serial.println("selected Both");
}
if(strstr(myCommand, "up"))
{
if(selectedHand)
selectedHand->move(HAND_UP);
else
selectedHand->moveAll(HAND_UP);
}
else if(strstr(myCommand, "down"))
{
if(selectedHand)
selectedHand->move(HAND_DOWN);
else
selectedHand->moveAll(HAND_DOWN);
}
else if(strstr(myCommand, "point"))
{
if(selectedHand)
selectedHand->move(HAND_POINT);
else
selectedHand->moveAll(HAND_POINT);
}
return 1;
}
Brazos.h (“arm”)
#ifndef BRAZOS_H
#define BRAZOS_H
#include <vector>
#include "Particle.h"
#define MOTION_SPEED_INTERVAL 5
enum Mano{
RIGHT_HAND,
LEFT_HAND
};
enum HandCommand{
HAND_UP,
HAND_DOWN,
HAND_POINT,
HAND_MASHED_POTATO,
};
class Brazos : public Servo {
public:
enum MotionState{
ARM_AT_REST,
ARM_MOVING
};
Brazos(Mano mano);
~Brazos();
void begin(int servo, int input);
void move(HandCommand command);
static void moveAll(HandCommand command);
static void update(uint32_t now);
static int getInstanceCount();
private:
static std::vector< Brazos* > instanceAddress;
Mano _mano;
int servoPin;
int inputPin;
MotionState motionState = ARM_MOVING;
uint32_t lastMotionMillis = 0;
int currentPositon;
int targetPosition;
static void setTargetPosition(Brazos* instance, int position);
};
#endif
Brazos.cpp
#include "Brazos.h"
#include <vector>
#include "Particle.h"
std::vector< Brazos* > Brazos::instanceAddress;
Brazos::Brazos(Mano mano)
{
instanceAddress.push_back(this);
_mano = mano;
}
Brazos::~Brazos(void)
{
// TO DO Erase vector element instanceAddress <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
}
int Brazos::getInstanceCount()
{
return instanceAddress.size();
}
void Brazos::begin(int servo, int input)
{
servoPin = servo;
pinMode(servoPin, OUTPUT);
attach(servoPin);
inputPin = input;
}
void Brazos::move(HandCommand command)
{
int newPosition = 0;
switch(command)
{
case HAND_UP:
newPosition = 180;
break;
case HAND_DOWN:
newPosition = 0;
break;
case HAND_POINT:
newPosition = 90;
break;
default:
//
break;
}
Serial.println("setting new servo target position");
setTargetPosition(this, newPosition);
}
void Brazos::setTargetPosition(Brazos* instance, int position)
{
instance->targetPosition = (instance->_mano == RIGHT_HAND)? position : 180 - position;
Serial.println("Just set instance servo target position");
}
void Brazos::moveAll(HandCommand command)
{
for(int i = 0; i < instanceAddress.size(); i++)
{
int newPosition = 0;
switch(command)
{
case HAND_UP:
newPosition = 180;
break;
case HAND_DOWN:
newPosition = 0;
break;
case HAND_POINT:
newPosition = 90;
break;
default:
//
break;
}
instanceAddress[i]->setTargetPosition(instanceAddress[i], newPosition);
}
}
void Brazos::update(uint32_t now)
{
for(int i = 0; i < instanceAddress.size(); i++)
{
if(now - instanceAddress[i]->lastMotionMillis > MOTION_SPEED_INTERVAL)
{
if(instanceAddress[i]->targetPosition == instanceAddress[i]->currentPositon)
{
if(instanceAddress[i]->attached())
{
instanceAddress[i]->detach();
Serial.printf("Servo:%d Detached\n", i);
}
return;
}
if(!instanceAddress[i]->attached())
{
instanceAddress[i]->attach(instanceAddress[i]->servoPin);
Serial.printf("Servo:%d Attached\n", i);
}
Serial.printf("Servo:%02d\tCurrent Position:%03d\tTarget Position:%03d\n", i, instanceAddress[i]->currentPositon, instanceAddress[i]->targetPosition);
if(instanceAddress[i]->targetPosition > instanceAddress[i]->currentPositon)
{
instanceAddress[i]->currentPositon++;
instanceAddress[i]->write(instanceAddress[i]->currentPositon);
}
else
{
instanceAddress[i]->currentPositon--;
instanceAddress[i]->write(instanceAddress[i]->currentPositon);
}
instanceAddress[i]->lastMotionMillis = now;
}
}
}
It seems to me that I am simply not traversing the instanceAddress vector in my update() function.
Thanks for reading this!
