Continuous servo rotation after few joy pad commands

Forums:

Hy,

I connected an UltraBorg to my ThunderBorg and Pi. At the TB four 12V DC-Motors are attached, at the UB there are 4 MG996R attached. Everything is powered by 6s Lipo and is doing fine.

At my code, when i press a button, the servos are rotating 45 degree forming a circle to spin arround. Another button moves the servos in the initial position to drive straight.

The problem i detected is, after a few commands (pressed more or less fast button 1 and press button 2 to play around and rejioce the working code) to rotate unrotate the servo, one of the servos rotates continuously. It is not every time the same servo to rotate permanent.

I don't know if i reproduce the failure or the failure comes up after a certain number of commands. I can't stop the rotation until i switch off the battery.

Do you have an idea what's wrong with the servo or UB? Otherwise the code runs well.

Thanks in advance for your answer.

Images: 
piborg's picture

It sounds like the servos might be getting a position beyond their movement range occasionally.

If you can attach the code it might be helpful diagnosing the cause.

I am not able to copy the code in a good format, can you help me with this too?
Starting with line 187 i set the position with button command defined at line 58 and 59.

The code is the tbJoystick with added ub parts.

#!/usr/bin/env python
# coding: Latin-1

# Load library functions we want
import time
import os
import sys
import pygame
import ThunderBorg

import UltraBorg

# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr

# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Setup the ThunderBorg
TB = ThunderBorg.ThunderBorg()
#TB.i2cAddress = 0x15                  # Uncomment and change the value if you have changed the board address
TB.Init()
if not TB.foundChip:
    boards = ThunderBorg.ScanForThunderBorg()
    if len(boards) == 0:
        print 'No ThunderBorg found, check you are attached :)'
    else:
        print 'No ThunderBorg at address %02X, but we did find boards:' % (TB.i2cAddress)
        for board in boards:
            print '    %02X (%d)' % (board, board)
        print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
        print 'TB.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    TB.SetCommsFailsafe(True)
    failsafe = TB.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (TB.i2cAddress)
    sys.exit()

# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = False              # Set this to True if up and down appear to be swapped
#axisLeftRight = 0                       # Joystick axis to read for left / right position
#axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonSlow = 2                          # Joystick button number for driving slowly whilst held (Button X)
slowFactor = 0.5                        # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFast = 3                          # Joystick button number for driving slowly whilst held (Button Y)
fastFactor = 1.5
buttonSpinLeft = 4                      # LB Button at Joypad
buttonSpinRight = 5                     # RB Button at Joypad
#buttonFastTurn = 9                     # Joystick button number for turning fast (R2)
buttonServoSpin = 0                     # Set Servos +/- 45° to spin around z-Axis (Button A)
buttonServoUnspin = 1                   # Set back Servos to drive normally (Button B)
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time

# Power settings
voltageIn = 25.2                        # Total battery voltage to the ThunderBorg
voltageOut = 12.0                       # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Show battery monitoring settings
battMin, battMax = TB.GetBatteryMonitoringLimits()
battCurrent = TB.GetBatteryReading()
print 'Battery monitoring settings:'
print '    Minimum  (red)     %02.2f V' % (battMin)
print '    Half-way (yellow)  %02.2f V' % ((battMin + battMax) / 2)
print '    Maximum  (green)   %02.2f V' % (battMax)
print
print '    Current voltage    %02.2f V' % (battCurrent)
print

# Setup pygame and wait for the joystick to become available
TB.MotorsOff()
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,1)
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
#pygame.display.set_mode((1,1))
print 'Waiting for joystick... (press CTRL+C to abort)'
while True:
    try:
        try:
            pygame.joystick.init()
            # Attempt to setup the joystick
            if pygame.joystick.get_count() < 1:
                # No joystick attached, set LEDs blue
                TB.SetLeds(0,0,1)
                pygame.joystick.quit()
                time.sleep(0.1)
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick, set LEDs blue
            TB.SetLeds(0,0,1)
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        TB.SetCommsFailsafe(False)
        TB.SetLeds(0,0,0)
        sys.exit()
print 'Joystick found'
joystick.init()
TB.SetLedShowBattery(True)
ledBatteryMode = True
try:
    print 'Press CTRL+C to quit'
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    # Loop indefinitely
    while running:
        # Get the latest events from the system
        hadEvent = False
        events = pygame.event.get()
        # Handle each event individually
        for event in events:
            if event.type == pygame.QUIT:
                # User exit
                running = False
            elif event.type == pygame.JOYBUTTONDOWN:
                # A button on the joystick just got pushed down
                hadEvent = True
            elif event.type == pygame.JOYBUTTONUP:
                # A button on the joystick just got released
                hadEvent = True
            elif event.type == pygame.JOYAXISMOTION:
                # A joystick has been moved
                hadEvent = True
            if hadEvent:
                # Read axis positions (-1 to +1)
                if axisUpDownInverted:
                    upDown = -joystick.get_axis(axisUpDown)
                else:
                    upDown = joystick.get_axis(axisUpDown)
                #if axisLeftRightInverted:
                    #leftRight = -joystick.get_axis(axisLeftRight)
                #else:
                    #leftRight = joystick.get_axis(axisLeftRight)
                
                # Apply steering speeds
                #if not joystick.get_button(buttonFastTurn):
                    #leftRight *= 0.5
                
                # Determine the drive power levels
                driveLeft = -upDown
                driveRight = -upDown
                if leftRight < -0.05:
                    # Turning left
                    driveLeft *= 1.0 + (2.0 * leftRight)
                elif leftRight > 0.05:
                    # Turning right
                    driveRight *= 1.0 - (2.0 * leftRight)
                
                # Check for button presses
                if joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                if joystick.get_button(buttonFast):
                    driveLeft *= fastFactor
                    driveRight *= fastFactor
                
                if joystick.get_button(buttonSpinLeft):
                    driveLeft = -0.5
                    driveRight = 0.5
                if joystick.get_button(buttonSpinRight):
                    driveLeft = 0.5
                    driveRight = -0.5
                    
                if joystick.get_button(buttonServoSpin):
                    UB.SetServoPosition1(0.9)
                    UB.SetServoPosition2(-0.9)
                    UB.SetServoPosition3(-0.9)
                    UB.SetServoPosition4(0.9)
                    
                if joystick.get_button(buttonServoUnspin):
                    UB.SetServoPosition1(0)
                    UB.SetServoPosition2(0)
                    UB.SetServoPosition3(0)
                    UB.SetServoPosition4(0)
                
                # Set the motors to the new speeds
                TB.SetMotor1(driveRight * maxPower)
                TB.SetMotor2(driveLeft * maxPower)
        # Change LEDs to purple to show motor faults
        if TB.GetDriveFault1() or TB.GetDriveFault2():
            if ledBatteryMode:
                TB.SetLedShowBattery(False)
                TB.SetLeds(1,0,1)
                ledBatteryMode = False
        else:
            if not ledBatteryMode:
                TB.SetLedShowBattery(True)
                ledBatteryMode = True
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    TB.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    TB.MotorsOff()
    TB.SetCommsFailsafe(False)
    TB.SetLedShowBattery(False)
    TB.SetLeds(0,0,0)
print

I configured the servos with the tuning GUI to set the max and min. To avoid the rotation or reaching max/min position i reduced the set.servopostion(), but this was not helpful.

thank you

piborg's picture

I cannot see anything wrong with your code, the problem must be elsewhere.

If you open the UltraBorg GUI it should show you where the UltraBorg has set the servo position to:

Try moving the sliders up and down to see if you can recreate the problem. If you can then what does the servo position value read for the servo which is constantly rotating?

Hy,

i detected no misconduct while playing with the ubGui. I moved the slider up and down or set directly to the min/max position (middle mouse button).

As i played with the joystick, i detected a delay of moving the servos, they move not identical or at the same time. What i think is, there is something wrong with the signals.

The next steps i'll do to detect the cause of the failure are:
- write delay in the code between the sensor moving
- play with 1, 2, 3 numbers of servos to detect at which number of servos the misbehavoiur occurs.

I report the results in the next days.

Nice Weekend to all of you

Hy,

I made some tests with increasing number of the servos. Up to two servos everything is fine and they move very good. with the third servo the misbehaviours begin. But It is hard to get the failure with 3 servos.

With delay after every servo moving works fine at the first try, but this looks weird and is not really usable. I inserted an single delay after the spin and unspin command. Unfortunately, after several pressed buttons, one of the servos started rotating again directly without counting a delay.

is there another way to configure the speed of the servos as described in the ubSequence.py

Do you have more test in mind i can do?

piborg's picture

In the ubSequence.py script we control the speed of the movement by making lots of small moves instead of one big one.

This is fairly easy to implement:

if joystick.get_button(buttonServoSpin):
    # Move to the target slowly
    stepDistance = 0.01
    stepDelay = 0.1
    position = UB.GetServoPosition1() + stepDistance
    while position < 0.9:
        UB.SetServoPosition1(+position)
        UB.SetServoPosition2(-position)
        UB.SetServoPosition3(-position)
        UB.SetServoPosition4(+position)
        time.sleep(stepDelay)
        position += stepDistance
    # Set the exact final position
    UB.SetServoPosition1(0.9)
    UB.SetServoPosition2(-0.9)
    UB.SetServoPosition3(-0.9)
    UB.SetServoPosition4(0.9)

The rate is controlled by both stepDistance and stepDelay. I would recommend reducing the delay to speed things up.

The problem is that you will loose any control until the change has finished.

Given the number of servos moving has an effect, this may simply be that the 5V supply for the servos is not able to deliver enough current for all four. What are you using to power the servos?

Hy,

like in the picture of NASAs clean room, I want to build my own Mars Rover and therefore the front and rear wheels has to move befor changing the direction. Thats why I need the four servo (I know I can spin/steer directly w/o the servo, this was done for introducing myself in the Borgs).

I don't added the speed of the servos like in the ub Sequence, but it will be in the upcoming week.
With a second UB, maybe i can avoid this misbevaiour of the servos too. Can I use one BattBorg for two UB or uses every UB his own BattBorg?

Thank you in advance
Daniel

Images: 
piborg's picture

It sounds like a fun project :)

You can power two UltraBorgs from the same BattBorg, they will share the power as needed.

Hy,

yes, the project gives me a lot of fun but also a lot of frustration :D But the finished steps motivates to continue.

I found in the forum (http://forum.piborg.org/node/1812) i can replace the battborg with an UBEC (i have an UBEC-3A-6S and using a 6S-LIPO). Is it this simple to replace the Battborg with the Ubec. The Pi is powered by the connection of the TB. I am using the connection of the picture below.

W.r.t to the connection below. You mentioned i can power two UB with one BattBorg. But for me it is not really clear how to wire. I would just add a connection from V+/ GND of UB1 to V+/ GND to UB2 = parallel connection.

thanks in advance.

Images: 
piborg's picture

Using the UBEC to power the servos is a good idea as it can provide more current for the servos than the BattBorg can.

The first thing to do is ensure both "5V link" jumpers are removed so that the UBEC is not connected to the Pi's 5V line:

The UBEC then needs to be wired so that the battery + and - are connected to the battery and the output + and - are connected to the V+ and GND screw terminals on both UltraBorgs. These are connected in parallel as you have described (UB1 V+ to UB2 V+, UB1 GND to UB2 GND).

Hy,
I replaced the Battborg with an UBEC to power the UB. I switched many times and very fast my servos with good behaviour, no rotations detected but the yellow flash appeared (with monitor attached to HDMI). I added the 4 motors too and tried again. Still good behaviour and the yellow flash also disappeared.

I am happy and can go one. Add the commands to the webIU and building the chassis.

Thanks for your support.
We read each other :D

Subscribe to Comments for &quot;Continuous servo rotation after few joy pad commands&quot;