The array of sonars are used for object avoidance. If an object is detected solely on the left or right sensor, the robot will veer to the opposing side. If an object is detected in the middle sonar, the robot will brake (hard stop), back up, and veer around the object as well as calculate a new heading and distance based on the turning radius and arc length from compass readings. The following defines the sonar class and returns an array of sonar readings.
import threadedserial
import timeclass Sonar( threadedserial.ThreadedSerial ):
def __init__( self, port, baud, command = 0 ):
super( Sonar, self ).__init__( 'Sonar', port, baud, command )
def getValidSonar( self ):
'''
This breaks down the sonar values into an array
Returns an array of obstacles
True = obstacle exists within threshold
False = no obstacle exists within threshold
'''
if self:
pings = self.reading.split( )
ping = [0 for i in range( len( pings ) )]
for i in range( len( pings ) ):
ping[len( pings ) - 1 - i] = int( pings[i] )
return ping
else:
return None
'''11 , 6'''
port_sonar = 2
The following code sets the turningRadius as 4 feet. It uses the math import and trigonometric functions to calculate a new heading and distance after making the correction.
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)
Chassis



The chassis was redesigned this year to lower the center of gravity and reduce tipping. In previous years, the limiting factor was the compass as magnetic fields generated by the motor would interfere with the compass readings. This year, a new compass module was acquired and proved reliable at its current mounting position at the from of the chassis just behind the bumper. Additionally, a rear bumper was installed in order to detect the possibility of backing up into the cone during the cone tracking portion of the code. The rear bumper will also be used as a start button so that the remote desktop can be disconnected prior to the competition run.
Servo Camera
![]() |
Above: Camera mounted on servo |
For additional functionality on the camera system, it was mounted on a servo. The servo rotates approximately 180 degrees allowing the robot to scan the surrounding area without moving. In the future, a function will track the cone and send servo values to the motor module. The test code defining panCamServo as well as the test function for sweeping the servo are below:
def panCamServo( self, amount = 0 ):
'''
The turn function takes a percentage and convert to appropriate turning angle
-100. to < 0. turns left, > 0. to +100. turns right
Any values outside of this range will place the servos back to neutral position
'''
if amount >= -100 and amount <=100:
self.panServo = int( amount / 100. * self.pwm_attr[self.panServo][1] )
elif amount < -100:
self.panServo = -self.pwm_attr[self.panServo][1]
elif amount > 100:
self.panServo = self.pwm_attr[self.panServo][1]
else:
self.panServo = 0
self.__send_pwm( self.panServo )
The following is the test function for moving the camera. The camera will start pointing all the way to the left of the robot and sweep to the right in increments of 5 (range of -100 to 100) every .5 seconds:
port_motor = 15
if __name__=="__main__":
holding = 100
motors = Maestro(port_motor)
motors.doSetAttr( [[1460,640],[1650,656],[1496,504],[1500,500],[1500,700]] )
while holding > -100:
motors.panCamServo(holding)
print "moving the camera", holding
holding = holding - 5
time.sleep(.5)
No comments:
Post a Comment