diff --git a/src/communication.py b/src/communication.py index dcb2be4..694084a 100644 --- a/src/communication.py +++ b/src/communication.py @@ -102,6 +102,7 @@ class Communication: self._status = Mode.EXPLORE self.navto = [] self.isfirstrun = True + self.target = None def status(self): return self._status @@ -173,6 +174,7 @@ class Communication: elif message.startswith(Command.PATH): self.stringtopath(*message[5:].rsplit(' ')) elif message.startswith(Command.TARGET): + self.target = str2tuple(*message[7:].rsplit(',')) self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(','))) if(self.navto is not None): self._status = Mode.TARGET diff --git a/src/main.py b/src/main.py index 2f5d4f0..355eaff 100644 --- a/src/main.py +++ b/src/main.py @@ -28,24 +28,30 @@ def run(): move.resetwheels() move.reset() while(communication.status() is not Mode.COMPLETE): - communication.update(move.getstats()) # TODO: Register in map + communication.update(move.getstats()) # correct odometry right here otherwise list of edges will be incorrect move.setcurdir(communication.getdir()) communication.process_edges(move.getstationedges()) if(communication.status() is Mode.GOTOSTATION or communication.status() is Mode.TARGET): # should be triggered if current station has no unexplored edges while len(communication.navto) > 0: move.turnto(communication.navto[0][1]) + move.reset() + move.resetwheels() move.traversetonextstation(True) move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]]) communication.update(None) communication.navto.pop(0) - communication._status = Mode.EXPLORE + if(communication.status() == Mode.TARGET): + communication._status = Mode.COMPLETE + else: + communication._status = Mode.EXPLORE else: move.turnto(communication.navto[0][1]) sleep(4) communication.planet.setcurdir(communication.navto[0][1]) move.setcurdir(communication.getdir()) move.reset() + move.resetwheels() move.traversetonextstation(False) diff --git a/src/move.py b/src/move.py index 15c7821..1fa7156 100644 --- a/src/move.py +++ b/src/move.py @@ -40,7 +40,8 @@ class Move: self._sensor.reset() sleep(1) self._sensor.calibrateBrightness() - sleep(2) + sleep(5) + ''' ev3.Sound.beep() while(not self._sensor.bumperwaspressed): self._sensor.checkbumper() @@ -55,7 +56,7 @@ class Move: sleep(1) self._sensor.calibrateRGB(self._sensor.blue) sleep(5) - + ''' ''' Function to correct errors should the robot wander astray ''' @@ -68,6 +69,8 @@ class Move: 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) @@ -88,8 +91,6 @@ class Move: found_edges = [] self._odometry.reset() while(True): - self._wheel_l.turnbyamount(40, 60) - self._wheel_r.turnbyamount(-40, 60) self._aligntoedge() self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) if(self._odometry.angle_get() in found_edges): @@ -100,9 +101,6 @@ class Move: self._wheel_l.turnbyamount(20, 40) self._wheel_r.turnbyamount(-20, 40) sleep(2) - self._aligntoedge() - self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) - self._odometry.angle_set(self._odometry.angle_get()) return found_edges def setcurdir(self, newdir): @@ -149,6 +147,13 @@ class Move: 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) @@ -167,7 +172,6 @@ class Move: self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed) 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) diff --git a/src/odometry.py b/src/odometry.py index b36a5c5..57e1b37 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -80,4 +80,4 @@ class Odometry: self._posxy[1] = self._posxy[1] + self._wheel_center * math.cos(self._v_angle) def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle) - return (destdir - (math.degrees(self._v_angle)%360)) * 2 + return (destdir - (math.degrees(self._v_angle) % 360)) * 2 diff --git a/src/sensor.py b/src/sensor.py index 0c4ef2e..b53b466 100644 --- a/src/sensor.py +++ b/src/sensor.py @@ -42,8 +42,8 @@ class Sensor: print("Error while setting up sensors. Please make sure that everything is plugged in properly.") self.lastcolor = None self.edgebrightness = None - self.red = None - self.blue = None + self.red = (40, 30, 30) + self.blue = (40, 40, 70) def refresh(self): self.RGB = self._camera.bin_data("hhh") @@ -66,16 +66,19 @@ class Sensor: def getcolor(self): if(self._camera.mode == 'RGB-RAW'): - if(self.RGB[0] > self.red[0] and (self.RGB[1], self.RGB[2]) < (self.red[1], self.red[2])): + if(self.RGB[0] > self.red[0] and self.RGB[1] < self.red[1] and self.RGB[2] < self.red[2]): + print("red: ", self.RGB) self.lastcolor = self.red - if(self.RGB[0] < self.blue[0] and (self.RGB[1], self.RGB[2]) > (self.blue[1], self.blue[2])): + if(self.RGB[0] < self.blue[0] and self.RGB[1] > self.blue[1] and self.RGB[2] > self.blue[2]): + print("blue ", self.RGB) self.lastcolor = self.blue def calibrateRGB(self, color): self.refresh() + print("Color:", self.RGB) if (color is self.red): - newRED = (self.RGB[0]-5, self.RGB[1]+5, self.RGB[2]+5) + newRED = (self.RGB[0]-20, self.RGB[1]+15, self.RGB[2]+15) self.red = newRED elif (color is self.blue): - newBLUE = (self.RGB[0]+5, self.RGB[1]-5, self.RGB[2]-5) + newBLUE = (self.RGB[0]+10, self.RGB[1]-10, self.RGB[2]) self.blue = newBLUE