Yue playground

external/controller/external_controller.gpr

@@ -4,8 +4,10 @@ project External_Controller is
                         "src/tcp_client/**",
                         "src/backend_thread/**",
                         "src/webots_thread/**",
+                        "src/path/**",
                         "src/knowledge/**",
                         "src/vendor/**");
+
    for Object_Dir use "obj";
    for Main use ("main.adb");
    for Exec_Dir use ".";

external/controller/src/.#main.adb#

@@ -0,0 +1,121 @@
+with Ada.Text_IO; use Ada.Text_IO;
+
+
+with Tcp_Client;
+with Backend_Thread;
+with Webots_Thread;
+with Types; use Types;
+with Mailbox;
+with Messages; use Messages;
+with Byte_Buffer;
+with Path_Following;
+with collision_detection;
+
+procedure Main is
+
+   task webots_task;
+   task backend_task;
+
+   task body webots_task is
+   begin
+      Webots_Thread.Main;
+   end webots_task;
+
+   task body backend_task is
+   begin
+      Backend_Thread.Main;
+   end backend_task;
+
+
+   Current_Mail : Mailbox.Mail;
+   alternator : Types.Uint8 := 1;
+
+   DS_Data : Messages.Distance_Sensor_Array := (others => 1000.0);
+   LS_Data : Messages.Light_Sensor_Array  := (others => 0.0);
+   V : Messages.Velocity_Message;
+   V_old : Messages.Velocity_Message;
+   -- D : Messages.Distance_Sensor_   Out_Buffer : Byte_Buffer.Buffer;
+
+   --current_packet : types.Communication_Packet;
+   --send_packet_path_following : types.Communication_Packet;
+   --send_packet_collision_avoidance : types.Communication_Packet;
+   --dist: types.Octets_8;
+  -- distance_sensor_data: collision_detection.Dtype;
+   is_object_collision: Boolean := False;
+begin
+
+   V_old := Messages.Velocity_Message_Create (0.0, 0.0);
+
+   -- threads have started here
+   while true loop
+
+      -- clear out both mailboxes
+      Backend_Thread.Backend_Mailbox.Clear;
+      Webots_Thread.Webots_Mailbox.Clear;
+
+      -- alternate between checking webots and backend mailbox first, then update alternator
+      Mailbox.check_mailbox (Backend_Thread.Backend_Mailbox, Webots_Thread.Webots_Mailbox, Current_Mail, alternator);
+      Mailbox.update_alternator (alternator);
+
+      -- Put_Line (Current_Mail.Message.Id'Image);
+
+      -- do calculations with current packet
+--  <<<<<<< HEAD
+--        --Ada.Text_IO.Put_Line(Integer'Image(Integer(current_packet.package_ID)));
+--
+--        send_packet_path_following.payload_length := 0;
+--        -- Path following
+--       if current_packet.package_ID = 67 then
+--           send_packet_path_following := pathfollowing.path_following(current_packet);
+--       end if;
+--
+--        -- Object collision
+--        if current_packet.package_ID = 66 then
+--           for J in uint32 range 0..8 loop
+--              for I in uint32 range 0..7 loop
+--                 dist(I) := current_packet.local_payload(I+J*8);
+--              end loop;
+--              distance_sensor_data(Integer(J)) := Types.uint64_to_float64(octets_to_uint64(dist));
+--           end loop;
+--           send_packet_collision_avoidance := detect(distance_sensor_data);
+--        end if;
+--        if send_packet_collision_avoidance.payload_length = 0 then
+--           if send_packet_path_following.payload_length /= 0 then
+--              send_bytes(Webots_Channel, send_packet_path_following);
+--           end if;
+--        else
+--           send_bytes(Webots_Channel, send_packet_collision_avoidance);
+--  =======
+      if Current_Mail.Message.Id = Messages.EXTERNAL_IMAGE_DATA then
+         V := Path_Following.Main (Messages.ID_Message_Ptr (Current_Mail.Message), DS_Data);
+         --Out_Buffer.Write_Message (V);
+         --if (V_old.Left_Speed = V.Left_Speed) and (V_old.Right_Speed = V.Right_Speed) then
+            --if alternator = 1 then
+             --  V.Left_Speed := V.Left_Speed + 0.1;
+            --   V.Right_Speed := V.Right_Speed + 0.1;
+           -- else
+             --  V.Left_Speed := V.Left_Speed - 0.1;
+              -- V.Right_Speed := V.Right_Speed - 0.1;
+         --   end if;
+        -- end if;
+        -- V_old := V;
+         declare Out_Buffer : Byte_Buffer.Buffer;
+            begin
+            Out_Buffer.Write_Message(V);
+            Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer);
+            end;
+      elsif Current_Mail.Message.Id = Messages.EXTERNAL_DISTANCE_SENSOR then
+         DS_Data := Messages.DS_Message_Ptr (Current_Mail.Message).Payload;
+      --elsif Current_Mail.Message.Id = Messages.EXTERNAL_LIGHT_SENSOR then
+         --LS_Data := Messages.LS_Message_Ptr (Current_Mail.Message).Payload;
+         --Put_Line (LS_Data(0)'Image);
+      end if;
+
+      --  Out_Buffer.Write_Message (V);
+      --  Byte_Buffer.Buffer'Write (Webots_Thread.Webots_Channel, Out_Buffer);
+      --  Out_Buffer.Delete_Bytes (V.Size);
+
+
+   end loop;
+
+end Main;

external/controller/src/Path/.#pathfollowing.adb#

@@ -0,0 +1,213 @@
+package body pathfollowing is
+   colour : Integer := 0;
+   r : Integer := 0;
+   g : Integer := 0;
+   b : Integer := 0;
+
+   bottomPoint : Integer := 0;
+   topPoint : Integer := 0;
+   leftPoint : Integer := 0;
+   rightPoint : Integer:= 0;
+   bottomPoint1 : Integer := 0;
+   topPoint1 : Integer := 0;
+   leftPoint1 : Integer := 0;
+   rightPoint1 : Integer:= 0;
+   --steeringAngle : float64 := 0.0;
+
+   red : Colour_Matrix := (others => (others => 0));
+   blue : Colour_Matrix := (others => (others => 0));
+   green : Colour_Matrix := (others => (others => 0));
+   grey : Colour_Matrix := (others => (others => 0));
+   binaImage : Colour_Matrix := (others => (others => 0));
+
+   wheehlvelocity : Wheehl_velocity := (others => 0.0);
+   --axleTrack : float64 := 1.1;
+   basicVelocity : float64 :=4.0 ;
+   --ratio : float64 := 8.0;
+   V_turn : float64 := 0.0;
+   offset : Integer := 32;
+   Kp : float64 := 0.08;
+   Ki : float64 :=0.08;
+   Kd : float64 := 0.08;
+   Error : float64 := 0.0;
+   lastError : float64 := 0.0;
+   integral : float64 := 0.0;
+   derivative : float64 := 0.0;
+
+
+   function path_following (imageInput : in Communication_Packet) return Communication_Packet is
+      raw : access types.payload := imageInput.local_payload;
+      index : Image_Index := 0;
+      o8 : Octets_8;
+      u64 : uint64;
+   begin
+
+      for I in Row_Index loop
+         for J in Column_Index loop
+            index := Image_Index(4 * (Integer(I) * width + Integer(J)));
+            blue (I) (J) := raw (types.uint32(index) + 4);
+            green (I) (J) := raw (types.uint32(index + 1) + 4);
+            red (I) (J) := raw (types.uint32(index + 2) + 4);
+            colour := Integer(blue (I) (J))*114/1000 + Integer(green (I) (J)) *587/1000 + Integer(red (I) (J))*299/1000;
+            grey (I) (J) := uint8(colour);
+         end loop;
+      end loop;
+
+      --Binarized
+      for I in Row_Index loop
+         for J in Column_Index loop
+            if grey (I)(J) > 100 then
+               binaImage (I)(J) := 255;
+            else
+               binaImage (I)(j) := 0;
+            end if;
+            --Put(uint8'Image (binaImage (I) (J)));
+
+         end loop;
+         --Put_Line("");
+      end loop;
+
+   --for I in Row_Index loop
+      --for J in Column_Index loop
+         --Put(Value'Image (blue (I) (J)));
+         --Put(Value'Image (green (I) (J)));
+         --Put(Value'Image (red (I) (J)));
+         --Put(Value'Image (grey (I) (J)));
+         --Put_Line("");
+      --end loop;
+      --end loop;
+-- find bottompoint
+      for J in Column_Index loop
+         if binaImage(Row_Index(height-5))(J) = 255 then
+            bottomPoint := Integer(J);
+         end if;
+         --
+      end loop;
+      for J in reverse Column_Index loop
+         if binaImage(Row_Index(height-5))(J) = 255 then
+            bottomPoint1 := Integer(J);
+         end if;
+      end loop;
+      bottomPoint := (bottomPoint + bottomPoint1) / 2;
+      -- find toppoint
+      for J in Column_Index loop
+         if binaImage(0)(J) = 255 then
+            topPoint := Integer(J);
+         end if;
+      end loop;
+      for J in reverse Column_Index loop
+         if binaImage(Row_Index(0))(J) = 255 then
+            topPoint1 := Integer(J);
+         end if;
+      end loop;
+      topPoint := (topPoint + topPoint1) / 2;
+      -- find rightpoint
+      for I in Row_Index loop
+         if binaImage(I)(Column_Index(width-1)) = 255 then
+            rightPoint:= Integer(I);
+         end if;
+      end loop;
+      for I in reverse Row_Index loop
+         if binaImage(I)(Column_Index(width-1)) = 255 then
+            rightPoint1:= Integer(I);
+         end if;
+      end loop;
+      rightPoint := (rightPoint + rightPoint1) /2;
+
+      --find leftPoint
+      for I in Row_Index loop
+         if binaImage(I)(0) = 255 then
+            leftPoint:= Integer(I);
+         end if;
+      end loop;
+      for I in reverse Row_Index loop
+         if binaImage(I)(0) = 255 then
+            leftPoint1 := Integer(I);
+         end if;
+      end loop;
+      leftPoint := (leftPoint + leftPoint1)/2;
+
+      Put_Line(Integer'Image(bottomPoint) & Integer'Image(topPoint) & Integer'Image(rightPoint) & Integer'Image(leftPoint));
+      --calculate steering angle
+      -- top and bottom
+      --if bottomPoint /= 0 and topPoint /= 0 and rightPoint = 0 and leftPoint = 0 then
+         --if topPoint - bottomPoint > 0 then
+            --steeringAngle := float64((topPoint - bottomPoint) / height) ;
+         --elsif topPoint - bottomPoint < 0 then
+            --steeringAngle := -float64((topPoint - bottomPoint) / height);
+         --else steeringAngle := 0.0;
+         --end if;
+      --end if;
+      -- bottom and left
+      --if bottomPoint /= 0 and leftPoint/= 0 and topPoint = 0 and rightPoint = 0 then
+         --steeringAngle := - float64(bottomPoint / (height - leftPoint));
+      --end if;
+      -- top and right
+      --if topPoint /= 0 and rightPoint /= 0 and bottomPoint = 0 and leftPoint= 0 then
+         --steeringAngle := - float64((width - topPoint) / rightPoint);
+      --end if;
+      --top and left
+      --if topPoint /= 0 and leftPoint /=0 and bottomPoint = 0 and rightPoint = 0 then
+         --steeringAngle := float64(topPoint / leftPoint);
+      --end if;
+      -- bottom and right
+      --if bottomPoint/= 0 and rightPoint /= 0 and topPoint =0 and leftPoint = 0 then
+         --steeringAngle := float64((width-bottomPoint) / (height - leftPoint));
+         --end if;
+      --Error : = bottomPoint - offset;
+      --integral :  =  integral + Error;
+      --derivative  : =  Error -  lastError;
+      --V_turn := Kp * Error + Ki  * integral + Kd * derivative;
+
+
+
+      if bottomPoint >= 36 then
+         V_turn := 3.6;
+      elsif bottomPoint <= 28  and bottomPoint > 0 then
+         V_turn := -3.6;
+         else V_turn := 0.0;
+      end if;
+
+
+      Put_Line (V_turn'Image);
+      --turn right
+
+      if V_turn > 0.0 then
+         wheehlvelocity(0) := basicVelocity + V_turn;
+         wheehlvelocity(1) := basicVelocity - V_turn;
+      --turn left
+      elsif V_turn < 0.0 then
+         wheehlvelocity(0) := basicVelocity + V_turn;
+         wheehlvelocity(1) := basicVelocity - V_turn;
+      else
+         wheehlvelocity(0) := basicVelocity;
+         wheehlvelocity(1) := basicVelocity;
+      end if;
+
+      Put_Line (wheehlvelocity (0)'Image & ", " & wheehlvelocity (1)'Image);
+
+      declare packet : Communication_Packet;
+      begin
+         packet.package_ID := 129;
+         packet.payload_length := 16 + 5;
+         packet.local_payload := new payload (0 .. 15);
+
+         u64 := types.float64_to_uint64 (wheehlvelocity (0));
+         o8 := types.uint64_to_octets (u64);
+
+         for I in o8'Range loop
+            packet.local_payload (I) := o8 (I);
+         end loop;
+
+         u64 := types.float64_to_uint64 (wheehlvelocity (1));
+         o8 := types.uint64_to_octets (u64);
+
+         for I in o8'Range loop
+            packet.local_payload (I + 8) := o8 (I);
+         end loop;
+
+         return packet;
+      end;
+   end path_following;
+
+end pathfollowing;