184 lines
6.2 KiB
Python
184 lines
6.2 KiB
Python
#!/usr/bin/env python3
|
|
|
|
import ev3dev.ev3 as ev3
|
|
from wheel import Wheel
|
|
from sensor import Sensor
|
|
from odometry import Odometry
|
|
from planet import Direction
|
|
from time import sleep
|
|
|
|
|
|
def capat100(value):
|
|
if(value > 100):
|
|
return 100
|
|
elif(value < -100):
|
|
return -100
|
|
else:
|
|
return value
|
|
|
|
|
|
class Move:
|
|
def __init__(self):
|
|
try:
|
|
self._wheel_l = Wheel('outB')
|
|
self._wheel_r = Wheel('outC')
|
|
self._sensor = Sensor()
|
|
self._calibrate()
|
|
except OSError:
|
|
print("ERROR: Cannot find Motor/Sensor")
|
|
self.defaultspeed = 20
|
|
self._odometry = Odometry()
|
|
|
|
'''
|
|
determine maximum and minimum brightness of lines/white space
|
|
depending on environment lighting
|
|
'''
|
|
def _calibrate(self):
|
|
ev3.Sound.beep()
|
|
while(not self._sensor.bumperwaspressed):
|
|
self._sensor.checkbumper()
|
|
self._sensor.reset()
|
|
sleep(1)
|
|
self._sensor.calibrateBrightness()
|
|
sleep(5)
|
|
'''
|
|
ev3.Sound.beep()
|
|
while(not self._sensor.bumperwaspressed):
|
|
self._sensor.checkbumper()
|
|
self._sensor.reset()
|
|
sleep(1)
|
|
self._sensor.calibrateRGB(self._sensor.red)
|
|
sleep(2)
|
|
ev3.Sound.beep()
|
|
while(not self._sensor.bumperwaspressed):
|
|
self._sensor.checkbumper()
|
|
self._sensor.reset()
|
|
sleep(1)
|
|
self._sensor.calibrateRGB(self._sensor.blue)
|
|
sleep(5)
|
|
'''
|
|
'''
|
|
Function to correct errors should the robot wander astray
|
|
'''
|
|
|
|
def _aligntoedge(self):
|
|
self._wheel_l.speed_set(20)
|
|
self._wheel_r.speed_set(-20)
|
|
self._wheel_l.run()
|
|
self._wheel_r.run()
|
|
self._sensor.refresh()
|
|
while(self._sensor.getbrightness() > self._sensor.edgebrightness):
|
|
self._sensor.refresh()
|
|
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
|
|
self._odometry.angle_set(self._odometry.angle_get())
|
|
self._wheel_l.stop()
|
|
self._wheel_r.stop()
|
|
self._wheel_l.turnbyamount(20, 40)
|
|
self._wheel_r.turnbyamount(-20, 40)
|
|
sleep(1)
|
|
|
|
def setcurcir(self, curdir):
|
|
self.odometry.angle_set(curdir[1])
|
|
|
|
'''
|
|
uses odometry and the planet object to map edges that are connected to
|
|
current node.
|
|
|
|
Use either relative motor pos or timed driving to center on node.
|
|
Next turn around and detect edges through drops in _sensor.getbrightness()
|
|
'''
|
|
def getstationedges(self):
|
|
found_edges = []
|
|
self._odometry.reset()
|
|
while(True):
|
|
self._aligntoedge()
|
|
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
|
|
if(self._odometry.angle_get() in found_edges):
|
|
break
|
|
else:
|
|
found_edges.append(self._odometry.angle_get())
|
|
self._odometry.angle_set(self._odometry.angle_get())
|
|
self._wheel_l.turnbyamount(20, 40)
|
|
self._wheel_r.turnbyamount(-20, 40)
|
|
sleep(2)
|
|
return found_edges
|
|
|
|
def setcurdir(self, newdir):
|
|
self._odometry.angle_set(newdir[1])
|
|
|
|
def getstats(self):
|
|
stats = list(self._odometry.coordinates_get())
|
|
if(self._sensor.bumperwaspressed):
|
|
stats.append(-1)
|
|
else:
|
|
stats.append(1)
|
|
return stats
|
|
|
|
def turnto(self, direction):
|
|
turnval = self._odometry.dir2ticks(direction)
|
|
self._wheel_l.turnbyamount(turnval, 120)
|
|
self._wheel_r.turnbyamount(-turnval, 120)
|
|
sleep(5)
|
|
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
|
|
|
|
def reset(self):
|
|
self._odometry.reset()
|
|
self._sensor.reset()
|
|
|
|
def resetwheels(self):
|
|
self._wheel_l._motor.position = 0
|
|
self._wheel_r._motor.position = 0
|
|
|
|
def traversetonextstation(self, isknownstation):
|
|
self._wheel_l.speed_set(self._sensor.edgebrightness//2)
|
|
self._wheel_r.speed_set(self._sensor.edgebrightness//2)
|
|
self._wheel_l.run()
|
|
self._wheel_r.run()
|
|
while (not self._sensor.bumperwaspressed):
|
|
self._sensor.refresh()
|
|
self._sensor.checkbumper()
|
|
self._sensor.getcolor()
|
|
if(self._sensor.lastcolor is not None):
|
|
break
|
|
try:
|
|
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
|
|
self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
|
|
except OSError:
|
|
print("ERROR: Invalid motor speed value")
|
|
if(not isknownstation):
|
|
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
|
|
self._wheel_l.stop()
|
|
self._wheel_r.stop()
|
|
if(self._sensor.bumperwaspressed is True):
|
|
ev3.Sound.beep()
|
|
sleep(0.2)
|
|
ev3.Sound.beep()
|
|
sleep(0.2)
|
|
ev3.Sound.beep()
|
|
sleep(0.2)
|
|
ev3.Sound.beep()
|
|
self._wheel_l.turnbyamount(-200, 120)
|
|
self._wheel_r.turnbyamount(-200, 120)
|
|
sleep(1)
|
|
self._wheel_l.turnbyamount(320, 120)
|
|
self._wheel_r.turnbyamount(-320, 120)
|
|
sleep(2)
|
|
self._aligntoedge()
|
|
self._wheel_l.run()
|
|
self._wheel_r.run()
|
|
while(True):
|
|
self._sensor.refresh()
|
|
self._sensor.getcolor()
|
|
if(self._sensor.lastcolor is not None):
|
|
break
|
|
try:
|
|
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
|
|
self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
|
|
except OSError:
|
|
print("ERROR: Invalid motor speed value")
|
|
if(not isknownstation):
|
|
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
|
|
self._wheel_l.turnbyamount(200, 90)
|
|
self._wheel_r.turnbyamount(200, 90)
|
|
sleep(2)
|