Yue playground

external/controller/external_controller.gpr

@@ -1,6 +1,6 @@
 project External_Controller is
 
-   for Source_Dirs use ("src/**", "src/tcp_client/**", "src/backend_thread/**", "src/webots_thread/**");
+   for Source_Dirs use ("src/**", "src/tcp_client/**", "src/backend_thread/**", "src/webots_thread/**", "src/Path/**");
    for Object_Dir use "obj";
    for Main use ("main.adb");
    for Exec_Dir use ".";

external/controller/src/Path/pathfollowing.adb

@@ -0,0 +1,169 @@
+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;
+   white_Line : 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 binarize (grey : in Colour_Matrix) return Colour_Matrix is
+
+   begin
+
+       for I in Row_Index loop
+         for J in Column_Index loop
+            if grey (I)(J) > 150 then
+               binaImage (I)(J) := 255;
+            else
+               binaImage (I)(j) := 0;
+            end if;
+            --Put(uint8'Image (binaImage (I) (J)));
+
+         end loop;
+         --Put_Line("");
+      end loop;
+      return binaImage;
+
+   end binarize;
+
+   function findLine (binarizedImage : in Colour_Matrix) return Integer is
+
+   begin
+      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;
+      Put_Line(Integer'Image(bottomPoint));
+      return bottomPoint;
+
+   end findLine;
+
+
+   function wheel_Velocity (whiteLine : in Integer; d_sensor : in Dtype) return Wheehl_velocity is
+      wheehlvelocity : Wheehl_velocity := (others => 0.0);
+
+   begin
+
+
+      if whiteLine >= 36 then
+         V_turn := 3.6;
+      elsif whiteLine <= 28  and whiteLine > 0 then
+         V_turn := -3.6;
+      elsif whiteLine > 28 and whiteLine < 36 then
+         V_turn := 0.0;
+      elsif whiteLine = 0 then
+         if d_sensor(6) < 500.0 then
+            V_turn := 1.0;
+         end if;
+         if d_sensor(3) < 500.0 then
+            V_turn := -1.0;
+         end if;
+      end if;
+
+      Put_Line (d_sensor(3)'Image & ", " & d_sensor(6)'Image);
+      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);
+
+      return wheehlvelocity;
+
+   end wheel_Velocity;
+
+   function path_following (dataInput : in Communication_Packet; d_sensor : in Dtype) return Communication_Packet is
+      raw : access types.payload := dataInput.local_payload;
+      index : Image_Index := 0;
+      o8 : Octets_8;
+      u64 : uint64;
+   begin
+      --grayscale Image
+      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
+      binaImage := binarize(grey);
+      --findLine
+      white_Line := findLine(binaImage);
+      --wheelvelocity calculate
+      wheehlvelocity := wheel_Velocity(white_Line, d_sensor);
+
+      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;

external/controller/src/Path/pathfollowing.ads

@@ -0,0 +1,30 @@
+with Ada.Text_IO; use Ada.Text_IO;
+with Ada.Float_Text_Io; use Ada.Float_Text_Io;
+with types; use types;
+with mailbox;
+
+package pathfollowing is
+
+   width : constant Integer := 64;
+   height : constant Integer := 64;
+   size : constant Integer := width * height * 4;
+
+   type Pixel is array(0 .. 3) of uint8;
+   type Column_Index is range 0 .. width - 1;
+   type Row_Index is range 0 .. height - 1;
+
+   type Image_Index is range 0 .. size - 1;
+   type Image_Raw is array(Image_Index) of uint8;
+
+   type Colour_Column is array(Column_Index) of uint8;
+   type Colour_Matrix is array(Row_Index) of Colour_Column;
+   type Wheehl_velocity is array(0..1) of float64;
+   type Dtype is array(0..8) of float64;
+
+   function path_following(dataInput : in Communication_Packet; d_sensor : in Dtype) return Communication_Packet;
+   function binarize(grey : in Colour_Matrix) return Colour_Matrix;
+   function findLine(binarizedImage : in Colour_Matrix) return Integer;
+   function wheel_Velocity(whiteLine : in Integer; d_sensor : in Dtype) return Wheehl_velocity;
+
+
+end pathfollowing;

external/controller/src/main.adb

@@ -2,7 +2,9 @@ with Ada.Text_IO; use Ada.Text_IO;
 with tcp_client; use tcp_client;
 with backend_thread; use backend_thread;
 with webots_thread; use webots_thread;
+with pathfollowing; use pathfollowing;
 with types; use types;
+
 with mailbox;
 
 
@@ -23,7 +25,9 @@ procedure Main is
 
    current_packet : types.Communication_Packet;
    alternator : types.uint8 := 1;
-
+   send_packet : types.Communication_Packet;
+   dist: types.Octets_8;
+   distance_sensor_data: pathfollowing.Dtype;
 begin
 
    -- threads have started here
@@ -40,6 +44,21 @@ begin
 
       -- do calculations with current packet
       Ada.Text_IO.Put_Line(Integer'Image(Integer(current_packet.package_ID)));
+      --Ada.Text_IO.Put_Line(Integer'Image(Integer(current_packet.payload_length)));
+
+      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;
+      end if;
+
+      if(current_packet.package_ID = 67) then
+         send_packet := pathfollowing.path_following(current_packet, distance_sensor_data);
+         send_bytes(Webots_Channel, send_packet);
+      end if;
 
    end loop;
 

external/controller/src/tcp_client/mailbox.adb

@@ -36,14 +36,14 @@ package body mailbox is
          select
             first.Collect(new_packet);
          else
-            delay(0.05);
+            delay(0.0000005);
             check_mailbox(second,first,new_packet,alternator);
          end select;
       else
          select
             second.Collect(new_packet);
          else
-            delay(0.05);
+            delay(0.0000005);
             check_mailbox(second,first,new_packet,alternator);
          end select;
       end if;

external/controller/src/tcp_client/tcp_client.adb

@@ -78,7 +78,7 @@ package body tcp_client is
       --  read package_length
       bytes_received := recv_bytes(server_stream, protocol_package_length, dynamic_buffer);
       byte_buffer.read_uint32(dynamic_buffer, conv_package_value_length);
-
+      conv_package_value_length := conv_package_value_length - 5;
       --  read package_ID
       bytes_received := recv_bytes(server_stream, protocol_ID_length, dynamic_buffer);
       byte_buffer.read_uint8(dynamic_buffer, conv_package_ID);

external/controller/src/webots_thread/webots_thread.adb

@@ -1,14 +1,27 @@
 package body webots_thread is
 
    procedure webots_main is
+      test : float64 := 4.0;
+      test_uint64 : uint64;
+      test_to_send : Octets_8;
+
 
    begin
 
-      Webots_Channel := build_connection (Webots_Client, 2000, Webots_Address);
+      Webots_Channel := build_connection (Webots_Client, 5555, Webots_Address);
 
-      Webots_Cmd.package_ID := protocol_join_ID;
-      Webots_Cmd.payload_length := 0;
+      test_uint64 := float64_to_uint64(test);
+      test_to_send :=uint64_to_octets(test_uint64);
+      Webots_Cmd.package_ID := 129;
+      Webots_Cmd.payload_length := 21;
       --Backend_Cmd.local_payload := new types.payload(0..1);
+      Webots_Cmd.local_payload := new types.payload(0..15);
+
+
+      for I in Webots_Cmd.local_payload'Range loop
+         Webots_Cmd.local_payload(I) := test_to_send(I mod 8);
+      end loop;
+
       send_bytes(Webots_Channel, Webots_Cmd);
 
       while true loop

external/controller/src/webots_thread/webots_thread.ads

@@ -2,7 +2,7 @@ with GNAT.Sockets; use GNAT.Sockets;
 with Ada.Streams; use Ada.Streams;
 with tcp_client; use tcp_client;
 with Ada.Text_IO;
-with types;
+with types;use types;
 with byte_buffer;
 with mailbox;
 

test/ada/Makefile

@@ -0,0 +1,5 @@
+all:
+	gprbuild -p -Pada
+clean:
+	-gprclean -Pada
+	rm -rf ada/obj/*

test/ada/ada.gpr

@@ -0,0 +1,26 @@
+limited with "../../../../GNAT/share/gpr/aunit.gpr";
+
+--with "aunit.gpr";
+
+project Ada is
+
+   for Source_Dirs use ("src/**", "../../external/controller/src/backend_thread/**", "../../external/controller/src/tcp_client/**", "../../external/controller/src/webots_thread/**", "src/mailbox_suite/**");
+   for Object_Dir use "obj";
+   for Main use ("test_main.adb");
+   for Exec_Dir use ".";
+
+   package Linker is
+      for Default_Switches ("ada") use ("-g");
+   end Linker;
+
+   package Binder is
+      for Default_Switches ("ada") use ("-E", "-static");
+   end Binder;
+
+   package Compiler is
+      for Default_Switches ("ada") use ("-gnatQ", "-gnatf", "-gnato", "-gnat2012");
+   end Compiler;
+
+end Ada;
+
+

test/ada/empty-loc.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<Locations >
+  <Project>
+    <vfs_file />
+    <file_marker column=" 1" line=" 7" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/controller/src/webots_thread/webots_thread.adb</vfs_file>
+    </file_marker>
+    <file_marker column=" 1" line=" 1" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/controller/src/webots_thread/webots_thread.ads</vfs_file>
+    </file_marker>
+    <file_marker column=" 6" line=" 5" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/controller/src/main.adb</vfs_file>
+    </file_marker>
+    <file_marker column=" 112" line=" 3" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/controller/external_controller.gpr</vfs_file>
+    </file_marker>
+    <file_marker column=" 53" line=" 127" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/controller/src/Path/pathfollowing.adb</vfs_file>
+    </file_marker>
+    <file_marker column=" 1" line=" 1" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/test/ada/ada.gpr</vfs_file>
+    </file_marker>
+    <file_marker column=" 1" line=" 1" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/external/harness/harness.gpr</vfs_file>
+    </file_marker>
+    <file_marker column=" 1" line=" 1" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/test/ada/ada.gpr</vfs_file>
+    </file_marker>
+    <file_marker column=" 1" line=" 23" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/test/ada/ada.gpr</vfs_file>
+    </file_marker>
+    <file_marker current="true" column=" 1" line=" 1" >
+      <vfs_project />
+      <vfs_file>/home/kingmoon/Documents/Github/autonomouscabs/test/ada/empty.gpr</vfs_file>
+    </file_marker>
+  </Project>
+</Locations>