def __init__(self):
self.midbrain = Midbrain()
def __init__(self): self.midbrain=Midbrain()
class Forebrain:
def __init__(self):
self.midbrain = Midbrain()
def test1(self):
self.midbrain.limitSailServo()
## sail setting!
def setSails(self):
# '''Sets the sails to the correct location for the wind data coming in. '''
currentWindHeading = self.midbrain.readWindDirecrion()
currentHeading = self.midbrain.readHeading()
print 'current wind heading is: ', currentWindHeading
print 'Current bot heading is: ', currentHeading
windAngle = math.fabs(currentWindHeading - currentHeading)
if windAngle > 180:
windAngle = math.fabs(windAngle - 360)
print 'calculated wind angle is:', windAngle
if windAngle < 0:
print 'Error: negative wind angle!'
sailDesired = 40
elif windAngle < 45:
print 'Bot is in irons!'
sailDesired = 40
#outOfIrons()
if (windAngle < 180) & (windAngle > 45):
sailDesired = (.75 * windAngle) - 30
if windAngle > 180:
sailDesired = 90
print 'Error: wind angle over 180 degrees'
print 'Desired sail angle is: ', sailDesired
return sailDesired
## Going to ppoints!
def slope(self, p2, p1):
# '''gives the slope of the line to be followed
#tested to work 12:45 PM, 6/11/14 '''
m = float(p2[1] - p1[1]) / float(p2[0] - p1[0])
#print 'Slope is: ' + str(m)
return m
def headingToPoint(self, posDesired):
#'''gives the heading (angle in degrees from the +x axis) of the line to be followed'''
posCurrent = self.midbrain.readPosition() #robot's current position
m = self.slope(posDesired, posCurrent)
angle = math.degrees(math.atan(m / 1))
xcheck = (posDesired[0] - posCurrent[0]) > 0 #if true, in I or IV
ycheck = (posDesired[1] - posCurrent[1]) > 0 #if true, in I or II
#print 'x check: ' +str(xcheck) + '. y check:' +str(ycheck)+'.'
if (ycheck == True) & (xcheck == False): #quadrent 2
angle = 90 + math.fabs(angle)
#print 'Boat should be traveling into quadrent 2!'
if (xcheck == True) & (ycheck == False): #quadrent 4
angle = 270 + math.fabs(angle)
#print 'Boat should be heading into quadrent 4!'
if (xcheck == False) & (ycheck == False): #quadrent 3
angle = 180 + math.fabs(angle)
#print 'Boat should be heading into quadrent 3!'
#if quadrent 1, the angle doesn't need to be parsed.
return angle
def goToPoint(self, pt):
# '''outputs the necessary, global heading in order to go toward a point'''
currentHeading = self.midbrain.readHeading()
headingtoPoint = self.headingToPoint(pt)
print 'currentHeading' + str(currentHeading)
print 'headingtoPoint' + str(headingtoPoint)
desiredHeading = float(currentHeading + headingtoPoint) / 2
print 'the desire heading is:' + str(desiredHeading)
return desiredHeading
## Following lines!
#def slope(self,posDesired,linstart):
# '''gives the slope of the line to be followed
#tested to work 12:45 PM, 6/11/14'''
# m=float(posDesired[1]-linstart[1])/float(posDesired[0]-linstart[0])
#print 'Slope is: ' + str(m)
# return m
def heading_line(self, linstart, posDesired):
#'''gives the heading (angle in degrees from the +x axis) of the line to be followed'''
m = self.slope(posDesired, linstart)
angle = math.degrees(math.atan(m / 1))
xcheck = (posDesired[0] - linstart[0]) > 0 #if true, in I or IV
ycheck = (posDesired[1] - linstart[1]) > 0 #if true, in I or II
#print 'x check: ' +str(xcheck) + '. y check:' +str(ycheck)+'.'
if (ycheck == True) & (xcheck == False): #quadrent 2
angle = 90 + math.fabs(angle)
#print 'Boat should be traveling into quadrent 2!'
if (xcheck == True) & (ycheck == False): #quadrent 4
angle = 270 + math.fabs(angle)
#print 'Boat should be heading into quadrent 4!'
if (xcheck == False) & (ycheck == False): #quadrent 3
angle = 180 + math.fabs(angle)
#print 'Boat should be heading into quadrent 3!'
#if quadrent 1, the angle doesn't need to be parsed.
return angle
def above_below_on_line(self, posCurrent, linstart, posDesired):
# '''gives whether the bot is above, below, or on the line it should be following.
#tested to work at 13:14 on 6/11/14
# Based on: http://math.stackexchange.com/questions/324589/detecting-whether-a-point-is-above-or-below-a-slope'''
posCurrent = self.midbrain.readPosition()
m = self.slope(posDesired, linstart)
b = posDesired[1] - (m * posDesired[0])
check = m * posCurrent[0] + b
if check < posCurrent[1]:
print 'Bot is above the line'
return 'above'
if check > posCurrent[1]:
print 'Bot is below the line'
return 'below'
if check == posCurrent[1]:
print 'Bot is on the line!'
return 'on'
def followLine(self, linstart, posDesired):
# '''Outputs the necessary, global heading for the robot to follow a line with specified endpoints'''
posCurrent = self.midbrain.readPosition()
bot_posVlin = self.above_below_on_line(posCurrent, linstart,
posDesired)
lineheading = self.heading_line(linstart, posDesired)
botheading = self.midbrain.readHeading()
print 'Line heading: ' + str(lineheading)
print 'Bot heading: ' + str(botheading)
print 'bot_posVlin', bot_posVlin
#check through possible cases and assign the correct, desired, global heading accordingly
if (bot_posVlin
== 'above') & (botheading > lineheading
): #above line and heading away from it.
print 'Bot above and heading away from line to be followed'
headDesired = ((lineheading - botheading) / 2) + lineheading
elif (bot_posVlin == 'below') & (
botheading > lineheading): #below line and heading toward it.
print 'Bot below and heading away from line to be followed'
headDesired = ((lineheading - botheading) / 2) + lineheading
elif (bot_posVlin
== 'below') & (botheading < lineheading
): #below line and heading away from it.
print 'Bot below and heading toward from line to be followed'
headDesired = lineheading - math.fabs(
(lineheading - botheading) / 2)
elif (bot_posVlin == 'above') & (
botheading < lineheading): #above line and heading toward it.
print 'Bot above and heading toward from line to be followed'
headDesired = lineheading - math.fabs(
(lineheading - botheading) / 2)
elif bot_posVlin == 'on': #on line! :)
print 'Bot is on the line to be followed!'
headDesired = lineheading
print 'The desired heading for the bot is:' + str(headDesired)
return headDesired
## Maintaining heading!
def mtnHeading(self, desiredHeading):
# '''Keeps the robot on a desired heading. Output: desired, global heading'''
#currentHeading = self.midbrain.readHeading()
currentHeading = 45
###TODO: uncomment when the hindbrain compas sensors come in and work
print 'Current heading: ' + str(currentHeading)
print 'Desired heading:' + str(desiredHeading)
delta = abs((currentHeading - desiredHeading) / 2)
print 'Delta: ' + str(delta)
if currentHeading > desiredHeading:
desiredStearing = currentHeading - delta
print 'desired greater than current'
elif currentHeading < desiredHeading:
desiredStearing = currentHeading + delta
print 'current greater than desired'
elif currentHeading == desiredHeading:
desiredStearing = currentHeading
print 'current heading is the desired heading'
print 'the desired stearing angle is: ' + str(desiredStearing)
return desiredStearing
## Avoid obstacles!
def obsAvoid(self):
# '''works with IR sensor to avoid obstacles. Essentially integral control.'''
dist_to_object = self.midbrain.readIR1(
) #distance to object from PIR/Sharp sensor
if dist_to_object > 50:
print "Servo moving necessary so I don't hit a rock!"
stearingDesired = 30 * dist_to_object / 256
sailDesired = self.setSails()
print 'desired stearing is: ' + str(stearingDesired) + ' degrees'
print 'desired sail is: ' + str(sailDesired) + ' degrees'
return (stearingDesired, sailDesired)
def readPosition(self):
'''reads the current GPS position off the robot via the hindbrain's method'''
return self.midbrain.readPosition()
class Forebrain: def __init__(self): self.midbrain=Midbrain() def test1(self): self.midbrain.limitSailServo() ## sail setting! def setSails(self): # '''Sets the sails to the correct location for the wind data coming in. ''' currentWindHeading = self.midbrain.readWindDirecrion() currentHeading = self.midbrain.readHeading() print 'current wind heading is: ', currentWindHeading print 'Current bot heading is: ', currentHeading windAngle=math.fabs(currentWindHeading-currentHeading) if windAngle>180: windAngle=math.fabs(windAngle-360) print 'calculated wind angle is:', windAngle if windAngle<0: print 'Error: negative wind angle!' sailDesired=40 elif windAngle<45: print 'Bot is in irons!' sailDesired=40 #outOfIrons() if (windAngle<180) & (windAngle>45): sailDesired=(.75*windAngle)-30 if windAngle>180: sailDesired=90 print 'Error: wind angle over 180 degrees' print 'Desired sail angle is: ', sailDesired return sailDesired ## Going to ppoints! def slope(self,p2,p1): # '''gives the slope of the line to be followed #tested to work 12:45 PM, 6/11/14 ''' m=float(p2[1]-p1[1])/float(p2[0]-p1[0]) #print 'Slope is: ' + str(m) return m def headingToPoint(self,posDesired): #'''gives the heading (angle in degrees from the +x axis) of the line to be followed''' posCurrent = self.midbrain.readPosition() #robot's current position m=self.slope(posDesired,posCurrent) angle=math.degrees(math.atan(m/1)) xcheck=(posDesired[0]-posCurrent[0])>0#if true, in I or IV ycheck=(posDesired[1]-posCurrent[1])>0#if true, in I or II #print 'x check: ' +str(xcheck) + '. y check:' +str(ycheck)+'.' if (ycheck==True) & (xcheck==False): #quadrent 2 angle=90+math.fabs(angle) #print 'Boat should be traveling into quadrent 2!' if (xcheck==True) & (ycheck==False): #quadrent 4 angle=270+math.fabs(angle) #print 'Boat should be heading into quadrent 4!' if (xcheck==False) & (ycheck==False): #quadrent 3 angle =180+math.fabs(angle) #print 'Boat should be heading into quadrent 3!' #if quadrent 1, the angle doesn't need to be parsed. return angle def goToPoint(self,pt): # '''outputs the necessary, global heading in order to go toward a point''' currentHeading = self.midbrain.readHeading() headingtoPoint=self.headingToPoint(pt) print 'currentHeading' +str(currentHeading) print 'headingtoPoint' +str(headingtoPoint) desiredHeading=float(currentHeading+headingtoPoint)/2 print 'the desire heading is:'+str(desiredHeading) return desiredHeading ## Following lines! #def slope(self,posDesired,linstart): # '''gives the slope of the line to be followed #tested to work 12:45 PM, 6/11/14''' # m=float(posDesired[1]-linstart[1])/float(posDesired[0]-linstart[0]) #print 'Slope is: ' + str(m) # return m def heading_line(self,linstart,posDesired): #'''gives the heading (angle in degrees from the +x axis) of the line to be followed''' m=self.slope(posDesired,linstart) angle=math.degrees(math.atan(m/1)) xcheck=(posDesired[0]-linstart[0])>0#if true, in I or IV ycheck=(posDesired[1]-linstart[1])>0#if true, in I or II #print 'x check: ' +str(xcheck) + '. y check:' +str(ycheck)+'.' if (ycheck==True) & (xcheck==False): #quadrent 2 angle=90+math.fabs(angle) #print 'Boat should be traveling into quadrent 2!' if (xcheck==True) & (ycheck==False): #quadrent 4 angle=270+math.fabs(angle) #print 'Boat should be heading into quadrent 4!' if (xcheck==False) & (ycheck==False): #quadrent 3 angle =180+math.fabs(angle) #print 'Boat should be heading into quadrent 3!' #if quadrent 1, the angle doesn't need to be parsed. return angle def above_below_on_line(self,posCurrent,linstart,posDesired): # '''gives whether the bot is above, below, or on the line it should be following. #tested to work at 13:14 on 6/11/14 # Based on: http://math.stackexchange.com/questions/324589/detecting-whether-a-point-is-above-or-below-a-slope''' posCurrent = self.midbrain.readPosition() m=self.slope(posDesired, linstart) b=posDesired[1]-(m*posDesired[0]) check=m*posCurrent[0]+b if check<posCurrent[1]: print 'Bot is above the line' return 'above' if check>posCurrent[1]: print 'Bot is below the line' return 'below' if check == posCurrent[1]: print 'Bot is on the line!' return 'on' def followLine(self,linstart,posDesired): # '''Outputs the necessary, global heading for the robot to follow a line with specified endpoints''' posCurrent=self.midbrain.readPosition() bot_posVlin=self.above_below_on_line(posCurrent,linstart,posDesired) lineheading=self.heading_line(linstart,posDesired) botheading = self.midbrain.readHeading() print 'Line heading: '+str(lineheading) print 'Bot heading: '+str(botheading) print 'bot_posVlin', bot_posVlin #check through possible cases and assign the correct, desired, global heading accordingly if (bot_posVlin=='above') & (botheading>lineheading): #above line and heading away from it. print 'Bot above and heading away from line to be followed' headDesired=((lineheading-botheading)/2)+lineheading elif (bot_posVlin=='below') & (botheading>lineheading): #below line and heading toward it. print 'Bot below and heading away from line to be followed' headDesired=((lineheading-botheading)/2)+lineheading elif (bot_posVlin=='below') & (botheading<lineheading): #below line and heading away from it. print 'Bot below and heading toward from line to be followed' headDesired=lineheading-math.fabs((lineheading-botheading)/2) elif (bot_posVlin=='above') & (botheading<lineheading): #above line and heading toward it. print 'Bot above and heading toward from line to be followed' headDesired=lineheading-math.fabs((lineheading-botheading)/2) elif bot_posVlin=='on': #on line! :) print 'Bot is on the line to be followed!' headDesired=lineheading print 'The desired heading for the bot is:' + str(headDesired) return headDesired ## Maintaining heading! def mtnHeading(self, desiredHeading): # '''Keeps the robot on a desired heading. Output: desired, global heading''' #currentHeading = self.midbrain.readHeading() currentHeading=45; ###TODO: uncomment when the hindbrain compas sensors come in and work print 'Current heading: ' + str(currentHeading) print 'Desired heading:' +str(desiredHeading) delta=abs((currentHeading-desiredHeading)/2) print 'Delta: '+ str(delta) if currentHeading > desiredHeading: desiredStearing=currentHeading-delta print 'desired greater than current' elif currentHeading<desiredHeading: desiredStearing=currentHeading+delta print 'current greater than desired' elif currentHeading==desiredHeading: desiredStearing=currentHeading print 'current heading is the desired heading' print 'the desired stearing angle is: ' + str(desiredStearing) return desiredStearing ## Avoid obstacles! def obsAvoid(self): # '''works with IR sensor to avoid obstacles. Essentially integral control.''' dist_to_object=self.midbrain.readIR1() #distance to object from PIR/Sharp sensor if dist_to_object >50: print "Servo moving necessary so I don't hit a rock!" stearingDesired = 30*dist_to_object/256; sailDesired=self.setSails() print 'desired stearing is: ' + str(stearingDesired) + ' degrees' print 'desired sail is: ' + str(sailDesired) + ' degrees' return (stearingDesired, sailDesired) def readPosition(self): '''reads the current GPS position off the robot via the hindbrain's method''' return self.midbrain.readPosition()
