Merge branch 'fkzheng-24-sensor-attribution' into 'main'

Working 24 controls + attribution

See merge request cs452-kernel!70

track_info/track_data_b.c

@@ -1214,84 +1214,84 @@ void init_trackb(track_node* track) {
 		track[i].idx = i;
 	}
 
-	track[0].image = "\033[49;68Ho";
-	track[1].image = "\033[49;68Ho";
-	track[2].image = "\033[41;64Ho";
-	track[3].image = "\033[41;64Ho";
-	track[4].image = "\033[27;66Ho";
-	track[5].image = "\033[27;66Ho";
-	track[6].image = "\033[29;68Ho";
-	track[7].image = "\033[29;68Ho";
-	track[8].image = "\033[31;70Ho";
-	track[9].image = "\033[31;70Ho";
-	track[10].image = "\033[37;68Ho";
-	track[11].image = "\033[37;68Ho";
-	track[12].image = "\033[47;71Ho";
-	track[13].image = "\033[47;71Ho";
-	track[14].image = "\033[42;68Ho";
-	track[15].image = "\033[42;68Ho";
-	track[16].image = "\033[31;37Ho";
-	track[17].image = "\033[31;37Ho";
-	track[18].image = "\033[34;37Ho";
-	track[19].image = "\033[34;37Ho";
-	track[20].image = "\033[47;37Ho";
-	track[21].image = "\033[47;37Ho";
-	track[22].image = "\033[31;83Ho";
-	track[23].image = "\033[31;83Ho";
-	track[24].image = "\033[27;83Ho";
-	track[25].image = "\033[27;83Ho";
-	track[26].image = "\033[29;83Ho";
-	track[27].image = "\033[29;83Ho";
-	track[28].image = "\033[36;31Ho";
-	track[29].image = "\033[36;31Ho";
-	track[30].image = "\033[38;64Ho";
-	track[31].image = "\033[38;64Ho";
-	track[32].image = "\033[36;35Ho";
-	track[33].image = "\033[36;35Ho";
-	track[34].image = "\033[27;18Ho";
-	track[35].image = "\033[27;18Ho";
-	track[36].image = "\033[29;52Ho";
-	track[37].image = "\033[29;52Ho";
-	track[38].image = "\033[27;50Ho";
-	track[39].image = "\033[27;50Ho";
-	track[40].image = "\033[31;53Ho";
-	track[41].image = "\033[31;53Ho";
-	track[42].image = "\033[47;49Ho";
-	track[43].image = "\033[47;49Ho";
-	track[44].image = "\033[49;51Ho";
-	track[45].image = "\033[49;51Ho";
-	track[46].image = "\033[29;38Ho";
-	track[47].image = "\033[29;38Ho";
-	track[48].image = "\033[43;30Ho";
-	track[49].image = "\033[43;30Ho";
-	track[50].image = "\033[47;29Ho";
-	track[51].image = "\033[47;29Ho";
-	track[52].image = "\033[46;04Ho";
-	track[53].image = "\033[46;04Ho";
-	track[54].image = "\033[47;03Ho";
-	track[55].image = "\033[47;03Ho";
-	track[56].image = "\033[32;03Ho";
-	track[57].image = "\033[32;03Ho";
-	track[58].image = "\033[29;27Ho";
-	track[59].image = "\033[29;27Ho";
-	track[60].image = "\033[31;29Ho";
-	track[61].image = "\033[31;29Ho";
-	track[62].image = "\033[34;29Ho";
-	track[63].image = "\033[34;29Ho";
-	track[64].image = "\033[43;36Ho";
-	track[65].image = "\033[43;36Ho";
-	track[66].image = "\033[45;28Ho";
-	track[67].image = "\033[45;28Ho";
-	track[68].image = "\033[47;16Ho";
-	track[69].image = "\033[47;16Ho";
-	track[70].image = "\033[49;14Ho";
-	track[71].image = "\033[49;14Ho";
-	track[72].image = "\033[33;04Ho";
-	track[73].image = "\033[33;04Ho";
-	track[74].image = "\033[29;14Ho";
-	track[75].image = "\033[29;14Ho";
-	track[76].image = "\033[31;16Ho";
-	track[77].image = "\033[31;16Ho";
-	track[78].image = "\033[45;38Ho";
-	track[79].image = "\033[45;38Ho";
+	track[0].image = "\033[49;68H";
+	track[1].image = "\033[49;68H";
+	track[2].image = "\033[41;64H";
+	track[3].image = "\033[41;64H";
+	track[4].image = "\033[27;66H";
+	track[5].image = "\033[27;66H";
+	track[6].image = "\033[29;68H";
+	track[7].image = "\033[29;68H";
+	track[8].image = "\033[31;70H";
+	track[9].image = "\033[31;70H";
+	track[10].image = "\033[37;68H";
+	track[11].image = "\033[37;68H";
+	track[12].image = "\033[47;71H";
+	track[13].image = "\033[47;71H";
+	track[14].image = "\033[42;68H";
+	track[15].image = "\033[42;68H";
+	track[16].image = "\033[31;37H";
+	track[17].image = "\033[31;37H";
+	track[18].image = "\033[34;37H";
+	track[19].image = "\033[34;37H";
+	track[20].image = "\033[47;37H";
+	track[21].image = "\033[47;37H";
+	track[22].image = "\033[31;83H";
+	track[23].image = "\033[31;83H";
+	track[24].image = "\033[27;83H";
+	track[25].image = "\033[27;83H";
+	track[26].image = "\033[29;83H";
+	track[27].image = "\033[29;83H";
+	track[28].image = "\033[36;31H";
+	track[29].image = "\033[36;31H";
+	track[30].image = "\033[38;64H";
+	track[31].image = "\033[38;64H";
+	track[32].image = "\033[36;35H";
+	track[33].image = "\033[36;35H";
+	track[34].image = "\033[27;18H";
+	track[35].image = "\033[27;18H";
+	track[36].image = "\033[29;52H";
+	track[37].image = "\033[29;52H";
+	track[38].image = "\033[27;50H";
+	track[39].image = "\033[27;50H";
+	track[40].image = "\033[31;53H";
+	track[41].image = "\033[31;53H";
+	track[42].image = "\033[47;49H";
+	track[43].image = "\033[47;49H";
+	track[44].image = "\033[49;51H";
+	track[45].image = "\033[49;51H";
+	track[46].image = "\033[29;38H";
+	track[47].image = "\033[29;38H";
+	track[48].image = "\033[43;30H";
+	track[49].image = "\033[43;30H";
+	track[50].image = "\033[47;29H";
+	track[51].image = "\033[47;29H";
+	track[52].image = "\033[46;04H";
+	track[53].image = "\033[46;04H";
+	track[54].image = "\033[47;03H";
+	track[55].image = "\033[47;03H";
+	track[56].image = "\033[32;03H";
+	track[57].image = "\033[32;03H";
+	track[58].image = "\033[29;27H";
+	track[59].image = "\033[29;27H";
+	track[60].image = "\033[31;29H";
+	track[61].image = "\033[31;29H";
+	track[62].image = "\033[34;29H";
+	track[63].image = "\033[34;29H";
+	track[64].image = "\033[43;36H";
+	track[65].image = "\033[43;36H";
+	track[66].image = "\033[45;28H";
+	track[67].image = "\033[45;28H";
+	track[68].image = "\033[47;16H";
+	track[69].image = "\033[47;16H";
+	track[70].image = "\033[49;14H";
+	track[71].image = "\033[49;14H";
+	track[72].image = "\033[33;04H";
+	track[73].image = "\033[33;04H";
+	track[74].image = "\033[29;14H";
+	track[75].image = "\033[29;14H";
+	track[76].image = "\033[31;16H";
+	track[77].image = "\033[31;16H";
+	track[78].image = "\033[45;38H";
+	track[79].image = "\033[45;38H";
 }

user/pathfinding.c

@@ -173,6 +173,31 @@ char is_our_own_parking_spot(uint8_t node, uint8_t* our_parking_spots, uint8_t n
 	return 0;
 }
 
+int is_section_blocked2(uint16_t velocity,
+						uint32_t blocked[144][6][2],
+						int node_idx,
+						uint32_t dist,
+						uint32_t start_time,
+						int parking_spot) {
+	debug_prints("is_section_blocked2\r\n");
+	uint8_t our_parking_spots[40];
+	uint8_t num_our_parking_spots = get_nodes_for_parking(parking_spot, our_parking_spots);
+	uint32_t time_to_loc = ((dist * 100) / velocity) + start_time;
+	uint32_t time_to_loc_min = time_to_loc < 100 ? 0 : time_to_loc - 100; // - 1 second for error
+	uint32_t time_to_loc_max = time_to_loc + 400;						  // + 4 second for error
+	if (is_our_own_parking_spot(node_idx, our_parking_spots, num_our_parking_spots)) {
+		return 0;
+	}
+
+	for (int i = 0; i < 6; i++) {
+		if (time_to_loc_min <= blocked[node_idx][i][1] && blocked[node_idx][i][0] <= time_to_loc_max) {
+			return 1;
+		}
+	}
+	debug_prints("is_section_blocked2 done\r\n");
+	return 0;
+}
+
 // checks if we cant park at the given node
 // parking spot is our own parking spot, we can park on nodes that are blocked by it
 char parking_spot_is_blocked(uint8_t node, uint32_t blocked[144][6][2], uint8_t parking_spot, uint32_t curtime) {

user/pathfinding.h

@@ -27,4 +27,7 @@ uint8_t dijkstra_tc2(uint8_t src,
 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);
+char parking_spot_is_blocked(uint8_t node, uint32_t blocked[144][6][2], uint8_t parking_spot, uint32_t curtime);
+int is_section_blocked2(
+	uint16_t velocity, uint32_t blocked[144][6][2], int node_idx, uint32_t dist, uint32_t start_time, int parking_spot);
 #endif
\ No newline at end of file

user/sensors.h

@@ -14,7 +14,6 @@
 
 struct sensor_data_publisher_request;
 
-void read_garbage_bytes_then_spawn_sensor_servers();
 void sensor_read_courier();
 void sensor_read_courier_no_cts();
 void sensor_query_server();

user/track_admin.c

@@ -553,8 +553,9 @@ void track_admin() {
 			_print_reservation_table(track_node_reservations, curtime, cur_res_screen_state);
 			break;
 		case TRACK_GET_SWITCH:
-			reply[0] = switch_states[(uint8_t)req->switch_num];
+			reply[0] = switch_states[(int)req->switch_num];
 			Reply(sender, reply, 1);
+
 			break;
 		case TRACK_RESERVE_SEGMENTS:
 			reply[0] = 1;
@@ -581,8 +582,9 @@ void track_admin() {
 		case TRACK_UNPARK:
 			ASSERT(req->node_count == 1);
 			reply[0] = 1;
-			_unpark_train(track_node_reservations, req->track_nodes_encoded[0]);
 			Reply(sender, reply, 1);
+			_unpark_train(track_node_reservations, req->track_nodes_encoded[0]);
+
 			break;
 		case TRACK_PARK:
 			ASSERT(req->node_count == 1);
@@ -774,13 +776,14 @@ void flip_next_switches(struct Ring_Buffer* path) {
 	}
 	sw_count = turn_switches(switches, directions, sw_count);
 	// printi("Flipping ", sw_count, " switches\r\n");
-	Delay(get_tid_by_name(CLOCK_SERVER), 100);
+	// Delay(get_tid_by_name(CLOCK_SERVER), 100);
 }
 
 char get_switch_state(uint8_t switch_num) {
 	int track_admin_tid = get_tid_by_name(TRACK_ADMIN);
 
 	// prints("DEBUG\r\n");
+	debug_printi("switch_num: ", switch_num, "\r\n");
 	struct Track_Request track_req = { .request_type = TRACK_GET_SWITCH, .switch_num = switch_num };
 	char reply[1];
 	Send(track_admin_tid, (char*)&track_req, sizeof(struct Track_Request), reply, 1);
@@ -798,11 +801,9 @@ 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];
 
@@ -843,19 +844,16 @@ void switch_worker() {
 	// send solenoid off
 	cmd[0] = SWITCH_SOLENOID_OFF;
 	cmd[1] = '\0';
-	Delay(get_tid_by_name(CLOCK_SERVER), SOLENOID_OFF_DELAY);
-	if (switch_flipped) {
+	if (track_req.sw_count) {
+		Delay(get_tid_by_name(CLOCK_SERVER), SOLENOID_OFF_DELAY);
 		send_switch_command(uart_train_tx_tid, cmd);
+		char reply[1];
+		Send(get_tid_by_name(TRACK_ADMIN), (char*)&track_req, sizeof(struct Track_Request), reply, 1);
 	}
-
-	int track_admin_tid = WhoIs(TRACK_ADMIN);
-	char reply[1];
-	Send(track_admin_tid, (char*)&track_req, sizeof(struct Track_Request), reply, 1);
-
+	debug_prints("Switch worker exiting\r\n");
 	Exit();
 }
 
-// TODO: Have this ask the track admin which switches actually need to be flipped because switch flipping is very slow
 int turn_switches(char* switch_nums, char* directions, uint8_t sw_count) {
 	ASSERT(sw_count < 23);
 	struct Switch_Request req = { .sw_count = 0 };
@@ -872,10 +870,11 @@ int turn_switches(char* switch_nums, char* directions, uint8_t sw_count) {
 			printi("invalid switch direction on switch: ", switch_num, ", must be one of {'S', 'C'}\r\n");
 			return 0;
 		}
-
-		req.switch_nums[i] = switch_num;
-		req.directions[i] = direction;
-		req.sw_count++;
+		if (get_switch_state(switch_num) != direction) {
+			req.switch_nums[i] = switch_num;
+			req.directions[i] = direction;
+			req.sw_count++;
+		}
 	}
 
 	if (req.sw_count == 0) {

user/track_admin.h

@@ -50,7 +50,7 @@ void track_admin();
 void track_ui_admin();
 void flip_next_switches(struct Ring_Buffer* path);
 int turn_switches(char* switch_nums, char* directions, uint8_t sw_count);
-
+char get_switch_state(uint8_t switch_num);
 // returns 1 on successfully reserving all nodes, otherwise returns 0 (fake for reserve nodes now)
 int reserve_nodes(uint8_t nodes_to_reserve[50],
 				  uint8_t num_nodes_to_reserve,

user/train_control.c

@@ -54,6 +54,17 @@ static void send_train_command(int tid, const char* cmd) {
 	Send(tid, (char*)&req, sizeof(struct UART_Server_Request), reply, 1);
 }
 
+static void honk() {
+	// we don't need the reply
+	char honk_78[3] = { 79, 78, 0 };
+	char honk_58[3] = { 79, 58, 0 };
+	char reply[1];
+	struct UART_Server_Request req1 = create_uart_request(UART_SEND_TRAIN, honk_78);
+	struct UART_Server_Request req2 = create_uart_request(UART_SEND_TRAIN, honk_58);
+	Send(get_tid_by_name(UART_SERVER_TRAIN_SEND), (char*)&req1, sizeof(struct UART_Server_Request), reply, 1);
+	Send(get_tid_by_name(UART_SERVER_TRAIN_SEND), (char*)&req2, sizeof(struct UART_Server_Request), reply, 1);
+}
+
 void send_speed_command(char train_num, char speed) {
 	char reply_buf[1];
 	struct Train_Worker_Request train_worker_request;
@@ -207,8 +218,10 @@ void train_worker() {
 			if (!(request->speed >= TRAIN_STOP && request->speed <= TRAIN_MAX_SPEED)) {
 				debug_prints("invalid speed\r\n");
 				continue;
+			} else if (cur_speed == request->speed) {
+				continue;
 			}
-			// char cmd[] = { (request->speed == 0 ? TRAIN_STOP + TRAIN_LIGHTS_ON : request->speed), train_num, '\0' };
+
 			char cmd[] = { request->speed + TRAIN_LIGHTS_ON, train_num, '\0' };
 			send_train_command(uart_train_tx_tid, cmd);
 			cur_speed = request->speed;
@@ -386,10 +399,10 @@ void init_train_metadata(struct Train_Info* train_metadata) {
 	train_metadata[5].stopping_distance = 110;
 	train_metadata[5].alpha_denom = 2;
 	train_metadata[5].has_path = 0;
-	train_metadata[5].cur_loc = &track_nodes[7];
-	train_metadata[5].cur_dest = 7;
-	train_metadata[5].parking_spot = 7;
-	train_metadata[5].possible_next_sensors[0] = 38;
+	train_metadata[5].cur_loc = &track_nodes[12];
+	train_metadata[5].cur_dest = 12;
+	train_metadata[5].parking_spot = 12;
+	train_metadata[5].possible_next_sensors[0] = 44;
 	train_metadata[5].possible_next_sensors[1] = -1;
 	train_metadata[5].possible_next_sensors[2] = -1;
 	train_metadata[5].possible_next_sensors[3] = -1;
@@ -523,10 +536,10 @@ void handle_possible_stop(struct Train_Info* train_metadata,
 }
 
 int check_which_train_triggered_sensor(struct Train_Info train_metadata[], int sensor_id) {
-	uint8_t possible_train = 0;
+	int possible_train = 0;
 	uint32_t min_arrival_time = UINT32_MAX;
 	// must have path
-	for (int i = 0; i < MAX_NUM_TRAINS; i++) {
+	for (int i = 0; i < 6; i++) {
 		// we only move the trains if they have a path
 		if (!train_metadata[i].has_path) {
 			continue;
@@ -541,6 +554,15 @@ int check_which_train_triggered_sensor(struct Train_Info train_metadata[], int s
 		}
 	}
 
+	// if train not found, check train 24 (user controlled)
+	if (!possible_train) {
+		for (int j = 0; j < 8; j++) {
+			if (train_metadata[2].possible_next_sensors[j] == sensor_id) {
+				possible_train = train_metadata[2].train_num;
+			}
+		}
+	}
+
 	// if no train expected to go over the sensor, we ignore and return 0
 	return possible_train;
 }
@@ -551,6 +573,8 @@ int update_sensor_attribution(struct Train_Info train_metadata[], int cur_train_
 		train_metadata[cur_train_ind].possible_next_sensors[i] = -1;
 	}
 	// bfs for next sensor including branches
+	ASSERT(train_metadata[cur_train_ind].cur_loc && train_metadata[cur_train_ind].cur_loc->idx <= 144
+		   && train_metadata[cur_train_ind].cur_loc->idx >= 0);
 	struct track_node* cur = train_metadata[cur_train_ind].cur_loc;
 	int queue[40];
 	int front = 0, rear = 0, count = 0;
@@ -584,7 +608,7 @@ void try_to_start_path() {
 	int clock_server_tid = get_tid_by_name(CLOCK_SERVER);
 	int count = 0;
 	while (reply == 0) {
-		debug_printi("attempting to set path for train ", req->train_num, "\r\n");
+		// debug_printi("attempting to set path for train ", req->train_num, "\r\n");
 		Send(sender_tid, (char*)req, sizeof(struct Train_Admin_Request), &reply, 1);
 		Delay(clock_server_tid, 100);
 		count++;
@@ -718,6 +742,7 @@ void _update_train_ui(struct Train_Info train_metadata[6]) {
 
 		// location
 		if (train_metadata[i].cur_loc) {
+			ASSERT(train_metadata[i].cur_loc);
 			_print_ui_sensor(loc_y, loc_x + offset, train_metadata[i].cur_loc->idx);
 		}
 		// time error
@@ -769,10 +794,10 @@ void _start_train_path() {
 	struct start_path_request* req = (struct start_path_request*)rcv_buf;
 
 	// debug_printi("Parking info for train: ", req->train_metadata.train_num, "\r\n");
-	park_train(req->possibly_new_dest, track_admin_tid);
+	// park_train(req->possibly_new_dest, track_admin_tid);
 
 	// Back up train if nessecary
-
+	ASSERT(req->train_metadata.cur_loc);
 	if (is_branch_ahead(req->train_metadata.cur_loc->idx)) {
 		// debug_prints("Found Branch ahead of cur_loc, backing up\r\n");
 		back_up_train(clock_server_tid, req->train_metadata);
@@ -784,7 +809,7 @@ void _start_train_path() {
 
 	flip_next_switches(&req->train_metadata.path);
 
-	unpark_train(req->train_metadata.parking_spot, track_admin_tid);
+	// unpark_train(req->train_metadata.parking_spot, track_admin_tid);
 
 	/*
 	Short move logic:
@@ -827,7 +852,7 @@ void _pathfind_courier() {
 	Receive(&sender, rcvbuf, sizeof(struct random_path_request));
 	Reply(sender, reply, 1);
 
-	Delay(get_tid_by_name(CLOCK_SERVER), 500);
+	// Delay(get_tid_by_name(CLOCK_SERVER), 100);
 	uint8_t dest = get_random_sensor();
 	while (dest == req->cur_loc) {
 		dest = get_random_sensor();
@@ -843,15 +868,48 @@ void set_random_path(uint8_t train_num, uint8_t cur_loc) {
 	Send(child_tid, (char*)&req, sizeof(struct random_path_request), reply, 1);
 }
 
+int can_reach_sensor_ahead(int cur_node_idx, uint32_t blocked[144][6][2], uint32_t curtime, uint32_t velocity) {
+	// assuming cur_node_idx is sensor node
+	struct track_node* next = &track_nodes[cur_node_idx];
+	char switch_state;
+	uint32_t dist = 0;
+	debug_prints("can reach sensor ahead start\r\n");
+	do {
+		// transition
+		if (next->type == NODE_BRANCH) {
+			switch_state = get_switch_state(next->num);
+			ASSERT(switch_state == 'C' || switch_state == 'S');
+			if (switch_state == 'C') {
+				next = next->edge[DIR_CURVED].dest;
+				dist += next->edge[DIR_CURVED].dist;
+			} else {
+				next = next->edge[DIR_STRAIGHT].dest;
+				dist += next->edge[DIR_STRAIGHT].dist;
+			}
+		} else {
+			next = next->edge[DIR_AHEAD].dest;
+			dist += next->edge[DIR_AHEAD].dist;
+		}
+	} while (next->type != NODE_SENSOR);
+	// return parking_spot_is_blocked(next->idx, blocked, cur_node_idx, Time(get_tid_by_name(CLOCK_SERVER)))
+	// 		   ? 0
+	// 		   : (1 + next->idx);
+	debug_printi("Found sensor ahead: ", next->idx, "\r\n");
+
+	return is_section_blocked2(velocity, blocked, next->idx, dist, curtime, cur_node_idx) ? 0 : (next->idx + 1);
+}
+
 void train_admin() {
 	RegisterAs(TRAIN_ADMIN);
 
 	int clock_server_tid = get_tid_by_name(CLOCK_SERVER);
+	int track_admin_tid = get_tid_by_name(TRACK_ADMIN);
 	uint32_t curtime;
 
 	int run_random_paths = 0;
 
 	int user_train = 24;
+	int chase_train = 78;
 	// train metadata
 	char cur_train = 24;
 	int cur_train_ind = 2;
@@ -915,12 +973,14 @@ void train_admin() {
 			} else if (user_command == 'A') {
 				debug_prints("User hit A\r\n");
 				// left
+				ASSERT(train_metadata[user_train_ind].cur_loc);
 				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;
+				// TODO use g_branch_info_b and update b to correct directions
 				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);
@@ -939,6 +999,7 @@ void train_admin() {
 				}
 			} else if (user_command == 'D') {
 				debug_prints("User hit D\r\n");
+				ASSERT(train_metadata[user_train_ind].cur_loc);
 				user_train_next_branch = next_branch_from_sensor(train_metadata[user_train_ind].cur_loc->idx);
 
 				if (user_train_next_branch == 0xFF) {
@@ -1031,6 +1092,28 @@ void train_admin() {
 			update_sensor_attribution(train_metadata, cur_train_ind);
 			// debug_printi("", count, " possible sensors next\r\n");
 
+			// figure out train 24 actions (final project)
+			if (cur_train == user_train) {
+				get_reserved_segments(blocked);
+				// 1. look ahead 1 sensor based on switch state
+				//    1a. If train on ahead node, stop (send 0 immediately)
+				ASSERT(train_metadata[cur_train_ind].cur_loc);
+				int next_sensor_id = can_reach_sensor_ahead(train_metadata[cur_train_ind].cur_loc->idx,
+															blocked,
+															curtime,
+															train_metadata[cur_train_ind].train_velocity[1]);
+				if (next_sensor_id) {
+					debug_printi("Sensor ahead of Train 24:", next_sensor_id - 1, "\r\n");
+					park_train(next_sensor_id - 1, track_admin_tid);
+					unpark_train(train_metadata[cur_train_ind].cur_loc->idx, track_admin_tid);
+				} else {
+					debug_prints("Sensor ahead of Train 24 Blocked\r\n");
+					send_speed_command(user_train, 0);
+					send_speed_command(chase_train, 0);
+					honk();
+				}
+			}
+
 			if (train_metadata[cur_train_ind].has_path) {
 				update_path(&train_metadata[cur_train_ind].path, train_metadata[cur_train_ind].cur_loc, cur_train);
 				flip_next_switches(&train_metadata[cur_train_ind].path);
@@ -1048,11 +1131,12 @@ void train_admin() {
 			}
 
 			// 3. if we hit all the nodes, stop all actions
-			if (ring_buffer_is_empty(&train_metadata[cur_train_ind].path)) {
+			if (train_metadata[cur_train_ind].has_path && ring_buffer_is_empty(&train_metadata[cur_train_ind].path)) {
 				train_metadata[cur_train_ind].has_path = 0;
 
 				if (train_metadata[cur_train_ind].reversing) {
 					debug_prints("reversing train\n");
+					ASSERT(train_metadata[cur_train_ind].cur_loc);
 					train_metadata[cur_train_ind].cur_loc = train_metadata[cur_train_ind].cur_loc->reverse;
 					train_metadata[cur_train_ind].reversing = 0;
 					// rev immediate
@@ -1063,7 +1147,7 @@ void train_admin() {
 				debug_printi("Finished path for train: ", cur_train, "\r\n");
 				// debug_printi("cur_loc: ", train_metadata[cur_train_ind].cur_loc->idx, "\r\n");
 				// debug_printi("cur_dest: ", train_metadata[cur_train_ind].cur_dest, "\r\n");
-
+				ASSERT(train_metadata[cur_train_ind].cur_loc);
 				if (train_metadata[cur_train_ind].cur_loc->idx != train_metadata[cur_train_ind].cur_dest) {
 					child_tid = Create(3, try_to_start_path);
 					struct Train_Admin_Request new_path_req = { .request_type = TA_PATH_UPDATE,
@@ -1078,7 +1162,6 @@ void train_admin() {
 				}
 			}
 
-			// TODO: remove the reliance on having a path
 			// 4. Calculate the current velocity with ewma
 			if (train_metadata[cur_train_ind].has_path && train_metadata[cur_train_ind].last_sensor_trigger_time != 0) {
 				// TODO: this node distance call is scuffed, change it
@@ -1154,11 +1237,13 @@ void train_admin() {
 
 			// 7. update last sensor hit time and next sensor distance
 			if (train_metadata[cur_train_ind].has_path) {
+
 				train_metadata[cur_train_ind].last_sensor_trigger_time = curtime;
 				train_metadata[cur_train_ind].next_sensor_distance
 					= distance_to_next_sensor_on_path(&train_metadata[cur_train_ind].path,
 													  train_metadata[cur_train_ind].cur_loc); // mm
 			}
+			ASSERT(train_metadata[cur_train_ind].cur_loc);
 			update_track_ui(TRACK_UI_SENSOR_TRAIN, train_metadata[cur_train_ind].cur_loc->idx, cur_train);
 			break;
 		case TA_PATH_UPDATE:
@@ -1168,6 +1253,7 @@ void train_admin() {
 			}
 			cur_train_ind = train_num_to_index(req->train_num);
 			child_tid = Create(3, try_to_start_path);
+			ASSERT(train_metadata[cur_train_ind].cur_loc);
 			req->src = train_metadata[train_num_to_index(req->train_num)].cur_loc->idx;
 			// debug_printi("Got path update for train: ", req->train_num, "\r\n");
 			// debug_printi("PATHING FROM NODE WITH INDEX: ", req->src, "\r\n");
@@ -1189,10 +1275,10 @@ void train_admin() {
 			// 4. reverse instantly
 			// 5. Delay until curtime + X
 			// 6. Start the path
-			int train_start_time = Time(clock_server_tid);
+			int train_start_time = Time(clock_server_tid) + 100; // base offset to account for acceleration
 			// use backup flag to signal dijkstras the train will start ~1 second behind sensor
 			uint8_t need_back_up = 0;
-
+			ASSERT(train_metadata[cur_train_ind].cur_loc);
 			if (is_branch_ahead(train_metadata[cur_train_ind].cur_loc->idx)) {
 				train_start_time += BACK_UP_TIME;
 				need_back_up = 1;
@@ -1211,6 +1297,7 @@ void train_admin() {
 			prints("\0337\033[1;40H\033[31mTC\033[0m\0338");
 			if (train_metadata[cur_train_ind].reversing && train_metadata[cur_train_ind].path.size == 1) {
 				debug_prints("reversing train in place\n");
+				ASSERT(train_metadata[cur_train_ind].cur_loc);
 				train_metadata[cur_train_ind].cur_loc = train_metadata[cur_train_ind].cur_loc->reverse;
 				train_metadata[cur_train_ind].reversing = 0;
 				// rev immediate
@@ -1238,6 +1325,7 @@ void train_admin() {
 				// path is impossible
 				reply = 3;
 				Reply(sender, &reply, 1);
+				ASSERT(train_metadata[cur_train_ind].cur_loc);
 				train_metadata[cur_train_ind].cur_dest = train_metadata[cur_train_ind].cur_loc->idx;
 				break;
 			}
@@ -1272,6 +1360,7 @@ void train_admin() {
 			train_metadata[cur_train_ind].cur_loc = &track_nodes[req->update];
 			train_metadata[cur_train_ind].direction = 0;
 			update_sensor_attribution(train_metadata, cur_train_ind);
+			ASSERT(train_metadata[cur_train_ind].cur_loc);
 			update_track_ui(TRACK_UI_SENSOR_TRAIN, train_metadata[cur_train_ind].cur_loc->idx, req->train_num);
 			break;
 		default:

user/uart.c

@@ -130,7 +130,7 @@ enum uart_train_tx_state uart_train_tx_transition(enum uart_train_tx_state curst
 		} else {
 			// TODO: FIGURE OUT WHY WE SOMETIMES GET DOUBLE TX INTERRUPTS
 			ASSERT(interrupt == UART_TX_TRIGGERED_1);
-			// prints("additional tx interrupt\r\n");
+			prints("additional tx interrupt\r\n");
 			// ASSERT(0);
 			return UART_TRAIN_TXHI_CTS_1;
 		}
@@ -162,6 +162,9 @@ void uart_train_server_rx() {
 	enum uart_rx_states rx_state = UART_RXLO;
 	int sensor_reader_tid = 0;
 
+	int bytes_read = 0;
+	int clock_server_tid = get_tid_by_name(CLOCK_SERVER);
+
 	for (;;) {
 		enable_rx_interrupt(1);
 		Receive(&sender, rcvbuf, sizeof(struct UART_Server_Request));
@@ -183,7 +186,12 @@ void uart_train_server_rx() {
 
 		// read as many as we can
 		while (rx_state == UART_RX_READY && can_read_char(1)) {
+			if (bytes_read == 0) {
+				Delay(clock_server_tid, 4);
+			}
 			char c = UartGetc(1);
+			bytes_read += 1;
+			bytes_read %= 10;
 			ASSERT(!ring_buffer_is_full(&char_buffer));
 			ring_buffer_append(&char_buffer, c);
 		}
@@ -205,6 +213,7 @@ int is_directional_control(char byte) {
 
 void uart_term_server_rx() {
 	RegisterAs(UART_SERVER_TERM_RECEIVE);
+	int clock_server_tid = get_tid_by_name(CLOCK_SERVER);
 	int sender;
 	char rcvbuf[sizeof(struct UART_Server_Request)];
 	char reply[] = "R";
@@ -213,6 +222,7 @@ void uart_term_server_rx() {
 	enum uart_rx_states rx_state = UART_RXLO;
 
 	struct Ring_Buffer terminal_admin_buffer = create_buffer(INT);
+	uint32_t last_train_cmd_time = 0;
 
 	char setup_terminal[] = "\033[55;80r"; // scrolling region for terminal and debug
 	prints(setup_terminal);
@@ -238,9 +248,13 @@ void uart_term_server_rx() {
 			char c = UartGetc(0);
 			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);
+				uint32_t curtime = Time(clock_server_tid);
+				if (last_train_cmd_time < curtime - 20) {
+					last_train_cmd_time = curtime;
+					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);
@@ -461,8 +475,9 @@ void uart_train_server_tx_no_cts() {
 			debug_printi("Current receiving_sensor_bytes: ", receiving_sensor_bytes, "\r\n");
 			debug_printi("Error, more than 5s delay to train set, current state: ", tx_state, "\r\n");
 			debug_prints("Going back to ready state...\r\n");
-			ASSERT(0);
+			// ASSERT(0);
 			tx_state = UART_TRAIN_TX_READY;
+			receiving_sensor_bytes = 0;
 		}
 
 		if (req->request_type == UART_SEND_TRAIN) {
@@ -478,6 +493,10 @@ void uart_train_server_tx_no_cts() {
 					debug_printi("size of buffer when stop bytes added: ", char_buffer_1.size, "\r\n");
 					debug_printi("added stop byte to queue at time: ", curtime, "\r\n");
 				} else {
+					if (req->msg[i] == 133 && receiving_sensor_bytes) {
+						i++;
+						continue;
+					}
 					ring_buffer_append(&char_buffer_1, req->msg[i]);
 				}
 
@@ -507,7 +526,6 @@ void uart_train_server_tx_no_cts() {
 			}
 		} else if (req->request_type == UART_DONE_RECEIVING_SENSOR_BYTES) {
 			receiving_sensor_bytes = 0;
-			Delay(clock_server_tid, 1);
 		} else {
 			ASSERT(0);
 		}

util/debug.c

@@ -120,7 +120,7 @@ void debug_prints(char* msg) {
 }
 
 void aFailed(char* file, int line) {
-	kernel_prints("\033[1;60HASSERTION FAILED: ");
+	kernel_prints("ASSERTION FAILED: ");
 	kernel_prints(file);
 	kernel_prints(":");
 	kernel_printi(line);