Skip to content
Permalink
Browse files

target verification

  • Loading branch information...
martind
martind committed Oct 10, 2014
1 parent 8ca9250 commit 0e8db786c3769dd2cdbf3a65f029e2fa2d80fce4
Showing with 14 additions and 3 deletions.
  1. +14 −3 sickday2014.py
@@ -385,7 +385,9 @@ def goToDigit( self, digit, info = None ):
break
self.robot.setSpeedPxPa( *cmd )
self.robot.update()
self.approachFeeder( digitHelper=digit )
if not self.approachFeeder( digitHelper=digit, verifyTarget = True ):
self.driver.goStraight( -1.0, timeout=15 )
self.approachFeeder( digitHelper=digit, verifyTarget = False )
return True


@@ -409,9 +411,10 @@ def wait( self, duration ):
self.robot.update()


def approachFeeder( self, timeout=60, digitHelper=None ):
def approachFeeder( self, timeout=60, digitHelper=None, verifyTarget=True ):
"robot should be within 1m of the feeder"
print "Approaching Feeder"
verified = False
desiredDist = 0.4 #0.2
countOK = 0
startTime = self.robot.time
@@ -434,6 +437,7 @@ def approachFeeder( self, timeout=60, digitHelper=None ):
for a in arr:
for digit, (x,y,dx,dy) in a:
if digit == 'X':
verified = True
angularSpeed = (320-(x+dx/2))/100.0
# print "angularSpeed", math.degrees(angularSpeed), self.robot.cameraData[1], (x,y,dx,dy)
centerX = 320
@@ -492,11 +496,16 @@ def approachFeeder( self, timeout=60, digitHelper=None ):
self.robot.update()
print "done."

if verifyTarget and not verified:
print "TARGET not verified!"
return False

# compute proper rotation and backup distance
toTurn, toBackup = computeLoadManeuver( minDistL, frontDist, minDistR )
print "Suggestion: ", math.degrees(toTurn), toBackup
self.driver.turn( toTurn, angularSpeed = math.radians(20), timeout=30, verbose=True )
self.driver.goStraight( toBackup, timeout=30 )
return True


def waitForCode( self, timeout=10 ):
@@ -706,7 +715,9 @@ def ver0( self, verbose=False ):
gameStartTime = self.robot.time
while True:
self.goToCenterArea()
self.approachFeeder()
if not self.approachFeeder( verifyTarget = True ):
self.driver.goStraight( -1.0, timeout=15 )
self.approachFeeder( verifyTarget = False )
self.turnLights( on=True )
digit = self.waitForCode( timeout=10 )
self.turnLights( on=False )

0 comments on commit 0e8db78

Please sign in to comment.
You can’t perform that action at this time.