Continuous servo rotation after few joy pad commands

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

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?

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

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.

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