merge franklins code

user/term_admin.c

@@ -11,6 +11,9 @@ struct Command_Token get_next_token(char* command, int* offset) {
 	} else if (command[(*offset)] == 't' && command[(*offset) + 1] == 'r') {
 		token.type = COMMAND_TR;
 		*offset += 2;
+	} else if (command[(*offset)] == 'r' && command[(*offset) + 1] == 'v' && command[(*offset) + 2] == 'i') {
+		token.type = COMMAND_RV_I;
+		*offset += 3;
 	} else if (command[(*offset)] == 'r' && command[(*offset) + 1] == 'v') {
 		token.type = COMMAND_RV;
 		*offset += 2;
@@ -79,6 +82,8 @@ void terminal_admin() {
 			send_speed_command(tokens[1].value, tokens[2].value);
 		} else if (tokens[0].type == COMMAND_RV && tokens[1].type == COMMAND_INT && tokens[2].type == COMMAND_EOL) {
 			send_reverse_command(tokens[1].value);
+		} else if (tokens[0].type == COMMAND_RV_I && tokens[1].type == COMMAND_INT && tokens[2].type == COMMAND_EOL) {
+			reverse_instantly(tokens[1].value);
 		} else if (tokens[0].type == COMMAND_SW && tokens[1].type == COMMAND_INT && tokens[2].type == COMMAND_CHAR
 				   && tokens[3].type == COMMAND_EOL) {
 			char switch_num[1] = { tokens[1].value };

user/term_admin.h

@@ -15,6 +15,7 @@ enum command_token_type {
 	COMMAND_SET_TRAIN,
 	COMMAND_PATHFIND,
 	COMMAND_CALIBRATE,
+	COMMAND_RV_I,
 };
 
 struct Command_Token {

user/track_admin.c

@@ -624,3 +624,8 @@ void unpark_train(uint8_t node, int track_admin_tid) {
 	char reply;
 	Send(track_admin_tid, (char*)&req, sizeof(struct Track_Request), &reply, 1);
 }
+
+int is_branch_ahead(uint8_t node_idx) {
+	ASSERT(node_idx <= 79);
+	return (track_nodes[node_idx].edge[DIR_AHEAD].dest->type == NODE_BRANCH);
+}

user/track_admin.h

@@ -52,5 +52,7 @@ void park_train(uint8_t node, int track_admin_tid);
 void unpark_train(uint8_t node, int track_admin_tid);
 
 uint8_t get_nodes_for_parking(uint8_t node, uint8_t* buf);
+int is_branch_ahead(uint8_t node_idx);
+
 extern const uint32_t PARKING_SPOT;
 #endif
\ No newline at end of file

user/train_control.c

@@ -4,6 +4,7 @@ const char TRAIN_STOP = 0;
 const char TRAIN_MAX_SPEED = 14;
 const char TRAIN_REVERSE = 15;
 const char TRAIN_LIGHTS_ON = 16;
+const uint16_t BACK_UP_TIME = 300;
 // trains { 1, 2, 24, 58, 74, 78 };
 
 const char g_train_speeds[6][3]
@@ -76,6 +77,20 @@ void send_reverse_command(char train_num) {
 	Send(worker_id, (char*)&train_worker_request, sizeof(struct Train_Worker_Request), reply_buf, 1);
 }
 
+void reverse_instantly(char train_num) {
+	char reply_buf[1];
+	struct Train_Worker_Request train_worker_request;
+	char train_worker_name[] = "TRAIN_WORKER_XX";
+	if (train_num != 1 && train_num != 2 && train_num != 24 && train_num != 58 && train_num != 74 && train_num != 78) {
+		prints("\r\nInvalid Train Number");
+		return;
+	}
+	get_train_worker_name(train_num, train_worker_name);
+	int worker_id = get_tid_by_name(train_worker_name);
+	train_worker_request.request_type = RV_I;
+	Send(worker_id, (char*)&train_worker_request, sizeof(struct Train_Worker_Request), reply_buf, 1);
+}
+
 void set_path2(uint8_t train, uint8_t dest, uint64_t offset) {
 	int train_admin_tid = get_tid_by_name(TRAIN_ADMIN);
 	char replybuf[1];
@@ -123,7 +138,7 @@ void train_worker() {
 		struct Train_Worker_Request* request = (struct Train_Worker_Request*)buf;
 		if (request->request_type == TR) {
 			if (!(request->speed >= TRAIN_STOP && request->speed <= TRAIN_MAX_SPEED)) {
-				debug_prints("invalid speed");
+				debug_prints("invalid speed\r\n");
 				continue;
 			}
 			char cmd[] = { (request->speed == 0 ? TRAIN_STOP + TRAIN_LIGHTS_ON : request->speed), train_num, '\0' };
@@ -145,6 +160,16 @@ void train_worker() {
 			char set_speed[] = { cur_speed, train_num, '\0' };
 			Delay(clock_server_tid, 10);
 			send_train_command(uart_train_tx_tid, set_speed);
+		} else if (request->request_type == RV_I) {
+			// reverse immediately and turn on the lights
+			char reverse[] = { TRAIN_REVERSE, train_num, '\0' };
+			send_train_command(uart_train_tx_tid, reverse);
+			Delay(clock_server_tid, 10);
+			char stop_cmd[] = { TRAIN_STOP + TRAIN_LIGHTS_ON, train_num, '\0' };
+			send_train_command(uart_train_tx_tid, stop_cmd);
+		} else {
+			debug_printi("INVALID REQUEST: ", request->request_type, "\r\n");
+			ASSERT(0);
 		}
 	}
 }
@@ -428,7 +453,7 @@ void handle_possible_stop(struct Train_Info* train_metadata,
 	}
 }
 
-int check_which_train_triggered_sensor(struct Train_Info train_metadata[], int sensor_id, uint32_t curtime) {
+int check_which_train_triggered_sensor(struct Train_Info train_metadata[], int sensor_id) {
 	uint8_t possible_train = 0;
 	uint32_t min_arrival_time = UINT32_MAX;
 	// must have path
@@ -447,18 +472,6 @@ int check_which_train_triggered_sensor(struct Train_Info train_metadata[], int s
 		}
 	}
 
-	// debug time delta for the train running over the sensor
-	if (possible_train) {
-		int delta = curtime > min_arrival_time ? curtime - min_arrival_time : min_arrival_time - curtime;
-		char prefix[50];
-		char train_num[5];
-		itos(possible_train, train_num, 1);
-		str_copy("Selected train ", prefix);
-		str_cat(prefix, train_num);
-		str_cat(prefix, " with delta ");
-		debug_printi(prefix, delta, "\r\n");
-	}
-
 	// if no train expected to go over the sensor, we ignore and return 0
 	return possible_train;
 }
@@ -633,6 +646,88 @@ void _update_train_ui(struct Train_Info train_metadata[6]) {
 	}
 }
 
+void back_up_train(int clock_server_tid, struct Train_Info train) {
+	// time to back up calculate 10cm~ish at low speed
+	uint16_t time_offset = 50; // adjust for delay it takes for speed command to execute
+	// time in ticks
+	uint32_t ticks_to_back_up = ((100 * 100) / train.train_velocity[0]) + time_offset;
+	debug_printi("TICKS TO BACK UP: ", ticks_to_back_up, "\r\n");
+
+	reverse_instantly(train.train_num);
+	Delay(clock_server_tid, 10);
+	send_speed_command(train.train_num, g_train_speeds[train_num_to_index(train.train_num)][0]);
+	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);
+}
+
+struct start_path_request {
+	struct Train_Info train_metadata;
+	int possibly_new_dest;
+	int train_start_time;
+};
+void _start_train_path() {
+	int sender;
+	char reply[1] = { 0 };
+	char rcv_buf[sizeof(struct start_path_request)];
+	Receive(&sender, rcv_buf, sizeof(struct start_path_request));
+	Reply(sender, reply, 1);
+	debug_prints("EXECUTING PATH START LOGIC\r\n");
+
+	int track_admin_tid = get_tid_by_name(TRACK_ADMIN);
+	int clock_server_tid = get_tid_by_name(CLOCK_SERVER);
+	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);
+
+	// Back up train if nessecary
+
+	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);
+		// Start the train at the time the pathfinding expected it to
+		debug_printi("DONE BACKING UP, STARTING TRAIN AT: ", req->train_start_time, "\r\n");
+		DelayUntil(clock_server_tid, req->train_start_time);
+		debug_prints("Starting from backed up location\r\n");
+	}
+
+	flip_next_switches(&req->train_metadata.path);
+
+	unpark_train(req->train_metadata.parking_spot, track_admin_tid);
+
+	/*
+	Short move logic:
+		1. A short move is defined as a path that has 1 or 2 sensors on it (including the beginning and end
+	sensors)
+		2. We require the train to hit at least 1 sensor
+	*/
+	if (sensors_left(&req->train_metadata.path) <= 2) {
+		debug_prints("Short move: going on low speed\r\n");
+		send_speed_command(req->train_metadata.train_num,
+						   g_train_speeds[train_num_to_index(req->train_metadata.train_num)][0]);
+	} else {
+		debug_prints("Going on medium speed\r\n");
+		send_speed_command(req->train_metadata.train_num,
+						   g_train_speeds[train_num_to_index(req->train_metadata.train_num)][1]);
+	}
+
+	Exit();
+}
+
+void start_path(struct Train_Info train_metadata, int possibly_new_dest, int train_start_time) {
+	//
+	struct start_path_request req = { .train_metadata = train_metadata,
+									  .possibly_new_dest = possibly_new_dest,
+									  .train_start_time = train_start_time };
+	char reply[1];
+	int child_id = Create(2, _start_train_path);
+	Send(child_id, (char*)&req, sizeof(struct start_path_request), reply, 1);
+}
+
 void train_admin() {
 	RegisterAs(TRAIN_ADMIN);
 
@@ -724,7 +819,7 @@ void train_admin() {
 				Send(calibration_task_tid, (char*)&calibration_request, sizeof(struct Calibration_Request), &reply, 1);
 			}
 			// determine which train triggered the sensor
-			cur_train = check_which_train_triggered_sensor(train_metadata, req->update, curtime);
+			cur_train = check_which_train_triggered_sensor(train_metadata, req->update);
 			if (!cur_train) {
 				break;
 			}
@@ -819,9 +914,9 @@ void train_admin() {
 				}
 
 				// TODO: move this to its own section of the screen
-				char prefix[] = "Sensor:  ";
-				prefix[8] = 'A' + (req->update) / 16;
-				debug_printi(prefix, (req->update % 16) + 1, "\r\n");
+				// char prefix[] = "Sensor:  ";
+				// prefix[8] = 'A' + (req->update) / 16;
+				// debug_printi(prefix, (req->update % 16) + 1, "\r\n");
 				// debug_printi_maybe_negative("Prediction was off by: ", time_error, "ticks \r\n");
 				// debug_printi_maybe_negative("Distance error: ", distance_error, "mm \r\n");
 				train_metadata[cur_train_ind].time_error = time_error;
@@ -877,10 +972,20 @@ void train_admin() {
 			cur_train_ind = train_num_to_index(req->train_num);
 			train_metadata[cur_train_ind].path = create_buffer(CHAR);
 			get_reserved_segments(blocked);
+			// if we are in front of a branch:
+			// 1. Compute Dijkstra_tc2 for X seconds in the future
+			// 2. reverse instantly
+			// 3. travel 10cm
+			// 4. reverse instantly
+			// 5. Delay until curtime + X
+			// 6. Start the path
+			int train_start_time = is_branch_ahead(train_metadata[cur_train_ind].cur_loc->idx)
+									   ? Time(clock_server_tid) + BACK_UP_TIME
+									   : Time(clock_server_tid);
 			uint8_t possibly_new_dest = dijkstra_tc2(req->src,
 													 req->dest,
 													 train_metadata[cur_train_ind].train_velocity[1],
-													 Time(clock_server_tid),
+													 train_start_time,
 													 blocked,
 													 train_metadata[cur_train_ind].parking_spot,
 													 &train_metadata[cur_train_ind].path,
@@ -896,6 +1001,7 @@ void train_admin() {
 				Reply(sender, &reply, 1);
 				break;
 			}
+
 			if (ring_buffer_is_empty(&train_metadata[cur_train_ind].path)
 				|| train_metadata[cur_train_ind].path.size == 1) {
 				reply = 0;
@@ -904,9 +1010,9 @@ void train_admin() {
 			}
 			reply = 1;
 			Reply(sender, &reply, 1);
-			debug_printi("Parking info for train: ", req->train_num, "\r\n");
-			park_train(possibly_new_dest, track_admin_tid);
-			unpark_train(train_metadata[cur_train_ind].parking_spot, track_admin_tid);
+
+			start_path(train_metadata[cur_train_ind], possibly_new_dest, train_start_time);
+
 			train_metadata[cur_train_ind].parking_spot = possibly_new_dest;
 			train_metadata[cur_train_ind].has_path = 1;
 			train_metadata[cur_train_ind].last_sensor_trigger_time = 0;
@@ -916,21 +1022,6 @@ void train_admin() {
 			train_metadata[cur_train_ind].prev_loc = NULL;
 			train_metadata[cur_train_ind].cur_loc = NULL;
 			train_metadata[cur_train_ind].cur_dest = req->dest;
-			// TODO: display the destinations of each train
-
-			/*
-			Short move logic:
-				1. A short move is defined as a path that has 1 or 2 sensors on it (including the beginning and end
-			sensors)
-				2. We require the train to hit at least 1 sensor
-			*/
-			if (sensors_left(&train_metadata[cur_train_ind].path) <= 2) {
-				debug_prints("Short move: going on low speed\r\n");
-				send_speed_command(req->train_num, g_train_speeds[cur_train_ind][0]);
-			} else {
-				debug_prints("Going on medium speed\r\n");
-				send_speed_command(req->train_num, g_train_speeds[cur_train_ind][1]);
-			}
 			break;
 		case TA_LOOP_STOP:
 			debug_printi("Stopping train at ", req->dest, "\r\n");

user/train_control.h

@@ -15,6 +15,7 @@
 enum train_worker_request_type {
 	TR,
 	RV,
+	RV_I,
 };
 
 struct Train_Worker_Request {
@@ -70,6 +71,7 @@ int turn_switches(char* switch_nums, char* directions, uint8_t sw_count);
 
 void send_speed_command(char train_num, char speed);
 void send_reverse_command(char train_num);
+void reverse_instantly(char train_num);
 void set_path2(uint8_t train, uint8_t dest, uint64_t offset);
 void calibrate_train(char train);
 void set_cur_train(char train);