Working 24 controls + attribution

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: