def __init__(self):

self.w = wheels.wheels()

self.s = Sensors()

self.config = configuration.config()

self.w.setConfig(self.config)

self.s.setConfig(self.config)

self.s.calibration = True

def point(self,goal):

heading = self.s.getHeading()

power = 0

newPower = 0

if heading > goal:

self.w.setSpeed(0, wheels.Direction.Right)

else:

self.w.setSpeed(0,wheels.Direction.Left)

delta = abs(heading - goal)

while delta>2:

if delta > 90:

power = int(self.minPower * 1.5)

sec = .25

elif delta > 30:

power = int(self.minPower * 1.25)

sec = .1

else:

sec = .05

power = self.minPower

self.w.pulse(seconds=sec, bump=power)

time.sleep(.05)

heading = self.s.getHeading()

delta = abs(heading - goal)

def calibrateCompassNWheels(self):

#First read seems wonky so this is a throw away

north = self.s.getReadings()

time.sleep(.5)

#Actual first reading, also used for north

north = self.s.getReadings()

#Find minimum spin level - > 10 x at .1 second rate

x0 = north[0]

x1 = x0

self.minPower = 0

bump = 5

self.w.setSpeed(0,wheels.Direction.Right)

while abs(x1 - x0) < 10:

x0 = x1

self.minPower += bump

self.w.pulse(seconds = .1, bump = self.minPower)

time.sleep(.1)

x1 = self.s.getReadings()[0]

print("Minimum level: {}".format(self.minPower))

self.w.setSpeed(self.minPower,wheels.Direction.Right)

for i in range(200):

readings = self.s.getReadings()

time.sleep(.05)

print("{}\t{}\t{}\t{}".format(self.s.minx, self.s.maxx,self.s.miny,self.s.maxy))

self.s.setNorth(north[0],north[1])

self.w.setSpeed(0,wheels.Direction.Left)