2018-03-18 14:23:17 +01:00
|
|
|
#!/usr/bin/env python3
|
|
|
|
|
|
|
|
from wheel import Wheel
|
|
|
|
from sensor import Sensor
|
2018-03-23 19:09:49 +01:00
|
|
|
from odometry import Odometry
|
2018-03-18 14:23:17 +01:00
|
|
|
|
|
|
|
|
|
|
|
class Move:
|
|
|
|
def __init__(self, planet):
|
2018-03-22 02:14:44 +01:00
|
|
|
try:
|
|
|
|
self._wheel_l = Wheel('outB')
|
|
|
|
self._wheel_r = Wheel('outC')
|
2018-03-22 14:34:55 +01:00
|
|
|
self._sensor = Sensor()
|
|
|
|
except OSError:
|
|
|
|
print("ERROR: Cannot find Motor/Sensor")
|
2018-03-23 19:09:49 +01:00
|
|
|
self.maxbrightness = 0
|
|
|
|
self.minbrightness = 0
|
|
|
|
self.edgebrightness = 0
|
|
|
|
self._odometry = Odometry()
|
|
|
|
|
2018-03-19 00:44:57 +01:00
|
|
|
'''
|
|
|
|
determine maximum and minimum brightness of lines/white space
|
|
|
|
depending on environment lighting
|
|
|
|
'''
|
2018-03-18 14:23:17 +01:00
|
|
|
def _calibrate(self):
|
2018-03-22 17:40:19 +01:00
|
|
|
self._sensor.calibrateRGB()
|
|
|
|
self.calibrateBrightness()
|
|
|
|
# pass
|
2018-03-18 14:23:17 +01:00
|
|
|
|
2018-03-22 17:40:19 +01:00
|
|
|
def calibrateBrightness(self):
|
|
|
|
i = 0
|
2018-03-23 19:09:49 +01:00
|
|
|
self.maxbrightness = self._camera.getbrightness()
|
|
|
|
self.minbrightness = self._camera.getbrightness()
|
|
|
|
self._wheel_l.speed_set(15)
|
|
|
|
self._wheel_r.speed_set(-15)
|
2018-03-22 17:40:19 +01:00
|
|
|
self._wheel_r.run()
|
|
|
|
self._wheel_l.run()
|
2018-03-23 19:09:49 +01:00
|
|
|
tempvalue = 0
|
2018-03-22 17:40:19 +01:00
|
|
|
while(i < 500):
|
|
|
|
self._camera.refresh()
|
|
|
|
tempvalue = self._camera.getbrightness()
|
|
|
|
if(tempvalue > self.maxbrightness):
|
|
|
|
self.maxbrightness = tempvalue
|
|
|
|
if(tempvalue < self.minbrightness):
|
|
|
|
self.minbrightness = tempvalue
|
|
|
|
i = i + 1
|
|
|
|
self._wheel_l.stop()
|
|
|
|
self._wheel_r.stop()
|
2018-03-23 19:09:49 +01:00
|
|
|
self.edgebrightness = (self.maxbrightness + self.minbrightness)//2
|
|
|
|
while(not (self._camera.getbrightness() == self.edgebrightness)):
|
|
|
|
self._wheel_l.speed_set(15)
|
|
|
|
self._wheel_r.speed_set(-15)
|
2018-03-22 17:40:19 +01:00
|
|
|
self._wheel_l.stop()
|
|
|
|
self._wheel_r.stop()
|
2018-03-22 14:34:55 +01:00
|
|
|
|
2018-03-19 00:44:57 +01:00
|
|
|
'''
|
|
|
|
Function to correct errors should the robot wander astray
|
|
|
|
'''
|
2018-03-21 14:54:24 +01:00
|
|
|
|
2018-03-18 14:23:17 +01:00
|
|
|
def _aligntoedge(self):
|
|
|
|
pass
|
2018-03-19 00:03:35 +01:00
|
|
|
|
2018-03-19 00:44:57 +01:00
|
|
|
'''
|
|
|
|
uses odometry and the planet object to map edges that are connected to
|
|
|
|
current node.
|
2018-03-19 02:46:25 +01:00
|
|
|
|
|
|
|
Use either relative motor pos or timed driving to center on node.
|
|
|
|
Next turn around and detect edges through drops in _sensor.getbrightness()
|
2018-03-19 00:44:57 +01:00
|
|
|
'''
|
2018-03-19 00:03:35 +01:00
|
|
|
def getstationedges(self):
|
2018-03-19 10:14:42 +01:00
|
|
|
self._wheel_l.turnbyamount(500, 250)
|
|
|
|
self._wheel_r.turnbyamount(-500, 250)
|
2018-03-19 00:03:35 +01:00
|
|
|
|
|
|
|
def traversetonextstation(self, isknownstation):
|
2018-03-23 19:09:49 +01:00
|
|
|
self._wheel_l.speed_set(self.edgebrightness//2)
|
|
|
|
self._wheel_r.speed_set(self.edgebrightness//2)
|
2018-03-19 00:03:35 +01:00
|
|
|
self._wheel_l.run()
|
|
|
|
self._wheel_r.run()
|
2018-03-23 19:09:49 +01:00
|
|
|
|
2018-03-22 14:34:55 +01:00
|
|
|
while (not self._sensor.bumperispressed() and self._sensor.lastcolor == None):
|
2018-03-22 07:09:30 +01:00
|
|
|
self._camera.refresh()
|
2018-03-22 14:34:55 +01:00
|
|
|
self._wheel_l.speed_set(self.maxbrightness//2 - self._sensor.getbrightness()//2)
|
|
|
|
self._wheel_r.speed_set(self._sensor.getbrightness()//2)
|
|
|
|
self._sensor.getcolor()
|
2018-03-22 17:40:19 +01:00
|
|
|
if(not isknownstation):
|
2018-03-23 19:09:49 +01:00
|
|
|
print(self._odometry.degree(self._wheel_l.getmovement(), self._wheel_r.getmovement(), 0))
|
2018-03-22 14:34:55 +01:00
|
|
|
|
2018-03-19 00:03:35 +01:00
|
|
|
self._wheel_l.stop()
|
|
|
|
self._wheel_r.stop()
|