Go Down

Topic: Servo object inside custom class not working (Read 184 times) previous topic - next topic

GreenOlive

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)
Code: [Select]
#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.

Code: [Select]
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:

Code: [Select]
#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 :D
Thanks in advance

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 ? 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.
To 'Correct' you have to be Correct. (and not be condescending..)

GreenOlive

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

Deva_Rishi

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)
To 'Correct' you have to be Correct. (and not be condescending..)

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?
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

GreenOlive

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  :smiley-confuse:  :smiley-sweat:

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

GreenOlive

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

Delta_G

#7
Sep 13, 2019, 10:31 pm Last Edit: Sep 13, 2019, 10:33 pm by Delta_G
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:

Code: [Select]
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. 


|| | ||| | || | ||  ~Woodstock

Please do not PM with technical questions or comments.  Keep Arduino stuff out on the boards where it belongs.

GreenOlive

Yes, you have.  These lines:

Code: [Select]
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
Code: [Select]
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

Code: [Select]
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

Deva_Rishi

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  :smiley-confuse:  :smiley-sweat:
Seeing what you did do i disagree with you statement. Firstly look at this comment from servo.cpp
Code: [Select]
   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
Code: [Select]
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
Code: [Select]
 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
Code: [Select]
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)
To 'Correct' you have to be Correct. (and not be condescending..)

Deva_Rishi

#10
Sep 13, 2019, 10:55 pm Last Edit: Sep 13, 2019, 10:58 pm by 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.
To 'Correct' you have to be Correct. (and not be condescending..)

GreenOlive

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.

GreenOlive

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

Deva_Rishi

#13
Sep 14, 2019, 10:02 am Last Edit: Sep 14, 2019, 10:04 am by Deva_Rishi
Quote
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
Code: [Select]
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)
To 'Correct' you have to be Correct. (and not be condescending..)

Deva_Rishi

#14
Sep 14, 2019, 10:07 am Last Edit: Sep 14, 2019, 10:10 am by Deva_Rishi
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.)
To 'Correct' you have to be Correct. (and not be condescending..)

Go Up