Improve simple control algorithm and add drawing and mouse control

remote_control/simple_control.py

@@ -13,18 +13,27 @@ args = parser.parse_args()
 bot = args.bot
 meas = args.meas
 
+w = 640
+h = 480
 
 pygame.init()
-pygame.display.set_mode((640, 480))
+surf = pygame.display.set_mode((640, 480))
+bg = pygame.Color(0,0,0)
+botcol = pygame.Color(255, 0, 0)
+setcol = pygame.Color(0, 255, 0)
 
-meas_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+meas_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+try:
+    meas_socket.connect((meas, 42424)) # connect to robot
+except socket.error:
+    print("could not connect to measurement socket")
 rc_socket = socket.socket()
 try:
     rc_socket.connect((bot, 1234)) # connect to robot
 except socket.error:
     print("could not connect to bot socket")
 
-meas_socket.sendto(f"{args.id}\n".encode(), (meas, 42424))
+meas_socket.sendall(f"{args.id}\n".encode())
 
 class Bot(object):
     FAR, CLOSE, ANGLE, REACHED = range(4)
@@ -41,6 +50,10 @@ class Bot(object):
         self.pps = True
         self.pas = True
 
+        print()
+        print(f"GOING TO {x},{y}")
+        print()
+
         self.state = Bot.FAR
 
     def rotate(self, angle):
@@ -56,26 +69,58 @@ class Bot(object):
             dp = ((self.tx - x)**2 + (self.ty - y)**2)**0.5
             da = atan2(self.ty - y, self.tx - x) - angle
         
-        if da > pi:
-            da -= 2*pi
-        elif da < -pi:
-            da += 2*pi
-
         # Position state: Determine if the angle to the target flips over +-pi/2, i.e. going past the target now
         def pst(angle):
-            return abs(angle) > pi
+            while angle > pi:
+                angle -= 2*pi
+            while angle <-pi:
+                angle += 2*pi
+            return abs(angle) > pi/2
         # Angle state: When 'close' to target angle (absolute value <= pi/2) use the sign, else 0. When the angle flips over 0, the difference is +- 2
         def ast(angle):
+            while angle > pi:
+                angle -= 2*pi
+            while angle <-pi:
+                angle += 2*pi
             return 0 if abs(angle) >= pi/2 else 1 if angle >= 0 else -1
         # Move with speed and change towards angle
-        def control(speed, angle):
+        def control(speed, angle, backwards = True):
+            ca = cos(angle)
             speed = speed * cos(angle)
+
+            
+            # Move angle to [0..2*pi]
+            while angle < 0:
+                angle += 2*pi
+            while angle > 2*pi:
+                angle -= 2*pi
+
+            if backwards:
+                # If we can go backwards, angle should be pi or 0
+                if angle >= pi/2 and angle < 3*pi/2:
+                    # pi/2 .. 3/2 pi should move towards pi
+                    angle -= pi
+                    print(f"Go for pi, {angle}")
+                else:
+                    # Everything else should be 0. Make sure angles in 3/2 pi .. 2 pi don't lead to huge outputs.
+                    if angle >= 3*pi/2:
+                        angle -= 2*pi
+                    print(f"Go for 0, {angle}")
+            else:
+                # Have to go forward, just try to get angle to 0
+                # Correct angle to be [-pi..pi]
+                while angle > pi:
+                    angle -= 2*pi
+                    print(f"Always go for 0, {angle}")
             ul, ur = speed, speed
+            if speed == 0 and abs(angle) < 0.5:
+                angle = 0.5 if angle > 0 else -0.5 if angle < 0 else 0
             ul -= angle * 0.5
             ur += angle * 0.5
             vd = max(1, abs(ul)/umax, abs(ur)/umax)
             ul /= vd
             ur /= vd
+            print(f"Movement: {ul},{ur}")
             self.sock.send(f'({ul},{ur})\n'.encode())
             self.pps = pst(angle)
             self.pas = ast(angle)
@@ -83,20 +128,25 @@ class Bot(object):
         if self.state == Bot.FAR:
             if dp <= 0.3:
                 # Close to the target
-                self.state = Bot.NEAR
+                self.state = Bot.CLOSE
+                self.pps = pst(da)
             else:
                 # When far, just move towards the target
+                print(f"FAR  {umax}, {da}")
                 control(umax, da)
-        if self.state == Bot.NEAR:
+        if self.state == Bot.CLOSE:
-            if pst(da) != self.pps:
+            if pst(da) != self.pps and dp <= 0.1:
                 # The angle flips over +- pi/2, go for the angle setpoint
                 self.state = Bot.ANGLE
+                self.pas = 0
             else:
-                control(umax * dp / 0.3, da)
+                print(f"CLOSE {umax*dp/0.3} {da} {pst(da)} {self.pps}")
+                control(0.3+(umax*dp/0.3), da)
         if self.state == Bot.ANGLE:
             if self.ta == None:
                 # No angle setpoint, we're done
                 self.state = Bot.REACHED
+                bot.stop()
             else:
                 # Use angle setpoint for angle difference
                 da = self.ta - angle
@@ -106,18 +156,25 @@ class Bot(object):
                 if abs(ast(da) - self.pas) == 2:
                     # Done
                     self.state = Bot.REACHED
+                    bot.stop()
                 else:
                     # Don't move, just rotate
-                    control(0, da)
+                    print(f"ANGL {0} {da} {ast(da)} {self.pas}")
+                    control(0, da, False)
+
+meas_file = meas_socket.makefile('rw', encoding='utf8', newline='\n')
 
 bot = Bot(rc_socket)
 bot.move(0, 0, 0)
 
 running = True
 ml = 0.5
+
+scale = 220 / 0.5
+
 while running:
-    received = json.loads(str(meas_socket.recv(1024), "utf-8").strip())
+    received = json.loads(meas_file.readline().strip())
-    print("Received: {}".format(received))
+    received = json.loads(meas_file.readline().strip())
 
     # Extract position, angle
     x = received['x']
@@ -138,6 +195,22 @@ while running:
             if event.key == pygame.K_ESCAPE:
                 bot.stop()
                 running = False
+        elif event.type == pygame.MOUSEBUTTONDOWN:
+            x,y = pygame.mouse.get_pos()
+            bot.move((x-w/2)/scale, (y-h/2)/scale)
+
+    surf.fill(bg)
+    pygame.draw.circle(surf, botcol, (w/2 + scale * x, h/2 + scale * y), 10, 2)
+    pygame.draw.line(surf, botcol, (w/2 + scale * x, h/2 + scale * y), (w/2 + scale * x + 15 * cos(a), h/2 + scale * y + 15 * sin(a)))
+    
+    if bot.tx != None and bot.ty != None:
+        pygame.draw.circle(surf, setcol, (w/2 + scale * bot.tx, h/2 + scale * bot.ty), 10, 2)
+        if bot.ta != None:
+            pygame.draw.line(surf, setcol,
+                    (w/2 + scale * bot.tx, h/2 + scale * bot.ty),
+                    (w/2 + scale * bot.tx + 15 * cos(bot.ta), h/2 + scale * bot.ty + 15 * sin(bot.ta)))
+    pygame.display.flip()
+
     if running:
         bot.step(x, y, a)