Servo object inside custom class not working

Hello

I am making a robotic arm for a school project.

First, I created some simple code to test it, I have 4 POTs controlling each joint and a joystick for the wrist.
This all worked fine, there were no problems. (I guess this is my way of saying the electronics are fine)

#include <Servo.h>

Servo shoulder1_s;
Servo shoulder2_s;
Servo upperArm_s;
Servo elbow_s;
Servo wrist1_s;
Servo wrist2_s;

int pos1, pos2, pos3, pos4, pos5, pos6;



void setup() {

  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(8, OUTPUT);


  shoulder1_s.attach(13, 500, 2500);
  shoulder2_s.attach(12, 500, 2550);
  upperArm_s.attach(11, 550, 2400);
  elbow_s.attach(10, 700, 2300);
  wrist1_s.attach(9, 500, 2500);
  wrist2_s.attach(8, 500, 2500);

}

void loop() {

  pos1 = map(analogRead(A0), 0, 1023, 500, 2500);
  pos2 = map(analogRead(A1), 0, 1023, 500, 2550);
  pos3 = map(analogRead(A2), 0, 1023, 550, 2400);
  pos4 = map(analogRead(A3), 0, 1023, 700, 2300);
  pos5 = map(analogRead(A4), 0, 1023, 500, 2500);
  pos6 = map(analogRead(A5), 0, 1023, 500, 2500);



  shoulder1_s.writeMicroseconds(pos1);

  shoulder2_s.writeMicroseconds(pos2);

  upperArm_s.writeMicroseconds(pos3);

  elbow_s.writeMicroseconds(pos4);

  wrist1_s.writeMicroseconds(pos5);
  
  wrist2_s.writeMicroseconds(pos6);


}

My next step is to do the inverse kinematics with this I wanted to set the servo with an angle, not microseconds but I wanted better resolution then you can do with the servo.write() in the servo library, I wanted to be able to use doubles with two decimals points for my angles. So I wrote a class Joint that had a servo object inside it as well as some functions to convert a double angle into microseconds.
Note that I won't be using POTs in the future to input positions into the writeAngle function that is why there is the rounding off line.

class joint {
private:
	int angleMax; //upper limit of the servo
	int angleMin; // lower limit of the servo
	int secMax;	// milisecond max
	int secMin; // milisecond min
	double angle; // current angle
	double debug;
	double ratio;
	Servo servo; //servo object
public:
	double getAngle() {
		return angle;
	}

	void writeAngle(double pos) {
		angle = floor(pos * 100 + 0.5) / 100; // rounds angle to 2 decimal places
		angle = constrain(angle, angleMin, angleMax); //safty to stop angle being greater then where it  should move
		int value;
		value = round(angle * ratio) + secMin; //maps the angle to miliseconds
		//debug = value;
		servo.writeMicroseconds(value); //writes milis to the servo
	}


	void writeMicroS(int pos) {
		pos = constrain(pos, secMin, secMax); //safty to mkae sure the angle is safe
		angle = floor((pos - secMin) / ratio * 100 + 0.5) / 100; //converts milis to anlge 
		servo.writeMicroseconds(pos);
	}

	double getDebug() {
		return  servo.readMicroseconds();
	}


	joint(byte pin, int min, int max, double startAngle, int pAngleMin = 0, int pAngleMax = 180) {
		//set up for joint
		angleMin = pAngleMin;
		angleMax = pAngleMax;
		secMin = min;
		secMax = max;
		servo.attach(pin, min, max);
		writeAngle(startAngle);
		ratio = double(secMax - secMin) / (angleMax - angleMin);
	}

};

Here is the whole code:

#include "Servo.h"
#include "math.h"


double pos1, pos2, pos3, pos4;


// class joint: has a subclasses servo. tracks position of joint. allows for greater accuracy control
class joint {
private:
	int angleMax; //upper limit of the servo
	int angleMin; // lower limit of the servo
	int secMax;	// milisecond max
	int secMin; // milisecond min
	double angle; // current angle
	double debug;
	double ratio;
	Servo servo; //servo object
public:
	double getAngle() {
		return angle;
	}

	void writeAngle(double pos) {
		angle = floor(pos * 100 + 0.5) / 100; // rounds angle to 2 decimal places
		angle = constrain(angle, angleMin, angleMax); //safty to stop angle being greater then where it  should move
		int value;
		value = round(angle * ratio) + secMin; //maps the angle to miliseconds
		//debug = value;
		servo.writeMicroseconds(value); //writes milis to the servo
	}


	void writeMicroS(int pos) {
		pos = constrain(pos, secMin, secMax); //safty to mkae sure the angle is safe
		angle = floor((pos - secMin) / ratio * 100 + 0.5) / 100; //converts milis to anlge 
		servo.writeMicroseconds(pos);
	}

	double getDebug() {
		return  servo.readMicroseconds();
	}


	joint(byte pin, int min, int max, double startAngle, int pAngleMin = 0, int pAngleMax = 180) {
		//set up for joint
		angleMin = pAngleMin;
		angleMax = pAngleMax;
		secMin = min;
		secMax = max;
		servo.attach(pin, min, max);
		writeAngle(startAngle);
		ratio = double(secMax - secMin) / (angleMax - angleMin);
	}

};


//set up joints

joint shoulderIn(13, 500, 2500, 0);
joint ShoulderOut(12, 500, 2550, 90);
joint UpperArm(11, 550, 2400, 0);
joint Elbow(10, 700, 2300, 0, 0, 145);
joint WristIn(9, 500, 2500, 90);
joint WristOut(8, 500, 2500, 90);


//takes 6 doubles as servo position, wrist deafults to last wrist angle
void SetServos(double pos1, double pos2, double pos3, double pos4, double pos5 = double(WristIn.getAngle()
	), double pos6 = double(WristOut.getAngle())) {
	shoulderIn.writeAngle(pos1);
	ShoulderOut.writeAngle(pos2);
	UpperArm.writeAngle(pos3);
	Elbow.writeAngle(pos4);
	WristIn.writeAngle(pos5);
	WristOut.writeAngle(pos6);
}


void setup() {

	Serial.begin(9600);
}

void loop() {


	// maps POT input to angles
	
	pos1 = map(analogRead(A0), 0, 1023, 0, 180 * 100);
	pos1 = pos1 / 100;
	
	pos2 = map(analogRead(A1), 0, 1023, 0, 180 * 100);
	pos2 = pos2 / 100;

	pos3 = map(analogRead(A2), 0, 1023, 0, 180 * 100);
	pos3 = pos3 / 100;

	pos4 = map(analogRead(A3), 0, 1023, 0, 145 * 100);
	pos4 = pos4 / 100;

	


	SetServos(pos1, pos2, pos3, pos4);
	
	Serial.print(shoulderIn.getDebug());
	Serial.print("	");
	Serial.print(ShoulderOut.getDebug());
	Serial.print("	");
	Serial.print(UpperArm.getDebug());
	Serial.print("	");
	Serial.print(Elbow.getDebug());
	Serial.print("	");
	Serial.print(WristIn.getDebug());
	Serial.print("	");
	Serial.println(WristOut.getDebug());
	

}

When I use this program my arm moves jaggedly to a position and then jitters around there quite harshly. When I adjust my pots it doesn't respond. In my debug the of servo.read the outputs are all as I would expect. eg 2500 when the pot is turned all the way for the first servo.
My guess at the problem is that there is something wrong with instantiating a servo object in my joint class. As the first program without a class worked fine
So I did some experiments, I created only one joint object when I did this nothing happened and the servo wasn't holding a position, I could back drive it.
Then I tried with two joint objects, it did a similar thing as when I had all the objects. however, I was kind of able to position the second servo although it was very delayed.
Looking forward to hearing some suggestions :smiley:
Thanks in advance

well eh, if a servo moves to a position from a pulse between 544us & 2400us then how are you going to increase resolution ? the timers that are used by the servo class are 16bit. I haven't looked at your program in detail, but this is simply because your attempts are futile by definition. If somehow you want the maximum resolution possible, consider the following. The servos i have (MG996r) take a pulse that translates to a position between more or less the length earlier mentioned. (actually a little more range is available) and the servo library pulses at 50hz, every 20ms, but the servos work fine if the pulses come at a longer interval (i tested up to 70ms and found no problem) after a longer wait (say 1000ms) it tends to respond after about 2 or 3 pulses. Anyway it is not to hard to write some code which does not use the library at all and sends the exact pulselength you want.

Deva_Rishi:
well eh, if a servo moves to a position from a pulse between 544us & 2400us then how are you going to increase resolution ?

Lets take the first servo with a pulse range of 500 to 2500
if i were to say servo.write(1) it would send a pulse of 511, ((500 - 2500) / 180 * 1) + 500 . that means that means im only accurate to 11 microseconds. with my class i could say servo.write(0.1) and that would be the same as servo.writeMicroseconds(501) meaning i have a greater resolution?

My logic may be off, this is my first time using servos properly, and my first proper arduino project.I appologise if the language im using is wrong, hopefully you understand what i mean. Im not sure exactly what the lingo is for servo pulse length and stuff

GreenOlive:
Lets take the first servo with a pulse range of 500 to 2500
if i were to say servo.write(1) it would send a pulse of 511, ((500 - 2500) / 180 * 1) + 500 . that means that means im only accurate to 11 microseconds. with my class i could say servo.write(0.1) and that would be the same as servo.writeMicroseconds(501) meaning i have a greater resolution?

If that is what you want to do, either modify the servo library into your own version which contains a function servo.writeMicroseconds() and does what it is supposed to do. Now servo.write() takes an angle between 1 & 180 and calculates the microseconds and you want to start at that, convert back to an angle, and well i think you get my point. Keep in mind that the timers do have a resolution, prescalers etc. and consider if the timer solution outweighs the benefits of using a different solution. (in my case the timers were interrupting/interfering with another process to much)

You seem to be attempting to do I/O during a constructor.

Constructors typically run before the environment is setup, you should only be iniitializing instance variables, not doing anything in a constructor.

Use a begin() method perhaps?

Deva_Rishi:
If that is what you want to do, either modify the servo library into your own version which contains a function servo.writeMicroseconds() and does what it is supposed to do. Now servo.write() takes an angle between 1 & 180 and calculates the microseconds and you want to start at that, convert back to an angle, and well i think you get my point. Keep in mind that the timers do have a resolution, prescalers etc. and consider if the timer solution outweighs the benefits of using a different solution. (in my case the timers were interrupting/interfering with another process to much)

I am not sure i have the coding experience and knowledge to modify the servo library, i have looked at it and its a bit beyond me, that is why i am doing it slightly roundabout root.Also im not so sure i get your point :confused: :sweat_smile:

I think I am fine with the resolution of the 500 - 2500 for now.(Maybe something i can look at in the future)
I just want to have greater accuracy then just integers as a angle input

MarkT:
You seem to be attempting to do I/O during a constructor.

Constructors typically run before the environment is setup, you should only be iniitializing instance variables, not doing anything in a constructor.

Use a begin() method perhaps?

So should i make an attach method, similar to how the servo library works to setup my joint class? Not sure i have understood completely

GreenOlive:
So should i make an attach method, similar to how the servo library works to setup my joint class? Not sure i have understood completely

Yes, you have. These lines:

servo.attach(pin, min, max);
writeAngle(startAngle);

Shouldn't be in the constructor. They need to be in some method you can call from setup.

Here's something that's running you can look at. You're welcome to use anything you find in there.

Delta_G:
Yes, you have. These lines:

servo.attach(pin, min, max);

writeAngle(startAngle);




Shouldn't be in the constructor. They need to be in some method you can call from setup.

hmmm i updated my class to this
``class joint {
private:
int angleMax; //upper limit of the servo
int angleMin; // lower limit of the servo
int secMax; // milisecond max
int secMin; // milisecond min
double angle; // current angle
double debug;
double ratio;
Servo servo; //servo object
public:
double getAngle() {
return angle;
}

void writeAngle(double pos) {
angle = floor(pos * 100 + 0.5) / 100; // rounds angle to 2 decimal places
angle = constrain(angle, angleMin, angleMax); //safty to stop angle being greater then where it should move
int value;
value = round(angle * ratio) + secMin; //maps the angle to miliseconds
//debug = value;
servo.writeMicroseconds(value); //writes milis to the servo
}

void writeMicroS(int pos) {
pos = constrain(pos, secMin, secMax); //safty to mkae sure the angle is safe
angle = floor((pos - secMin) / ratio * 100 + 0.5) / 100; //converts milis to anlge
servo.writeMicroseconds(pos);
}

double getDebug() {
return servo.readMicroseconds();
}

void attach (byte pin, int min, int max, double startAngle, int pAngleMin = 0, int pAngleMax = 180) {
//set up for joint
angleMin = pAngleMin;
angleMax = pAngleMax;
secMin = min;
secMax = max;
servo.attach(pin, min, max);
writeAngle(startAngle);
ratio = double(secMax - secMin) / (angleMax - angleMin);
}
};

and called that method in the setup

joint shoulderIn;
joint shoulderOut;
joint upperArm;
joint elbow;
joint wristIn;
joint wristOut;

void setup() {

	shoulderIn.attach(13, 500, 2500, 0);
	shoulderOut.attach(12, 500, 2550, 90);
	upperArm.attach(11, 550, 2400, 0);
	elbow.attach(10, 700, 2300, 0, 0, 145);
	wristIn.attach(9, 500, 2500, 90);
	wristOut.attach(8, 500, 2500, 90);

	Serial.begin(9600);
}

but it hasnt seemed to make any difference

GreenOlive:
I am not sure i have the coding experience and knowledge to modify the servo library, i have looked at it and its a bit beyond me, that is why i am doing it slightly roundabout root.Also im not so sure i get your point :confused: :sweat_smile:

Seeing what you did do i disagree with you statement. Firstly look at this comment from servo.cpp

   this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
    this->max  = (MAX_PULSE_WIDTH - max)/4;

secondly have a look at the write() function

void Servo::write(int value)
{  
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0) value = 0;
    if(value > 180) value = 180;
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
  }
  this->writeMicroseconds(value);
}

and from servo.h

 void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
  void writeMicroseconds(int value); // Write pulse width in microseconds

so all in all, the library promises an accuracy of 4us.
if you want accuracy just writeMicroseconds() and actually write() does it already if you provide a value higher than the minimum pulselength. If you still want to have the angle as a float converted to microseconds a simple math function will do (minimumPL+ (angle.0*(maximumPL-minimumPL))/180) and there is no need for a whole class, i mean really, just for that ? if you want you can include it in the servo class.
And the highest accuracy can be acheived with something as simple as this

digitalWrite(pin,HIGH);delayMicroseconds(500+pulselength);
digitalWrite(pin,LOW);
delay(20);

Of course this would be blocking in nature and only 1 pin, but there are ways to do this sort of thing that may block but only for the maximum PL and fire all pins and don't have anything running in the background for all the rest of the time (or maybe 1 timer every 20 ms or a bit less often)

One more thing that came to mind. How much play is there on the gears of the servo ? Let me explain. My Father teaches Physics, my mother teaches Mathematics. The main difference is, do not write accuracy that isn't there.

Deva_Rishi:
If you still want to have the angle as a float converted to microseconds a simple math function will do (minimumPL+ (angle.0*(maximumPL-minimumPL))/180) and there is no need for a whole class, i mean really, just for that ? if you want you can include it in the servo class.

The problem with using a function is you have to have your min and max pulse length as parameters which is just tedious, this is why i wanted to make a class. I will try editing the servo class, however not sure which one to edit, there seem to be different .cpp files for different timers or processors? im not sure what my arduino has, i have the uno smd edition.

Deva_Rishi:
One more thing that came to mind. How much play is there on the gears of the servo ? Let me explain. My Father teaches Physics, my mother teaches Mathematics. The main difference is, do not write accuracy that isn't there.

good point there doesnt seem to be to much play in the gears, if its not too hard i may aswell right the accuracy and the hardware will do what it can do

I will try editing the servo class, however not sure which one to edit, there seem to be different .cpp files for different timers or processors? im not sure what my arduino has, i have the uno smd edition.

Ah yeah sorry i hadn't seen that i was looking through the library folder of the IDE 0023, but you should take the AVR though the write() function is exactly the same in all versions, easiest should be to just add a version taking a float as an argument, i might not even bother about renaming the library for now (though be careful when updating the IDE) something like this would do

void Servo::writeFloat(float value)
{
  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
  if (value < MIN_PULSE_WIDTH)
  {
    if (value < 0)
      value = 0;
    else if (value > 180)
      value = 180;
    value = (SERVO_MIN()+ (value * (SERVO_MAX() - SERVO_MIN()) ) / 180) ;
  }
  writeMicroseconds((int) value);
}

and add the prototype to servo.h hmm strange how they have identical functions in different files.
Or does the map() function takes floats as arguments (i rarely use it)

GreenOlive:
good point there doesnt seem to be to much play in the gears.

well 1 degree is fairly accurate and usually already beyond what most people can manufacture for things that can be controlled. Anyway writing the pulse directly without the use of timers is still the most accurate. (and gives you the processors undivided attention for the rest of the time.)

Deva_Rishi:
Ah yeah sorry i hadn't seen that i was looking through the library folder of the IDE 0023, but you should take the AVR though the write() function is exactly the same in all versions, easiest should be to just add a version taking a float as an argument, i might not even bother about renaming the library for now (though be careful when updating the IDE) something like this would do

void Servo::writeFloat(float value)

{
  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
  if (value < MIN_PULSE_WIDTH)
  {
    if (value < 0)
      value = 0;
    else if (value > 180)
      value = 180;
    value = (SERVO_MIN()+ (value * (SERVO_MAX() - SERVO_MIN()) ) / 180) ;
  }
  writeMicroseconds((int) value);
}



and add the prototype to servo.h hmm strange how they have identical functions in different files.
Or does the map() function takes floats as arguments (i rarely use it)

ok thanks ill try this. The map function takes a long as an argument, unless there is an overlaoded version they dont mention in the refrence.

Deva_Rishi:
well 1 degree is fairly accurate and usually already beyond what most people can manufacture for things that can be controlled. Anyway writing the pulse directly without the use of timers is still the most accurate. (and gives you the processors undivided attention for the rest of the time.)

by directly writing the pulse width do you mean this that you posted earlier?

digitalWrite(pin,HIGH);delayMicroseconds(500+pulselength);
digitalWrite(pin,LOW);
delay(20);

If you have any ideas why my class didnt work as id still like to know why in case i ever run into a similar problem.

Thanks for your help

by directly writing the pulse width do you mean this that you posted earlier?

Yes i do, though this was a rather crude version of it. Normally you'd just check to see if 20ms have elapsed at least (rather than the delay(20) ) and you should send a pulse out through all your pins within the same section of code as to not hold up your program any longer than necessary (though doing it 1 at a time is fine as well and you can use that for testing)) and you should probably disable interrupts while you are measuring the pulselength .

If you have any ideas why my class didnt work as id still like to know why in case i ever run into a similar problem.

nope sorry, it'ss not my specialty, but initiating an object that uses many other resources and doing that from within another class, it should be possible, but...

I have tried to edit the servo class but I am getting compile errors.

I added this to servo.cpp in the avr file:

void Servo::writeFloat(float value) {
	if (value < MIN_PULSE_WIDTH)
	{  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
		if (value < 0) value = 0;
		if (value > 180) value = 180;
		value = (SERVO_MIN() + (value * (SERVO_MAX() - SERVO_MIN())) / 180);
	}
	this->writeMicroseconds((int)value);
}

I defined the function in the servo.h file:

class Servo
{
public:
  Servo();
  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
  void detach();
  void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
  void writeFloat(float value); //excepts a float value instead of integer
  void writeMicroseconds(int value); // Write pulse width in microseconds 
  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
  int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
  bool attached();                   // return true if this servo is attached, otherwise false 
private:
   uint8_t servoIndex;               // index into the channel data for this servo
   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
};

And i tried to use it in my main program:

void setServos(float pos1, float pos2, float pos3, float pos4, float pos5 = 90, float pos6 = 90) {
	shoulderIn.writeFloat(pos1);
	shoulderOut.writeFloat(pos2);
	upperArm.writeFloat(pos3);
	elbow.writeFloat(pos4);
	wristIn.writeFloat(pos5);
	wristOut.writeFloat(pos6);

}

these were the errors i got

Compiling 'RobotArmControlV0.2' for 'Arduino/Genuino Uno'

cchP9XR8.ltrans0.ltrans.o*: In function setServos
RobotArmControlV0.2.ino:13: undefined reference to Servo writeFloat(float)

Error linking for board Arduino/Genuino Uno
RobotArmControlV0.2.ino:14: undefined reference to Servo writeFloat(float)
Build failed for project 'RobotArmControlV0.2'
RobotArmControlV0.2.ino:15: undefined reference to Servo writeFloat(float)
RobotArmControlV0.2.ino:16: undefined reference to Servo writeFloat(float)
RobotArmControlV0.2.ino:17: undefined reference to Servo writeFloat(float)
cchP9XR8.ltrans0.ltrans.o*: C:\Users\olive\Documents\Arduino\RobotArmControlV0.1 - Copy\RobotArmControlV0.2\RobotArmControlV0.2.ino:18: more undefined references to Servo::writeFloat(float) follow

collect2.exe*: error: ld returned 1 exit status

I get the feeling i didnt define it properly but im not sure how.
thanks

hmm maybe it's not the AVR file, i added to all servo.cpp 's and then this

#include <Servo.h>

Servo servo;

float var;

void setup() {

}

void loop() {
  servo.writeFloat(var);

}

compiles

aha! I found my problem, for some reason the Arduino IDE and visual studio cant access the default libraries so a while back I copied over the servo library to the 'client?' libraries. I was changing it the library that the compiler wasn't using. so I just updated the library in the right place. by then I had already put it in all the .cpp files so it might have been the avr one, but I don't feel like removing it from the others to find out.

Again thank you for your help, I really appreciate it.