Sunday, February 17, 2013

Sonar Object Avoidance, Chassis, Servo Camera

Sonar Object Avoidance


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 time

class 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