Problem with servo360 library

Roy Eltham

I have a bot driving around using the 360 feedback servos. Using abdrive360 with just drive_speed to move/turn and stop. I am using simpleIDE with the latest Learn folder libraries, but the same thing happens in BlocklyProp (since it uses the same libraries).

If the bot gets stuck against a wall while going forward (or backward) such that the wheels can not turn, then the servo360 code gets stuck constantly trying to turn the servos. Even if you do drive_speed(0,0), it just ignores that. No other drive_speed() calls will work. In order to get things unstuck you have to pickup the bot so the wheels can turn, and they will turn a bunch in the direction it was going when it got stuck. I've tried a bunch of different things to resolve this, including turning feedback off and back on, disabling and enabling the servos, and using servo360_getAngle followed by servo360_angle to make it stop trying to go. The angle functions sort of work to stop the problem, but after calling them (even once) the drive_speed calls become erratic and odd.

I looked over the abdrive360 and servo360 code a bunch and there doesn't appear to be any way for me to resolve this without changing the library code.

Also, there is no way that I could find to reset/disconnect the servos once you have called drive_init/Servo360_connect functions. I thought maybe I could detect that the bot is stuck and then reset things, but that's not an option either...

Any help on this would be greatly appreciated!

Thanks!

Whit

Roy,

I posted this as Issue #1552 on GitHub.

I assume this with your Parallaxy - Correct?

Roy Eltham

Whit,
Yeah, it's the Parallaxy. I have been updating and adding to the basic firmware code, and wanted to try and fix this issue. I guess I should have checked the github for issues.

Thanks for point it out to me Whit.

Andy Lindsay (Parallax)

Hi Roy,

If you edit the Simple Libraries in C, make sure to use the Demo branch.

I'm trying to make this problem happen in a controlled environment where I can see what's happening (in a way that leads to a fix), but no luck so far.

I've been using the code below, and holding various combinations of wheels for various amounts of time. All I see is the usual behavior, which is that when the wheel exceeds half a turn of error, it stops incrementing the distance goal. When I let go of one or both wheels, it (or they) briefly turn rapidly to catch up that half turn, and then behave normally.

Any advice on setting up the failure would be appreciated.

Thanks, Andy

#include "simpletools.h"
#include "abdrive360.h"

int lt, rt, lc, rc;

int main()                   
{
  drive_speed(64, 64);
  for(int n = 0; n < 20; n++)
  {
    drive_speed(64, 64);
    drive_getTicks(&lt, &rt);
    drive_getTicksCalc(&lc, &rc);
    lc = lc * 64 / 4096;
    rc = rc * 64 / 4096;
    print("Calc:  %d, %d\r", lc, rc);
    print("Ticks: %d, %d\r\r", lt, rt);
    pause(1000);
    //pause(2000);    
    //drive_speed(-64, -64);
    //pause(1000);    
    //drive_speed(0, 0);
    //pause(100);    
  }    
  drive_speed(0, 0);

  drive_speed(-64, 64);
  for(int n = 0; n < 20; n++)
  {
    drive_getTicks(&lt, &rt);
    drive_getTicksCalc(&lc, &rc);
    lc = lc * 64 / 4096;
    rc = rc * 64 / 4096;
    print("Calc:  %d, %d\r", lc, rc);
    print("Ticks: %d, %d\r\r", lt, rt);
    pause(100);    
  }    
  drive_speed(0, 0);

  drive_speed(64, -64);
  for(int n = 0; n < 20; n++)
  {
    drive_getTicks(&lt, &rt);
    drive_getTicksCalc(&lc, &rc);
    lc = lc * 64 / 4096;
    rc = rc * 64 / 4096;
    print("Calc:  %d, %d\r", lc, rc);
    print("Ticks: %d, %d\r\r", lt, rt);
    pause(100);    
  }    
  drive_speed(0, 0);

  drive_speed(-64, -64);
  for(int n = 0; n < 20; n++)
  {
    drive_getTicks(&lt, &rt);
    drive_getTicksCalc(&lc, &rc);
    lc = lc * 64 / 4096;
    rc = rc * 64 / 4096;
    print("Calc:  %d, %d\r", lc, rc);
    print("Ticks: %d, %d\r\r", lt, rt);
    pause(1000);    
  }    
  drive_speed(0, 0);

}

Andy Lindsay (Parallax)

Here's another idea.

servo360_setTurns(13, 0);
servo360_setTurns(12, 0);

Roy Eltham

Andy, the issue is that if you continue to stop the wheels from going forward, and issue a new drive_speed call with 0,0 or with -60, -60 (to back up), it will not respond to this new call. It will continue to try going forward.

The key is that the wheels are stuck and you use drive_speed() to go forward while they are already stuck, then you can't stop or back up.

The situation I am in is with the letsrobot.tv site where the robots are being controlled remotely via video stream. No one is at the bot in order to move it out of it's stuck state. You press the forward button driving the bot into a wall where it gets stuck the wheels can't turn and it is still trying to go forward. The bot code after a short delay does a drive_speed(0,0) call, but the bot still tries to go forward. If you try pressing the backup button which calls drive_speed(-60,-60), it also ignores that.

twm47099

With my ActivityBot 360 I have a whisker routine that waits (waitpne) until a whisker is pressed, then immediately takes control from the normal command cog, stops the bot, backs it up turns 90 deg, stops, gives control back, and goes back to sleep.

It stops quickly enough that the bot doesn't stall. When I tried stopping manually or even using whiskers in the main cog the bot would run up on the obstacle or stall.

Is it possible to add whiskers or some other sensor that would detect when the bot is about to hit and trigger an E-stop?

I agree that a method is needed to unlock a servo360 that was issued a drive_speed command but can't turn.

Tom

Andy Lindsay (Parallax)

Every 50th of a second, the servo360 library recalculates each servo's target position, compares it to the measured position, and adjusts drive level based on the difference. If the culprit is that the control system stops recalculating target position if the error is greater than half a wheel turn, the Simple Libraries folder in this should fix it.

https://github.com/parallaxinc/Simple-Libraries/archive/Dev.zip

It always recalculates target position, but resets it to the previous target position if it's greater than half a turn AND still increasing. If it's decreasing, it keeps the new target position. So, when the servo keeps trying to push the wall, new target positions are not calculated after target position error has increased to half a turn. By changing direction to back away from the wall, target error decreases, and after half a turn, it should stop pushing against the wall and back up.

If you try it, I would suggested deleting your current Simple Libraries folder and replacing it with the one in the zip.
Please let me know (1) if this does help it escape the wall trap, and (2) if it causes any other new and unexpected behaviors in normal navigation.

Andy Lindsay (Parallax)

Darn, sorry, if you downloaded the zip between the previous post and this one, it didn't have the test fix. It was committed, but not pushed. It is now pushed. Please retry.

Roy Eltham

Andy, I have just downloaded the file now, but I can't do the testing with it until later today/evening. Prior plans away from my bench.
I will test it out and get back to you either late tonight or tomorrow. Thanks!

Roy Eltham

Andy,
I have not tried this myself yet, but Jill from letsrobot.tv did try it and she says it works well. So that's at least one positive data point.

Whit

Roy Eltham wrote: »

Andy,
I have not tried this myself yet, but Jill from letsrobot.tv did try it and she says it works well. So that's at least one positive data point.

Cool!

Roy Eltham

Andy,
Sorry, but I haven't gotten to try this yet myself. Got sidetracked with other things that last two nights. I will check it out tonight.
Although, Jill did test it (like I said above) and I believe is using it in at least one of her bots on letsrobot.tv, so I am pretty sure it's fine.

Roy Eltham

Okay, I finally got this installed on my machine and updated my bot firmware. It's working great, I can't get my bot stuck anymore like I used too!
Thanks so much Andy!