Odometry now has less of an offset, still needs to be improved. Switched back to hard coded colors.

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

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)
 
 

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)

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

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.red = (40, 30, 30)
-        self.blue = None
+        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