diff --git a/src/main/motor_control/forward.sv b/src/main/motor_control/forward.sv
index 2f0d90f..1e257d6 100644
--- a/src/main/motor_control/forward.sv
+++ b/src/main/motor_control/forward.sv
@@ -102,6 +102,6 @@ module forward #(
     end
 
     // Ready signal when all bytes have been sent, including the newline
-    assign ready = (byte_index == NUM_BYTES) && uart_ready && (!uart_valid);  // Only ready after the last byte is fully sent
+    assign ready = (byte_index == NUM_BYTES-1) && uart_ready && (!uart_valid);  // Only ready after the last byte is fully sent
 
 endmodule
diff --git a/src/main/motor_control/json_command_sender_tb.sv b/src/main/tb/json_command_sender_tb.sv
similarity index 80%
rename from src/main/motor_control/json_command_sender_tb.sv
rename to src/main/tb/json_command_sender_tb.sv
index 88234d1..9a940f6 100644
--- a/src/main/motor_control/json_command_sender_tb.sv
+++ b/src/main/tb/json_command_sender_tb.sv
@@ -8,17 +8,19 @@ module json_command_sender_tb;
     // Clock and reset signals
     logic clk;
     logic rst;
+    logic [2:0] speed;
     logic uart_out;
     logic ready;
 
     // Instantiate the DUT (Device Under Test)
-    json_command_sender #(
+    forward #(
     .CLKS_PER_BIT(CLKS_PER_BIT),
     .BITS_N(BITS_N),
     .NUM_BYTES(25)
-    ) uut (
+    ) json_command (
         .clk(clk),
         .rst(rst),
+        .speed(speed),
         .uart_out(uart_out),
         .ready(ready)
     );
@@ -34,28 +36,22 @@ module json_command_sender_tb;
         rst = 1;
         #50;
         rst = 0;
-		  
+		
+        // Set Speed
+        speed = 3'b001;
+
         // End of simulation
         wait(ready) begin
-				#1000;
-				rst = 1;
-				#50;
-				rst = 0;
-        end
-		  
-		  wait (ready) begin
-				#50;
+			#50;
 				$display("Transmission complete.");
             $finish;
-		  end
+        end
     end
 	 
-	 
-
     // Monitor the UART output
     always @(posedge clk) begin
         if (!rst) begin
-            $display("Sending byte: %h at time %t", uut.current_byte, $time);
+            $display("Sending byte: %h at time %t", json_command.current_byte, $time);
         end
     end
 
diff --git a/src/main/tb/motor_integration_tb.sv b/src/main/tb/motor_integration_tb.sv
new file mode 100644
index 0000000..f316688
--- /dev/null
+++ b/src/main/tb/motor_integration_tb.sv
@@ -0,0 +1,164 @@
+`timescale 1ns / 1ps
+
+module motor_integration_tb;
+
+    localparam ONE_MS = 50_000;
+
+    localparam FREQUENCY = 13;
+    localparam TOO_CLOSE = 8'd30;
+
+    typedef enum logic [3:0] {
+        IDLE_BASE = 4'b0000,
+        FORWARDS,
+		TURN,
+		TO_TABLE,
+        IDLE_TABLE,
+        BACKWARDS,
+		TURN_BACK,
+		RETURN_HOME,
+		TO_FACE,
+        STOP
+    } state_type;
+
+    logic clk;
+    logic rst;
+
+    // Frequency Input
+    logic [9:0] frequency_input;
+    logic [4:0] threshold_frequency;
+
+    // Ultrasonic Input
+    logic [7:0] distance;
+
+    // Pixel Input
+    logic [16:0] red_pixels;
+    logic [16:0] green_pixels;
+    logic [16:0] blue_pixels;
+    logic [16:0] threshold_pixels;
+
+    // Output - direction
+    logic [3:0] direction;
+    direction_fsm #(
+        .FREQUENCY(FREQUENCY),
+        .TOO_CLOSE(TOO_CLOSE)
+    ) chassis_movement (
+        .clk(clk),
+        .frequency_input(frequency_input),
+        .threshold_frequency(threshold_frequency),
+        .reset(rst),
+        .distance(distance),
+        .red_pixels(red_pixels),
+        .green_pixels(green_pixels),
+        .blue_pixels(blue_pixels),
+        .threshold_pixels(threshold_pixels),
+        .direction(direction)
+    );
+
+    initial clk = 0;
+    always #10 clk = ~clk;
+
+    initial begin
+        $dumpfile("waveform.vcd");
+        $dumpvars();
+
+        rst = 0;
+
+        frequency_input = 0;
+        threshold_frequency = 0;
+
+        distance = 0;
+
+        red_pixels = 0;
+        green_pixels = 0;
+        blue_pixels = 0;
+        threshold_pixels = 0;
+
+        direction = 0;
+
+        #20;
+
+        threshold_frequency = 5'b11000; // 24
+
+        // 32768 pixels / 76800 pixels = 43% of all pixels
+        threshold_pixels = 17'b01000000000000000; 
+
+        rst = 1;
+        #60;
+        rst = 0;
+
+        #20;
+        frequency_input = 10'd12;
+        #50000;
+        frequency_input = 10'd48; // Double the value of the threshold
+
+        wait(direction == FORWARDS);
+        #(50000);
+
+        frequency_input = 10'b0;
+        blue_pixels = 17'd19200;
+        #50000;
+        blue_pixels = 17'd38400; // 50% of pixels
+
+        wait(direction == TURN);
+        #(50000);
+
+        blue_pixels = 17'b0;
+        green_pixels = 17'd19200;
+        #50000;
+        green_pixels = 17'd38400;
+
+        wait(direction == TO_TABLE);
+        #(50000);
+
+        green_pixels = 17'b0;
+        red_pixels = 17'd19200;
+        #50000;
+        red_pixels = 17'd38400;
+
+        wait(direction == TO_FACE);
+        #(20);
+
+        red_pixels = 17'b0;
+
+        wait(direction == IDLE_TABLE)
+        #(50000);
+
+        frequency_input = 10'd48;
+
+        wait(direction == BACKWARDS);
+        #(50000);
+
+        frequency_input = 10'b0;
+        green_pixels = 17'd38400;
+
+        wait(direction == TURN_BACK);
+        #(50000);
+
+        green_pixels = 17'b0;
+        blue_pixels = 17'd38400;
+
+        wait(direction == RETURN_HOME);
+        #(100);
+
+        blue_pixels = 17'b0;
+
+        // Check distance, must return to IDLE_BASE when distance < 30cm
+        distance = 8'd50; // 50cm
+
+        #50000;
+
+        distance = 8'd40;
+
+        #50000;
+
+        distance = 8'd25;
+
+        #50000;
+    
+        $display("Transmission complete.");
+
+        $finish();
+    end
+
+
+endmodule
\ No newline at end of file
diff --git a/src/main/tb/sensor_driver_tb.sv b/src/main/tb/sensor_driver_tb.sv
new file mode 100644
index 0000000..f5123c4
--- /dev/null
+++ b/src/main/tb/sensor_driver_tb.sv
@@ -0,0 +1,69 @@
+module sensor_driver_tb();
+
+parameter CLK_PERIOD = 20;
+
+logic clk;
+logic echo;
+logic trigger;
+logic start;
+logic reset;
+logic [7:0] distance;
+
+sensor_driver u0(
+	.clk(clk),
+	.echo(echo),
+	.measure(start),
+	.rst(reset),
+	.trig(trigger),
+	.distance(distance)
+
+);
+
+initial clk = 1'b0;
+
+always begin
+    #10 
+	 clk = ~clk;
+end
+  
+ initial begin
+
+	reset = 1;
+	start = 0;
+	distance = 0;
+
+	#(1 * CLK_PERIOD)
+	reset = 0; 
+	start = 1;
+	
+	#(1 * CLK_PERIOD)
+	start = 0;
+	
+	#(100 * CLK_PERIOD)
+	echo = 1;
+	#(100000 * CLK_PERIOD)
+	
+	#(1 * CLK_PERIOD)
+	echo = 0;
+	
+	#(10 * CLK_PERIOD)
+
+	#(1 * CLK_PERIOD)
+	start = 1;
+	
+	#(1 * CLK_PERIOD)
+	start = 0;
+	
+	#(100 * CLK_PERIOD)
+	echo = 1;
+	#(100000 * CLK_PERIOD)
+	
+	#(1 * CLK_PERIOD)
+	echo = 0;
+	
+	#(10 * CLK_PERIOD)
+
+	$finish();
+  end
+
+endmodule 
diff --git a/src/main/top_level.sv b/src/main/top_level.sv
index 66d6b12..faad36c 100644
--- a/src/main/top_level.sv
+++ b/src/main/top_level.sv
@@ -263,16 +263,16 @@ module top_level (
 	
 	/*
 	For reference: direction is an enum:
-		IDLE_BASE, 	0	0000
-      FORWARDS,	1	0001
+		IDLE_BASE, 		0	0000
+		FORWARDS,	    1	0001
 		TURN,			2	0010
-		TO_TABLE,	3	0011
-      IDLE_TABLE,	4	0100
-      BACKWARDS,	5	0101
-		TURN_BACK,	6	0110
-		RETURN_HOME,7	0111
-      STOP			8	1000
-	
+		TO_TABLE,		3	0011
+		TO_FACE,    	4   0100
+		IDLE_TABLE,		5	0101
+		BACKWARDS,		6	0110
+		TURN_BACK,		7	0111
+		RETURN_HOME,	8	1000
+		STOP			9	1001
 	*/
 	
 	assign LEDG[3:0] = direction;