Author Topic: SoftwareServo.h and 4 continuous rotation servos are hell-bent on destroying me! (Read 16184 times)

blackheart · « **on:** April 21, 2009, 05:18:31 PM »

Hey! Working on my arduino rover bot, rv-1, I realised that my 4 HSR-1422CR servos and the SoftwareServo.h library are playing some kind of psychological warfare on me. It goes forward and it also goes backwards! It goes right but no matter what I do, I can't get it to go left using all 4 servos.

Here is the code.

Code: [Select]

#include <SoftwareServo.h>

SoftwareServo s1, s2, s3, s4, sa, sb;
int pin_s1, pin_s2, pin_s3, pin_s4, pin_sa, pin_sb;
int pin_ir1, pin_ir2, pin_ir3;
float read_cm(byte pin) {
 int tmp;

 tmp = analogRead(pin);
 if (tmp < 3)
  return -1; // invalid value

 return (6787.0 /((float)tmp - 3.0)) - 4.0;
}

void drive_forward(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(180);
  s2.write(180);
  s3.write(0);
  s4.write(0);
  SoftwareServo::refresh();
  }
}

void drive_backwards(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(0);
  s2.write(0);
  s3.write(180);
  s4.write(180);
  SoftwareServo::refresh(); 
  }
}

void drive_left(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(180);
  s2.write(180);
  s3.write(180);
  s4.write(180);
  SoftwareServo::refresh();
  }
  
}

void drive_right(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(0);
  s2.write(0);
  s3.write(0);
  s4.write(0);
  SoftwareServo::refresh();
  }
}

void setup() {
  pin_s1 = 2;
  pin_s2 = 4;
  pin_s3 = 6;
  pin_s4 = 8;
  pin_sa = 10;
  pin_sb = 12;
  pin_ir1 = 0;
  pin_ir2 = 2;
  pin_ir3 = 4;
  s1.attach(pin_s1);
  s2.attach(pin_s2);
  s3.attach(pin_s3);
  s4.attach(pin_s4);
  sa.attach(pin_sa);
  sb.attach(pin_sb);
  digitalWrite(13, HIGH);
}

void loop() {
  drive_forward();
  drive_backwards();
  drive_left();
  drive_right();
}

When I turn on the bot, it goes forward, then it goes backwards, then it goes right. No matter what I do, I can't get it to go left. All 4 servos are working. Nothing's wrong with them. It bugs me because it only has a problem with the left side. if I call drive_left() by itself, it still goes right.

Please help.

Thank You!

Dscrimager · « **Reply #1 on:** April 21, 2009, 09:08:05 PM »

If I remember correctly that means that the centering is not correct on one or more servo? I.e. the pot needs to be centered.
Do they all stop when you send a centering pulse? I had something like this once,..

Doug

SmAsH · « **Reply #2 on:** April 21, 2009, 09:47:39 PM »

for most servos (unmodified), if you send a 1.5ms pulse it will center 1.0ms pulse it will turn left and 2.0ms pulse will turn right.
@blackheart, are you sure you modified them properly? and have you tried switching the servos around. ie, taking the ight servo and placing it on the left side of the bot and visa versa..?

blackheart · « **Reply #3 on:** April 22, 2009, 01:34:14 AM »

The servos are already modified by Hitec! I don't need to center them. I don't need any precise control on the position of the servos.

My problem is that the rover goes forward, goes backwards, goes right, but it doesn't go left. What am I doing wrong? It seems like everything is alright in the code, but if I call drive_left() or drive_right(), it still goes right and the functions are different!

Ro-Bot-X · « **Reply #4 on:** April 22, 2009, 05:03:34 AM »

Try using 1-179 instead of 0-180 as the range of speeds. Or make a function that will count down from 90 to 0 and see if the motors are going left. I think the 0 value is making them somehow jump to the other extreme and rotate in the oposite direction.

Razor Concepts · « **Reply #5 on:** April 22, 2009, 06:08:25 AM »

I checked the softwareservo source code and if you enter in values under 0 or over 180 it just limits it to 0 and 180.

blackheart · « **Reply #6 on:** April 22, 2009, 06:21:06 AM »

Ok, I could try that, but what I don't understand is the fact that it works with 0 and 180 when turning right, but it doesn't when turning left! How can you explain that?

And how can you explain the fact that it can go forward and backwards?
[img=http://img14.imageshack.us/img14/4082/robotlayout.th.jpg]

Razor Concepts · « **Reply #7 on:** April 22, 2009, 10:20:41 AM »

Move one servo at a time and see how it moves with values 0 and 180. It's just debugging, trying to find what each servo is actually doing.

blackheart · « **Reply #8 on:** April 23, 2009, 03:32:34 AM »

Please understand that each servo works the normal way individually. 0 and 180 as values work ok for max. speed in both directions. All servos work ok.

Here is the servo layout

S3 S1

S4 S2

Code: [Select]

void drive_forward(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(180);
  s2.write(180);
  s3.write(0);
  s4.write(0);
  SoftwareServo::refresh();
  }
}

This part works flawlessly. The rover moves forward. So does the reverse function...which makes the rover go backwards. I just exchange the values of s1, s2, s3, s4

Code: [Select]

void drive_backwards(void) {
  for(int i = 0; i < 5000; i++) {
  s1.write(0);
  s2.write(0);
  s3.write(180);
  s4.write(180);
  SoftwareServo::refresh(); 
  }
}

The only trouble I have is with getting the rover to go left. Meaning I have to move s1 and s2 in one direction, and s3 and s4 in the other direction. Considering the fact that they are placed opposite of each other, giving them all the same value would result in different direction for s1-2 and s3-4.

If I write 0 to all of them, the rover should go and goes right. All fine until now.

If I write 180 to all of the 4 servos, the rover should go left, BUT IT KEEPS GOING RIGHT, same as 0 to all of them.

That's what I don't understand. It goes forward, backwards and right, but it doesn't go left.

Ro-Bot-X · « **Reply #9 on:** April 23, 2009, 06:17:40 AM »

I had the surprise of seeing one servo going from full right to full left when I was sending commands to increase servo position step by step and the position reached 180. This is why I told you to try the range 1-179 instead of 0-180. Try it. Sometimes when all servos are on at the same time the software gets confused (probably a bug) and it throws the position out of range and resets it to 0.

Also I have noticed that you are issuing SoftwareServo::Refresh() too often, it takes the micro just a few cycles to go through the for loop and the refresh command shouldnt be issued faster than a few ms or problems could arrise. Add a delay(5) or so after the refresh command and shorten the number of times it goes through the for loop from 5000 to something like 500 and see what happens.

blackheart · « **Reply #10 on:** April 24, 2009, 03:19:27 AM »

I did exactly what you told me to... I added delay(5) after Servo::refresh(); and I changed the loop count to 500, instead of 5000. It works just the same. Forward, backwards and right, but still no left. It's driving me mad. I would rather have them all not working, because it would be easy to debug. WTF?

I am out of ideas!

Ro-Bot-X · « **Reply #11 on:** April 24, 2009, 06:04:09 AM »

Try this:

Code: [Select]

void step(void) {
  for(int i = 0; i <= 180; i++) {
  s1.write(i);
  s2.write(i);
  s3.write(i);
  s4.write(i);
  delay(20);
  SoftwareServo::refresh();
  }
}

blackheart · « **Reply #12 on:** April 24, 2009, 07:33:21 AM »

I already did try that, someone told me to. Every servo works with it. It goes one direction up to a point and then it changes directions. Still no luck with the rover.

I tried something else today. Taking turns with only 2 servos while blocking the others, but it's not efficient and it doesn't work well. I want to make it turn with all 4 servos. I also tried giving different values to the 4 servos, for example, 179, 178, 177, 176, and still no luck. If it goes forward, and it goes backwards, I thought it had a problem when all servos had the same value, so I changed it, but still no luck, as mentioned.

Do you think I should try controlling the servos the normal way, without SoftwareServo.h functions? Would that allow me to control 4 servos, or only 2 on pins 9 and 10?

Thank You very much!

Ro-Bot-X · « **Reply #13 on:** April 24, 2009, 07:07:22 PM »

Yes, you can use a Y cable to connect 2 servos in paralel to a single microcontroler pin. I would have done that in the first place instead of issuing 4 servo commands. Now you may want to go with the normal Servo library and use the range 0-180 or go with the ServoTimeTimer1 library to use real microseconds to fine tune the servo speeds. It all depends how accurate you need to be around the center (stop) point. I had to use microseconds because the servos were not perfectly centered and were slightly turning no matter where I set the stop point. Both libraries are SetAndForget type, that means the servo will be on at the specified speed until you tell it to stop. No Servo::Refresh command is necessary anymore.

blackheart · « **Reply #14 on:** April 25, 2009, 12:25:36 AM »

I don't want high accuracy. So I can still use the SoftwareServo.h library I guess(I am lazy, and this one is very easy for me). Wait, Y-cable? How come I didn't think of that? Thank you! A million times, thank you!

P.S. I told you I am lazy, so can I use the Y-cable just for the Signal(yellow) wire? The servos are already powered in parallel powered IN PARALLEL., that is OK, right?

Thank You!

SmAsH · « **Reply #15 on:** April 25, 2009, 01:33:39 AM »

yes, i suppose you can just use the y-harness for the signal, by parallel do you mean like from an external source?

blackheart · « **Reply #16 on:** April 25, 2009, 03:12:47 AM »

Yes, from an external source.

Here is the layout.

+ -

S1+ S1-

S2+ S2-

S3+ S3-

S4+ S4-

Ro-Bot-X · « **Reply #17 on:** April 25, 2009, 05:25:36 AM »

You can tie in paralel the signal wire for the both servos on the left side and for both servos on the right side. Connect left side to pin 9 and right side on pin 10. Use Servo library (uses Timer1 to generate the PWM signal) for these 2 servos and SoftwareServo library for others.

blackheart · « **Reply #18 on:** April 25, 2009, 06:01:06 AM »

Wait. I already did tie them together(the signal wire) and it works...but not to the left... I guess I will use pins 9 and 10 and use Servo library for these 4(actually 2) servos and SoftwareServo for the other 2 servos(non-modified) for sharp ir's.

Why and how do I use timer1? Can't I just do s1.write(180); and then delay(15);
?

Thank You! I will try this. Hope it works!

blackheart · « **Reply #19 on:** April 25, 2009, 09:33:30 AM »

sorry for double-posting. I couldn't modify the previous post again. I tried using the servo library, but still no luck, Works forwards, works backwards, works right, but doesn't work left. I really don't know what else to try. I can't explain why it does that.

Also, even if I don't put anything in the loop() function and I leave it blank, the servos still spin and the rover goes to the right. I think pins 9 and 10 are related to each other.

What else can I do?

UPDATE

Nothing? I am out of ideas. I tried anything. Should I try controlling the servos the normal way? without servo lib, or is that too difficult?

blackheart · « **Reply #20 on:** April 27, 2009, 08:11:08 AM »

Nobody knows what can be wrong? Now, it's like having 2 servos only, because the signal wire servo1 and servo2 are tied together and connected to a single digital pin(7) and the signal wires from servo3 and servo4 are also tied together and connected to digital pin(

. So even though it's like having only 2 servos, I am still having problems. I am going to say I have only 2 servos. Servo right(s1) and servo left(s2).

S2 S1

If I give s1 a value of 180 and I give s2 a value of 0, the rover(robot) moves forwards and I am happy with that.
If I give s1 a value of 0 and I give s2 a value of 180, the rover(robot) moves backwards and I am also very happy with that.
Now, here comes trouble.
If I give s1 a value of 180 and I give s2 the same value of 180, instead of going left, the rover goes right.
If I give s1 a value of 0 and I give s2 the same value of 0, the rover goes right.

Just to mention, both s1 and s2 work in both directions when they are controlled individually.

My question is why? When I had each servo hooked up individually, I thought the uC had a problem controlling 4 servos or something like this.

The purpose of this post is to update the thread and to give people more information in order to help me solve this problem.

Should I do the same for the power wires? Tie them together? For the power wires, I could tie them all 4 together(of course, separating positive from negative). Would that be ok?

hazzer123 · « **Reply #21 on:** April 27, 2009, 08:51:59 AM »

I just assembled a model of your robot... 4 servos attached to 2,4,6 and 8. I ran the code and it seemed fine.

Check you have the latest version of softwareservo?

Oh and you don't need to write to the servo each time in the loop. You should write to it once, then unless you want to change it, don't write to it again. Just keep refreshing it. Like this -

Code: [Select]

void drive_left(void) {
  s1.write(180);
  s2.write(180);
  s3.write(180);
  s4.write(180);
  for(int i = 0; i < 100; i++) {
    delay(10); //Delay 10ms
    SoftwareServo::refresh();
  }
  
}

Other than that, try using different ports for your servo, maybe 3,5,7,9 or sth. Switch servos around. Print out information over serial to your arduino console and see what is happening. Use an oscilloscope. Refresh your whole project directory and code. Check that you aren't actually compiling and programming the bot with old code. Switch the function calls around - fwd, back, right, left instead. Reinstall the arduino software. Try another arduino. Try it from a friends computer.

Just try everything you can think of, and get used to these kinds of problem, they are frustratingly common in my experience

blackheart · « **Reply #22 on:** April 27, 2009, 11:48:50 AM »

Thank You! I didn't solve the problem yet, but I thought I would post this.

Robot layout! Top View

S3 S1
| |
| |
| |
S4 S2

S1 is connected to S2 and S3 to S4, so it's like having 2 servos(I repeat this for the 10th time, sorry);

Here is the code that is uploaded right now on the Arduino. I have the same problem still

This code is still in its beta stage.

Code: [Select]

#include <SoftwareServo.h>
SoftwareServo sp1, sp2, sa, sb;
int pin_sp1, pin_sp2, pin_sa, pin_sb;
int pin_ir1, pin_ir2, pin_ir3;
float read_cm(byte pin) {
 int tmp;

 tmp = analogRead(pin);
 if (tmp < 3)
  return -1; // invalid value

 return (6787.0 /((float)tmp - 3.0)) - 4.0;
}

void fwd(void) {
  sp1.write(180);
  sp2.write(0);
  for(int i = 0; i < 100; i++) {
  SoftwareServo::refresh();
  delay(15);
  }
}

void back(void) {
  sp1.write(0);
  sp2.write(180);
  for(int i = 0; i < 100; i++) {
  SoftwareServo::refresh();
  delay(15); 
  }
}
  
void left(void) {
  sp1.write(180);
  sp2.write(180);
  for(int i = 0; i < 100; i++) {
  SoftwareServo::refresh();
  delay(15);
  }
}

void right(void) {
  sp1.write(0);
  sp2.write(0);
  for(int i = 0; i < 100; i++) {
  SoftwareServo::refresh();
  delay(15);
  }
}

void setup() {
  pin_sp1 = 3;
  pin_sp2 = 6;
  pin_sa = 10;
  pin_sb = 4;
  pin_ir1 = 0;
  pin_ir2 = 2;
  pin_ir3 = 4;
  sp1.attach(pin_sp1);
  sp2.attach(pin_sp2);
  digitalWrite(13, HIGH);
}

void loop() {
    fwd();
    back();  
    left();
    right();
}

I will try updating SoftwareServo.h and I will also try to upload the program from another PC...maybe a windows PC.

If you still have the robot assembled, please try it with this code.

S1(meaning s1 and s2) on pin 3 and S2(meaning s3 and s4) on pin 6.

Thank You very much!

hazzer123 · « **Reply #23 on:** April 27, 2009, 01:22:52 PM »

Hmmm it works fine on my setup...

I can't recommend anything else... Its STRANGE!

Here, i've attached my SoftwareServo as a zip. Try that out

blackheart · « **Reply #24 on:** April 27, 2009, 02:58:03 PM »

Stil, no luck! I used your SoftwareServo library, but it still doesn't work. I just don't get it! It is very weird. Could you please post some pictures, information, etc of your setup? I want to see what you did and compare.

Thank you. I am really out of ideas

hazzer123 · « **Reply #25 on:** April 27, 2009, 04:35:51 PM »

Hmm ok here is my setup. Im not sure it will help...

There is a shield i made on top of the arduino. Don't worry, the connections are definitely the way you described!

Ro-Bot-X · « **Reply #26 on:** April 27, 2009, 07:31:10 PM »

I am not sure this is going to help you or not, I just want to show you a different approach using ServoTimeTimer1 library. The following functions are used on my MiniEric robot. After receiving a command (Forward, Reverse, Left, Right) the functions will be called and the servos will accelerate to the top speed that was previously set. When a Stop command is received, the servos will be decelerated to a complete stop.

Code: [Select]

#include <ServoTimeTimer1.h>

#define RmotorPin 9     // digital pin  9, PWM pin
#define LmotorPin 10    // digital pin 10, PWM pin

ServoTimeTimer1 Lmotor;
ServoTimeTimer1 Rmotor;

byte Speed = 200;
byte TurnSpeed = 50;
byte Acc = 10;
byte direction = 1;

void setup() { 
  Lmotor.attach(LmotorPin);
  Rmotor.attach(RmotorPin);
  Lmotor.write(1315); // rev 1015 - 1315 - 1615 fwd
  Rmotor.write(1330); // rev 1030 - 1330 - 1630 fwd

}

void loop(){
  //get the commands here...
}


void Stop(){
  switch (direction) {
    case 0:                           // direction = reverse
      for (int s=0; s <= Speed; s=s+Acc) {
        Lmotor.write(1315-Speed+s);   // rev 1015 - 1315 - 1615 fwd
        Rmotor.write(1330-Speed+s);   // rev 1030 - 1330 - 1630 fwd  
        delay(20);
      }
      break;
    case 1:                           // direction = forward
      for (int s=0; s<=Speed; s=s+Acc) {
        Lmotor.write(1315+Speed-s);   // rev 1015 - 1315 - 1615 fwd
        Rmotor.write(1330+Speed-s);   // rev 1030 - 1330 - 1630 fwd  
        delay(20);
      }
    break;
    case 2:                               // direction = left
      for (int s=0; s<=TurnSpeed; s=s+Acc) {
        Lmotor.write(1315+TurnSpeed-s);   // rev 1015 - 1315 - 1615 fwd
        Rmotor.write(1330-TurnSpeed+s);   // rev 1030 - 1330 - 1630 fwd  
        delay(20);
      }
      break;
    case 3:                               // direction = right
      for (int s=0; s<=TurnSpeed; s=s+Acc) {
        Lmotor.write(1315-TurnSpeed+s);   // rev 1015 - 1315 - 1615 fwd
        Rmotor.write(1330+TurnSpeed-s);   // rev 1030 - 1330 - 1630 fwd  
        delay(20);
      }
      break;
  }
}

void Forward(){
  direction = 1;
  for (int f=0; f<=Speed; f=f+Acc) {
    Lmotor.write(1315+f); // rev 1015 - 1315 - 1615 fwd
    Rmotor.write(1330+f); // rev 1030 - 1330 - 1630 fwd  
    delay(20);
  }
}

void Reverse(){
  direction = 0;
  for (int r=0; r<=Speed; r=r+Acc) {
    Lmotor.write(1315-r); // rev 1015 - 1315 - 1615 fwd
    Rmotor.write(1330-r); // rev 1030 - 1330 - 1630 fwd  
    delay(20);
  }
}

void TurnLeft(){
  direction = 2;
  for (int L=0; L<=Speed; L=L+Acc) {
    Lmotor.write(1315+L); // rev 1015 - 1315 - 1615 fwd
    Rmotor.write(1330-L); // rev 1030 - 1330 - 1630 fwd  
    delay(20);
  }
}

void TurnRight(){
  direction = 3;
  for (int R=0; R<=Speed; R=R+Acc) {
    Lmotor.write(1315-R); // rev 1015 - 1315 - 1615 fwd
    Rmotor.write(1330+R); // rev 1030 - 1330 - 1630 fwd  
    delay(20);
  }
}

blackheart · « **Reply #27 on:** April 28, 2009, 12:57:11 AM »

Although I don't understand much of your code, maybe it will help. 2 servos work for me too. but when I have 4(even though they are tied as 2) they stop working correctly.

I will try using your code and I will see.

Thank You!

UPDATE
It doesn't even compile. I downloaded a version of Servotimetimer1 from arduino.cc, but it has lots of bugs.

Could you please upload your ver. of the lib?

I have a 6V NI-CD 620mAh battery powering the servos with a voltage regulator(because when it's fully charged, it goes up to 7V) . Could it be because of the battery? Is 620mAh not enough for 4 servos?

Thank you!

blackheart · « **Reply #28 on:** April 28, 2009, 09:49:31 AM »

No result. Could it be because of the low current the 6V Ni-CD supplies(620mAh)? What can I do regarding the Servotimetimer.h library? It has some errors!

UPDATE

One more piece of information that can maybe help. I have s1 on pin 3 and s2 on pin6. If I reverse the connections(s1 on 6 and s2 on 3) the functions fwd and back are reversed, meaning fwd is going backwards and back is going forwards. BUT, the functions left and right stay the same. Meaning the rover keeps going only right. I thought it was gonna go only left with the pins reversed, but it still goes only right. I think it is a hardware problem. WHAT DO YOU THINK?

hazzer123 · « **Reply #29 on:** April 28, 2009, 11:51:11 AM »

Wow something is seriously wrong there...

It seems your robot has developed a understanding of direction and prefers right

Hmmm I cant help again sorry.

But if you do find your problem (i suspect you will and you will kick yourself.... i have done it many many times) then definitely post it here! I'm so intrigued by this!

News:

Author Topic: SoftwareServo.h and 4 continuous rotation servos are hell-bent on destroying me! (Read 16184 times)