Skip to content
Snippets Groups Projects

Working 24 controls + attribution

Merged Franklin Kaifeng Zheng requested to merge fkzheng-24-sensor-attribution into main
+ 106
17
@@ -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:
Loading