During the Spring 2013 intercession, progress was made to the RoboMagellan platform in many aspects. I will be outlining the improvements made in the following areas: compass, magellanclass, heading corrections, and avoid function.
The compass placement on the platform has been the most troublesome hardware component on the platform. After the decision was made to remove the large expanded PVC tail from the robot, that had previously housed the compass, a new position for the compass was needed. The compass was first placed at the front of the robot, on the new chassis that Vincent built. This position provided consistent results and allowed testing of other systems to be accomplished. The compass was then found to not respond linearly. This was discovered by comparing the compass reading from the robot, and the compass reading from a smartphone with a compass, while turning the robot through 90 degrees, each time. This showed some 90 degree turns exibiting compass reading changes as large as 135 degrees or as small as 45 degrees depending on the compass position. The compass was repositioned to multiple locations and placed on top of supports, up to approximately 12 inches in length, in an attempt to correct the discrepancies. These proved to only make the problem more pronounced. The compass was reattached to its original location and a software solution was chosen.
The following function is called from within the getHeading function of the compass class and takes 2 arrays as arguments, it returns a heading that has been corrected based on the values in the table and linear interpolation.
def correctHeading(self, rawHeading, raw, calibrated):
#print rawHeading
for index in range(0,len(raw)):
#print index
if rawHeading >= raw[index] and rawHeading <= raw[index+1]:
headingSpacingRaw = float(raw[index]-raw[index+1])
#print "headingSpacingRaw",headingSpacingRaw
#headingSpacingCalibrated = float(calibrated[index]-calibrated[index+1]) #should be 10
headingSpacingCalibrated = float(-10) #avoids having to deal with 350 to 0
#print "headingSpacingCalibrated",headingSpacingCalibrated
#find the ratio of the spacing between raw and calibrated
correctionSlope = headingSpacingCalibrated/headingSpacingRaw
#print "correctionSlope",correctionSlope
#find the difference between the testvalue and the lower Raw value
InterpolationValue = (rawHeading - raw[index])
#print "InterpolationValue",InterpolationValue
#Look up the calibrated Value and add the interpolation
CorrectedValue = (calibrated[index] + InterpolationValue * correctionSlope)%360
#print CorrectedValue
return CorrectedValue
The code first finds where the value falls within the list. It then calculates the difference of each of the arrays. A ratio between the two differences is then calculated and is used as the slope of the function for linear interpolation. The difference between the heading and lower value is then multiplied by the slope and added to the lower calibrated value to determine the corrected heading. This has made the compass accurate enough to allow the robot to end within a range where the camera can discern the cone.
The magellanclass has seen evolutionary improvements to allow for new functionality and improve existing functions. The gains for the PID loop for steering were tuned to achieve stability while allowing for quick enough corrections to account for terrain changes and new headings. The PID values appear to be related to speed and if other improvements allow for greater speeds, this may need to be addressed.
During testing it was found that if the robot was started at a heading, other than the desired heading, errors were incured in the final position of the robot. This was determined to be caused by the turning radius of the robot and the distance traveled in the turn. The following function was written to eliminate these errors.
def correctInitialHeadError( self, desiredHeading, actualHeading, desiredDistance ):
headingDifference = self.angle_fix( desiredHeading, actualHeading )
print "headingDifference", headingDifference
turningRadius = 4
#if (headingDifference > 180):
# headingDifference = headingDifference - 360
#if (headingDifference < -180:
# headingDifference = headingDifference + 360
if headingDifference < -90:
xError = turningRadius * (-1 + math.sin(math.radians(headingDifference + 90)))
elif headingDifference > 90:
xError = turningRadius * (1 + math.sin(math.radians(headingDifference - 90)))
else:
xError = turningRadius * math.sin(math.radians(headingDifference))
yError = math.fabs(turningRadius * math.sin(math.radians(headingDifference)))
print "xError", xError, "yError", yError
actualDistance = math.hypot(xError, (desiredDistance - yError))
print "actualDistance", actualDistance
correctedHeading = desiredHeading - math.degrees(math.atan2(xError, actualDistance))
if(correctedHeading > 360):
correctedHeading = correctedHeading - 360
actualDistance = actualDistance + (math.pi * turningRadius) * (math.fabs(headingDifference) / 180)
print "actualDistance", actualDistance
return float(correctedHeading), float(actualDistance)
The function calculates the difference between the initial heading and the desired heading. it then uses the turning radius to calculate the x and y error due to the turn. These errors are then used to form a triangle and the pythagorean theorem to determine a new distance. This distance is then added to the distance traveled in the turn and the heading and actual distance are returned. This function greatly improved the accuracy and precision of the robots destination.
The avoid function was almost completely rewritten during the spring semester. This allowed for better object avoidance and to improve the clarity of the code. The new code has been performing well. Over the next few months we hope to implement the ability to get out of stuck situations, account for errors in heading and distance due to object avoidance, and the integration of the horn and fine tuning of parameters.
def avoid(self):
###############################################################################
# Tommy and Mason rewrite 2013
#
# 1. Check to see if there are obstacles in each of the three sensors at 100 cm, 150 cm or 100 cm or bump switch
# 2. Is it in the center?
# a. Backup
# b. Check the left, if not obstacle, go left by turning the wheel.
# c. Else, go to the right by turning the wheel
# d. Turn the motors on backward for 20 ticks ( about a foot) and until the obstacle is gone
# 3. Is there a obstacle on the left side? Turn the wheels right until it goes away
# 4. Is there a obstacle on the right side? Turn the wheels left until it goes away
if( self.hasObstacle( 100 )[0] or self.hasObstacle( 150 )[1] or self.hasObstacle( 100 )[2]): #or self.compass.getBumper( )
ticks = self.encoder.getValidEncoder( )
starting = self.compass.getHeading( )
print 'Found Obstacle',self.hasObstacle( )
if( self.hasObstacle( 150 )[1] or self.compass.getBumper( ) ):
ticks = self.encoder.getValidEncoder( )
starting = self.compass.getHeading( )
print 'Found Obstacle',self.hasObstacle( )
if( self.hasObstacle( 150 )[1] or self.compass.getBumper( ) ):
print 'something is in front of me, I should back up'
self.motors.drive(-10) #brake
time.sleep(.5)
if not self.hasObstacle( 200 )[0]:
print 'nothing on left, going there'
self.motors.turn( 100 )
else:
print 'default, going there'
self.motors.turn( -100 )
initialPosition = self.encoder.getValidEncoder( )
while ((self.encoder.getValidEncoder( )[0] > initialPosition[0] - 20) or (self.hasObstacle( 150 )[1])):
self.motors.drive( -40 )
self.motors.reset( )
time.sleep( 2 ) #Temp for debug
self.motors.turn( 0 )
print "Turned 20 degrees"
while (self.hasObstacle( )[0]): #Detect on Left
# print "Detect on Left"
self.motors.drive(40)
self.motors.turn(100)
while (self.hasObstacle( )[2]): #Detect on Right
# print "Detect on Right"
self.motors.drive(40)
self.motors.turn(-100)
self.motors.turn( 0 )
return self.encoder.getValidEncoder( )
Subscribe to:
Post Comments (Atom)
No comments:
Post a Comment