Merge branch 'hh6chen-directional-controls' into 'main'

wasd working tested with loc 24

See merge request cs452-kernel!68

kernel/task.h

@@ -18,7 +18,7 @@
 // The task has executed AwaitEvent()
 #define g_blocked_event g_max_task_priority + 3
 
-#define g_max_task_id 100 // largest possible ID for a task
+#define g_max_task_id 200 // largest possible ID for a task
 #define g_total_queues 9
 
 struct Task {

track_info/track_data_a.c

@@ -2,6 +2,14 @@
 
 #include "track_data_a.h"
 
+const uint8_t g_branch_info_a[22][2]
+	= { { 1, STRAIGHT_IS_RIGHT },	{ 2, STRAIGHT_IS_RIGHT },  { 3, STRAIGHT_IS_RIGHT },   { 4, STRAIGHT_IS_LEFT },
+		{ 5, STRAIGHT_IS_RIGHT },	{ 6, STRAIGHT_IS_LEFT },   { 7, STRAIGHT_IS_RIGHT },   { 8, STRAIGHT_IS_LEFT },
+		{ 9, STRAIGHT_IS_RIGHT },	{ 10, STRAIGHT_IS_RIGHT }, { 11, STRAIGHT_IS_RIGHT },  { 12, STRAIGHT_IS_RIGHT },
+		{ 13, STRAIGHT_IS_LEFT },	{ 14, STRAIGHT_IS_LEFT },  { 15, STRAIGHT_IS_RIGHT },  { 16, STRAIGHT_IS_RIGHT },
+		{ 17, STRAIGHT_IS_LEFT },	{ 18, STRAIGHT_IS_LEFT },  { 153, STRAIGHT_IS_RIGHT }, { 154, STRAIGHT_IS_RIGHT },
+		{ 155, STRAIGHT_IS_RIGHT }, { 156, STRAIGHT_IS_RIGHT } };
+
 static void* memset(void* s, int c, unsigned int n) {
 	unsigned char* p = s;
 	while (n-- > 0) {

track_info/track_data_a.h

@@ -2,10 +2,13 @@
 #define TRACK_DATA_A_H
 /* THIS FILE IS GENERATED CODE -- DO NOT EDIT */
 
+#include "stdint.h"
 #include "track_node.h"
 
 // The track initialization functions expect an array of this size.
 #define TRACK_MAX_A 144
 
 void init_tracka(track_node* track);
+
+extern const uint8_t g_branch_info_a[22][2];
 #endif
\ No newline at end of file

track_info/track_data_b.c

@@ -2,6 +2,14 @@
 
 #include "track_data_b.h"
 
+const uint8_t g_branch_info_b[22][2]
+	= { { 1, STRAIGHT_IS_LEFT },   { 2, STRAIGHT_IS_LEFT },	 { 3, STRAIGHT_IS_LEFT },	{ 4, STRAIGHT_IS_LEFT },
+		{ 5, STRAIGHT_IS_LEFT },   { 6, STRAIGHT_IS_LEFT },	 { 7, STRAIGHT_IS_LEFT },	{ 8, STRAIGHT_IS_LEFT },
+		{ 9, STRAIGHT_IS_LEFT },   { 10, STRAIGHT_IS_LEFT }, { 11, STRAIGHT_IS_LEFT },	{ 12, STRAIGHT_IS_LEFT },
+		{ 13, STRAIGHT_IS_LEFT },  { 14, STRAIGHT_IS_LEFT }, { 15, STRAIGHT_IS_LEFT },	{ 16, STRAIGHT_IS_LEFT },
+		{ 17, STRAIGHT_IS_LEFT },  { 18, STRAIGHT_IS_LEFT }, { 153, STRAIGHT_IS_LEFT }, { 154, STRAIGHT_IS_LEFT },
+		{ 155, STRAIGHT_IS_LEFT }, { 156, STRAIGHT_IS_LEFT } };
+
 static void* memset(void* s, int c, unsigned int n) {
 	unsigned char* p = s;
 	while (n-- > 0) {

track_info/track_data_b.h

@@ -2,10 +2,14 @@
 #define TRACK_DATA_B_H
 /* THIS FILE IS GENERATED CODE -- DO NOT EDIT */
 
+#include "stdint.h"
 #include "track_node.h"
 
 // The track initialization functions expect an array of this size.
 #define TRACK_MAX_B 140
 
 void init_trackb(track_node* track);
+
+extern const uint8_t g_branch_info_b[22][2];
+
 #endif
\ No newline at end of file

track_info/track_node.h

@@ -13,6 +13,9 @@ typedef enum {
 #define DIR_STRAIGHT 0
 #define DIR_CURVED 1
 
+#define STRAIGHT_IS_LEFT 1
+#define STRAIGHT_IS_RIGHT 2
+
 struct track_node;
 typedef struct track_node track_node;
 typedef struct track_edge track_edge;

user/bootstrap.c

@@ -122,19 +122,19 @@ void bootstrap_tc1() {
 	Create(0, clock_notifier);
 	Create(0, uart_term_server_tx);
 	if (NO_CTS) {
-		Create(0, uart_train_server_tx_no_cts);
+		Create(1, uart_train_server_tx_no_cts);
 	} else {
-		Create(0, uart_train_server_tx);
+		Create(1, uart_train_server_tx);
 	}
-	Create(0, uart_term_server_rx);
-	Create(0, uart_train_server_rx);
+	Create(1, uart_term_server_rx);
+	Create(1, uart_train_server_rx);
 	Create(0, uart_notifier);
 	Create(0, uart_notifier);
 	Create(0, uart_notifier);
 	Create(0, uart_notifier);
 	Create(0, uart_notifier);
 	Create(0, uart_notifier);
-	Create(0, random_server);
+	Create(4, random_server);
 	Create(4, idle_server);
 
 	// A0 servers
@@ -155,7 +155,7 @@ void bootstrap_tc1() {
 
 	Create(1, sensor_query_server);
 
-	for (int i = 0; i < 19; i++) {
+	for (int i = 0; i < 50; i++) {
 		Create(1, terminal_admin);
 	}
 	for (int i = 0; i < 6; i++) {

user/pathfinding.c

@@ -544,3 +544,19 @@ int sensors_left(struct Ring_Buffer* path_buf) {
 
 	return sensors_left;
 }
+
+// returns next branch on path, 0xFF if not possible
+uint8_t next_branch_from_sensor(uint8_t node) {
+	uint8_t next_node_idx = node;
+	for (int i = 0; i < 10; i++) {
+		uint8_t node_idx = next_node_idx;
+		struct track_node node = track_nodes[node_idx];
+
+		if (node.type == NODE_BRANCH) {
+			return node_idx;
+		} else {
+			next_node_idx = node.edge[DIR_AHEAD].dest->idx;
+		}
+	}
+	return 0xFF;
+}

user/pathfinding.h

@@ -26,4 +26,5 @@ uint8_t dijkstra_tc2(uint8_t src,
 					 uint8_t* reverse_train);
 int distance_to_next_sensor_on_path(struct Ring_Buffer* path_buf, struct track_node* cur_loc);
 int sensors_left(struct Ring_Buffer* path_buf);
+uint8_t next_branch_from_sensor(uint8_t node);
 #endif
\ No newline at end of file

user/term_admin.c

@@ -121,6 +121,10 @@ void terminal_admin() {
 			int train_num = tokens[1].value;
 			uint8_t sensor = (tokens[2].value - 'A') * 16 + tokens[3].value - 1;
 			locate_train(train_num, sensor);
+		} else if (tokens[0].type == COMMAND_CHAR && tokens[1].type == COMMAND_EOL
+				   && (tokens[0].value == 'W' || tokens[0].value == 'A' || tokens[0].value == 'S'
+					   || tokens[0].value == 'D')) {
+			control_user_train(tokens[0].value);
 		} else {
 			debug_prints("Invalid Command\r\n");
 		}

user/track_admin.c

@@ -798,9 +798,11 @@ void switch_worker() {
 	struct Track_Request track_req = { .request_type = TRACK_UPDATE_SWITCH, .sw_count = 0 };
 	int uart_train_tx_tid = get_tid_by_name(UART_SERVER_TRAIN_SEND);
 	char cmd[] = "  ";
+	char switch_flipped = 0;
 	for (int i = 0; i < req->sw_count; i++) {
 		// check if we need to flip it
 		if (req->directions[i] != get_switch_state(req->switch_nums[i])) {
+			switch_flipped = 1;
 			cmd[0] = req->directions[i] == 'S' ? SWITCH_STRAIGHT : SWITCH_CURVED;
 			cmd[1] = req->switch_nums[i];
 
@@ -842,7 +844,9 @@ void switch_worker() {
 	cmd[0] = SWITCH_SOLENOID_OFF;
 	cmd[1] = '\0';
 	Delay(get_tid_by_name(CLOCK_SERVER), SOLENOID_OFF_DELAY);
-	send_switch_command(uart_train_tx_tid, cmd);
+	if (switch_flipped){
+		send_switch_command(uart_train_tx_tid, cmd);
+	}
 
 	int track_admin_tid = WhoIs(TRACK_ADMIN);
 	char reply[1];

user/train_control.c

 #include "train_control.h"
 
+#define FORWARDS 0
+#define BACKWARDS 1
+
 const char TRAIN_STOP = 0;
 const char TRAIN_MAX_SPEED = 14;
 const char TRAIN_REVERSE = 15;
@@ -228,7 +231,6 @@ void train_worker() {
 		} else if (request->request_type == RV_I) {
 			// reverse immediately and turn on the lights
 			char reverse[] = { TRAIN_REVERSE, train_num, '\0' };
-			Delay(clock_server_tid, 40);
 			send_train_command(uart_train_tx_tid, reverse);
 			Delay(clock_server_tid, 10);
 			char stop_cmd[] = { TRAIN_STOP + TRAIN_LIGHTS_ON, train_num, '\0' };
@@ -743,6 +745,7 @@ void back_up_train(int clock_server_tid, struct Train_Info train) {
 	Delay(clock_server_tid, ticks_to_back_up);
 	// debug_prints("STOPPING AFTER BACKUP\r\n");
 	send_speed_command(train.train_num, 0);
+	Delay(clock_server_tid, 40);
 	// debug_prints("REVERSING INSTANTLY\r\n");
 	reverse_instantly(train.train_num); // this will delay 400ms by itself
 }
@@ -848,6 +851,7 @@ void train_admin() {
 
 	int run_random_paths = 0;
 
+	int user_train = 24;
 	// train metadata
 	char cur_train = 24;
 	int cur_train_ind = 2;
@@ -868,6 +872,12 @@ void train_admin() {
 	char calibrating_train = 0;
 	int calibration_task_tid = 0;
 
+	char user_command;
+	int user_train_ind;
+	uint8_t user_train_next_branch;
+	char user_train_switch_num;
+	char direction;
+
 	for (;;) {
 		// status:
 		// green: ready
@@ -877,6 +887,7 @@ void train_admin() {
 		prints("\0337\033[1;40H\033[32mTC\033[0m\0338");
 		Receive(&sender, rcvbuf, sizeof(struct Train_Admin_Request));
 
+		curtime = Time(clock_server_tid);
 		// prints("\0337\033[1;40H\033[31mTC\033[0m\0338");
 		struct Train_Admin_Request* req = (struct Train_Admin_Request*)rcvbuf;
 		if (req->request_type != TA_TRY_TO_START_PATH) {
@@ -884,6 +895,65 @@ void train_admin() {
 		}
 
 		switch (req->request_type) {
+		case TA_USER_TRAIN_COMMAND:
+			user_command = req->update;
+			user_train_ind = train_num_to_index(user_train);
+			if (user_command == 'W') {
+				debug_prints("User hit W\r\n");
+
+				if (train_metadata[user_train_ind].direction == BACKWARDS) {
+					if (train_metadata[user_train_ind].train_speed == 0) {
+						// reverse instantly and go forwards direction
+						reverse_instantly(user_train);
+						train_metadata[user_train_ind].direction = FORWARDS;
+					} else {
+						send_speed_command(user_train, 0);
+					}
+				} else {
+					send_speed_command(user_train, g_train_speeds[user_train_ind][1]);
+				}
+			} else if (user_command == 'A') {
+				debug_prints("User hit A\r\n");
+				// left
+				user_train_next_branch = next_branch_from_sensor(train_metadata[user_train_ind].cur_loc->idx);
+				if (user_train_next_branch == 0xFF) {
+					debug_prints("no branches ahead\r\n");
+					break;
+				}
+				user_train_switch_num = track_nodes[user_train_next_branch].num;
+				ASSERT(g_branch_info_a[(int)user_train_switch_num - 1][0] == user_train_switch_num);
+				direction = g_branch_info_a[(int)user_train_switch_num - 1][1] == STRAIGHT_IS_LEFT ? 'S' : 'C';
+				turn_switches(&user_train_switch_num, &direction, 1);
+			} else if (user_command == 'S') {
+				debug_prints("User hit S\r\n");
+				if (train_metadata[user_train_ind].direction == FORWARDS) {
+					if (train_metadata[user_train_ind].train_speed == 0) {
+						// reverse instantly and go in backwards direction
+						reverse_instantly(user_train);
+						train_metadata[user_train_ind].direction = BACKWARDS;
+					} else {
+						send_speed_command(user_train, 0);
+					}
+				} else {
+					send_speed_command(user_train, g_train_speeds[user_train_ind][1]);
+				}
+			} else if (user_command == 'D') {
+				debug_prints("User hit D\r\n");
+				user_train_next_branch = next_branch_from_sensor(train_metadata[user_train_ind].cur_loc->idx);
+
+				if (user_train_next_branch == 0xFF) {
+					debug_prints("no branches ahead\r\n");
+					break;
+				}
+				user_train_switch_num = track_nodes[user_train_next_branch].num;
+				ASSERT(g_branch_info_a[(int)user_train_switch_num - 1][0] == user_train_switch_num);
+				direction = g_branch_info_a[(int)user_train_switch_num - 1][1] == STRAIGHT_IS_LEFT ? 'C' : 'S';
+
+				turn_switches(&user_train_switch_num, &direction, 1);
+			} else {
+				ASSERT(0);
+			}
+			break;
 		case TA_CALIBRATION_BEGIN:
 			if (calibrating_train) {
 				debug_printi("Already calibrating train ", calibrating_train, ", please try again later\r\n");
@@ -939,7 +1009,6 @@ void train_admin() {
 			}
 			break;
 		case TA_SENSOR_UPDATE:
-			curtime = Time(clock_server_tid);
 			// If we're calibrating a train, need to forward this update to our calibration task
 			if (calibrating_train) {
 				calibration_request.request_type = CR_SENSOR;
@@ -1410,3 +1479,9 @@ void train_calibration_task() {
 	Send(get_tid_by_name(TRAIN_ADMIN), (char*)&end_req, sizeof(struct Train_Admin_Request), &reply, 1);
 	Exit();
 }
+
+void control_user_train(char command) {
+	struct Train_Admin_Request req = { .request_type = TA_USER_TRAIN_COMMAND, .update = command };
+	char reply;
+	Send(get_tid_by_name(TRAIN_ADMIN), (char*)&req, sizeof(struct Train_Admin_Request), &reply, 1);
+}
\ No newline at end of file

user/train_control.h

@@ -39,6 +39,7 @@ enum train_admin_request_type {
 	TA_SET_TRAIN,
 	TA_RNG,
 	TA_LOCATE_TRAIN,
+	TA_USER_TRAIN_COMMAND,
 };
 
 struct Train_Admin_Request {
@@ -82,4 +83,5 @@ int train_num_to_index(char train_num);
 void train_calibration_task();
 void toggle_random_paths();
 void locate_train(int train_num, int sensor);
+void control_user_train(char command);
 #endif
\ No newline at end of file

user/uart.c

@@ -200,6 +200,10 @@ void uart_train_server_rx() {
 	}
 }
 
+int is_directional_control(char byte) {
+	return (byte == 'W' || byte == 'A' || byte == 'S' || byte == 'D');
+}
+
 void uart_term_server_rx() {
 	RegisterAs(UART_SERVER_TERM_RECEIVE);
 	int sender;
@@ -233,7 +237,12 @@ void uart_term_server_rx() {
 
 		while (rx_state == UART_RX_READY && can_read_char(0)) {
 			char c = UartGetc(0);
-			if (c == '\r') {
+			if (is_directional_control(c) && ring_buffer_is_empty(&char_buffer)) {
+				ASSERT(!ring_buffer_is_empty(&terminal_admin_buffer));
+				int terminal_admin = ring_buffer_pop(&terminal_admin_buffer);
+				char command[2] = { c, '\0' };
+				Reply(terminal_admin, command, 2);
+			} else if (c == '\r') {
 				ASSERT(!ring_buffer_is_empty(&terminal_admin_buffer));
 				int terminal_admin = ring_buffer_pop(&terminal_admin_buffer);
 				char command[100];