// Entry point of track-server
void track_server_entry() {
RegisterAs(TRACK_SERVER);
// Initialize the track
track_init();
// Initialize the reservation list
reservation_init(train_reservations);
// So we can update our own cached copy of the switch table
int switch_courier_tid = CreateSwitchCourier(PRIORITY_HIGHEST - 1);
int sender_tid;
// Message buffer large enough to hold the largest message request
char message_buffer[32];
while (1) {
Receive(&sender_tid, message_buffer, sizeof(message_buffer));
if (sender_tid == switch_courier_tid) {
// This is an update from the switch server, record the switch that changed
// so that our cached table of switches is in sync
int reply = 0;
Reply(sender_tid, (char*)&reply, sizeof(reply));
struct switch_report* report = (struct switch_report*)message_buffer;
switch_table[(int)report->sw] = report->direction;
} else {
// API requests to the track_server
struct track_reply_message reply = {-1, -1};
int op = *(int*)message_buffer;
switch(op) {
// Handle track reservation requests
case TRACK_OP_RESERVATION: {
int reply = -1;
struct reservation_request_message* reservation_msg = (struct reservation_request_message*)message_buffer;
// Make the location be a sensor and positive offset that fits in the edge
if (normalize_location(track, switch_table, &reservation_msg->position) != -1) {
// Check if we can reserve this space for the train
int status = reservation_verify(reservation_msg->train, &(reservation_msg->position), reservation_msg->length, reservation_msg->branch_safety);
reply = status;
if (status == 0) {
// We succeeded. Reserve the track
// Find the train's reservation_node
struct reservation_node* train_r = get_reservation(train_reservations, reservation_msg->train);
if (train_r != 0) {
clear_train_reservations(train_r);
// Add new reservation
reservation_insert(train_r,
&(reservation_msg->position),
reservation_msg->length,
reservation_msg->branch_safety);
}
} else if (status == -2) {
// Reached max reservation
}
} else {
Printf("Could not normalize\r");
}
Reply(sender_tid, (char*)&reply, sizeof(reply));
} break;
case TRACK_OP_RESERVATION_STRING: {
struct reservation_request_message* reservation_msg = (struct reservation_request_message*)message_buffer;
struct reservation_node* train_r = get_reservation(train_reservations, reservation_msg->train);
char buf[300];
*buf = 0; // null termination character
int len = 0;
if (train_r != 0) {
len = reservation_print(train_r, buf);
}
Reply(sender_tid, (char*)buf, len); // +1 for null termination char
} break;
case TRACK_OP_NODE_IS_RESERVED: {
struct reservation_request_message* reservation_msg = (struct reservation_request_message*)message_buffer;
int reserved = node_is_reserved(reservation_msg->train, reservation_msg->position.node);
Reply(sender_tid, (char*)&reserved, sizeof(reserved));
} break;
// Handle requests for calculating the distance between 2 nodes
case TRACK_OP_TRACK_DISTANCE: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
if (track_msg->node1 < TRACK_MAX && track_msg->node2 < TRACK_MAX) {
reply.distance = dist_between_nodes(track_msg->node1, track_msg->node2);
}
Reply(sender_tid, (char*)&reply, sizeof(reply));
} break;
case TRACK_OP_TRACK_DISTANCE_UNTIL_GOAL: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
int distance = 0;
if (track_msg->node1 < TRACK_MAX && track_msg->node2 < TRACK_MAX) {
distance = dist_until_goal(track_msg->node1, track_msg->offset1, track_msg->node2, track_msg->offset2, track_msg->distance);
}
Reply(sender_tid, (char*)&distance, sizeof(distance));
} break;
// Handle requests for getting the next sensor on the track
case TRACK_OP_TRACK_NEXT_SENSOR: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
if (track_msg->node1 >= A1 && track_msg->node1 <= E16) {
int distance;
int next_sensor = next_sensor_node(track_msg->node1, track_msg->node2, &distance);
reply.node = next_sensor;
reply.distance = distance;
}
Reply(sender_tid, (char*)&reply, sizeof(reply));
} break;
case TRACK_OP_NORMALIZE: {
struct track_request_message *msg = (struct track_request_message*)message_buffer;
struct location loc;
loc.node = msg->node1;
loc.offset = msg->offset1;
if (normalize_location(track, switch_table, &loc) == -1) {
Reply(sender_tid, (char*)&loc, 0); // reply size 0 means error!
}
Reply(sender_tid, (char*)&loc, sizeof(loc));
} break;
// Handle requests for getting the reverse node of a given node
case TRACK_OP_TRACK_REVERSE_NODE: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
if (track_msg->node1 < TRACK_MAX) {
reply.node = track[track_msg->node1].reverse->num;
}
Reply(sender_tid, (char*)&reply, sizeof(reply));
} break;
case TRACK_OP_ROUTE: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
int train = track_msg->train;
int start_node_num = track_msg->node1;
int end_node_num = track_msg->node2;
short path[TRACK_MAX];
int num_nodes = 0;
if (IS_SENSOR(start_node_num) && IS_SENSOR(end_node_num)) {
struct track_node *start_node = &track[start_node_num];
struct track_node *end_node = &track[end_node_num];
mark_path(train, track, start_node, end_node);
if (end_node->routing_info.visited == 1) {
// we have a path!
// store all nodes, from bottom up
struct track_node *cur_node = end_node;
struct location loc;
while (cur_node->routing_info.previous != 0 && cur_node != start_node) {
node_to_location(cur_node, &loc);
path[num_nodes] = loc.node;
num_nodes++;
if (cur_node->routing_info.previous == cur_node->reverse) {
// the next node is the reverse of this node, so we squeeze in a "REVERSE" in the path
path[num_nodes] = -99; // magic number for REVERSE commands
num_nodes++;
}
cur_node = cur_node->routing_info.previous;
}
// we didn't store the beginning node in our path, so we should do that now
if (cur_node == start_node) {
node_to_location(start_node, &loc);
path[num_nodes] = loc.node;
num_nodes++;
}
} else {
Printf("Could not find route! num_nodes = %d\r", num_nodes);
}
}
Reply(sender_tid, (char*)path, sizeof(short) * num_nodes);
} break;
case TRACK_OP_SWITCH_DIRECTION: {
struct track_request_message* track_msg = (struct track_request_message*)message_buffer;
int switch_node = track_msg->node1;
int next_node = track_msg->node2;
AssertF(track[switch_node].type == NODE_BRANCH, "TrackSwitchDirection not given a switch node! Given node num %d, type %d", track[switch_node].num, track[switch_node].type);
int reserved = node_is_reserved(track_msg->train, switch_node);
if (reserved) {
// Printf("Attempting to switch %s if reserved .. \r", track[switch_node].name);
int direction = -1;
if (track[switch_node].edge[DIR_CURVED].dest == &(track[next_node])) {
direction = SWITCH_CURVED;
} else {
direction = SWITCH_STRAIGHT;
}
SwitchSetDirection(track[switch_node].num, direction, WhoIs(SWITCH_SERVER));
switch_table[track[switch_node].num] = direction;
}
Reply(sender_tid, (char*)&reserved, sizeof(reserved));
} break;
default:
AssertF(0, "Invalid message %d to the track server from %d", op, sender_tid);
Reply(sender_tid, (char*)&reply, sizeof(reply));
break;
}
}
}
Assert("Track server is quitting");
Exit();
}
static
VOID
InputParserpParseCommand
(
IN STRING buffer,
IN INT bufferLength
)
{
INT arg1 = 0;
CHAR arg1Buffer[12];
arg1Buffer[11] = '\0';
INT arg2 = 0;
CHAR arg2Buffer[12];
arg2Buffer[11] = '\0';
CHAR token[12];
INT read = RtStrConsumeToken(&buffer, token, sizeof(token));
if (read == 0)
{
return;
}
if (RtStrEqual(token, "tr"))
{
read = RtStrConsumeToken(&buffer, arg1Buffer, sizeof(arg1Buffer));
if (read && RT_SUCCESS(RtAtoi(arg1Buffer, &arg1)))
{
read = RtStrConsumeToken(&buffer, arg2Buffer, sizeof(arg2Buffer));
if (read && RT_SUCCESS(RtAtoi(arg2Buffer, &arg2)))
{
if (RtStrIsWhitespace(buffer))
{
TrainSetSpeed(arg1, arg2);
}
}
}
}
else if (RtStrEqual(token, "sw"))
{
read = RtStrConsumeToken(&buffer, arg1Buffer, sizeof(arg1Buffer));
if (read && RT_SUCCESS(RtAtoi(arg1Buffer, &arg1)))
{
SWITCH_DIRECTION direction = SwitchCurved;
read = RtStrConsumeToken(&buffer, arg2Buffer, sizeof(arg2Buffer));
if (read == 1 && InputParserpGetSwitchDirection(arg2Buffer[0], &direction))
{
if (RtStrIsWhitespace(buffer))
{
SwitchSetDirection(arg1, direction);
}
}
}
}
else if (RtStrEqual(token, "rv"))
{
read = RtStrConsumeToken(&buffer, arg1Buffer, sizeof(arg1Buffer));
if (read && RT_SUCCESS(RtAtoi(arg1Buffer, &arg1)))
{
if (RtStrIsWhitespace(buffer))
{
TrainReverse(arg1);
}
}
}
else if(RtStrEqual(token, "rt"))
{
read = RtStrConsumeToken(&buffer, arg1Buffer, sizeof(arg1Buffer));
if(read && RT_SUCCESS(RtAtoi(arg1Buffer, &arg1)))
{
if(RtStrIsWhitespace(buffer))
{
VERIFY(SUCCESSFUL(TrainDestinationForever(arg1)));
}
}
}
else if (RtStrEqual(token, "go"))
{
read = RtStrConsumeToken(&buffer, arg1Buffer, sizeof(arg1Buffer));
if (read && RT_SUCCESS(RtAtoi(arg1Buffer, &arg1)))
{
read = RtStrConsumeToken(&buffer, arg2Buffer, sizeof(arg2Buffer));
if (read && RT_SUCCESS(RtAtoi(&arg2Buffer[1], &arg2)))
{
if(RtStrIsWhitespace(buffer))
{
SENSOR sensor = { arg2Buffer[0], arg2 };
LOCATION location;
location.node = TrackFindSensor(&sensor);
location.distancePastNode = 0;
VERIFY(SUCCESSFUL(TrainDestinationOnce(arg1, &location)));
}
}
}
}
else if (RtStrEqual(token, "q"))
{
if (RtStrIsWhitespace(buffer))
{
Shutdown();
}
}
}
void velocity_task_entry() {
TRAIN = 36;
// int train_tid = WhoIs(TRAIN_COMMAND);
int train_controller_tid = WhoIs(TRAIN_CONTROLLER);
// int uart_tid = WhoIs(UART_SERVER);
int sensor_tid = WhoIs(SENSOR_SERVER);
int clock_tid = WhoIs(CLOCK_SERVER);
int switch_tid = WhoIs(SWITCH_SERVER);
int curspeed = 14;
int time;
Delay(10 SEC, clock_tid);
TrainControllerSpeed(TRAIN, 5, train_controller_tid);
Delay(10 SEC, clock_tid);
for ( ; curspeed >= 10 ; --curspeed) {
Printf("Running train %d at speed %d\r", TRAIN, curspeed);
TrainControllerSpeed(TRAIN, curspeed, train_controller_tid);
Delay(80 SEC, clock_tid);
SwitchSetDirection(16, SWITCH_STRAIGHT, switch_tid);
Printf("Switched sw16\r", TRAIN, curspeed);
Delay(60 SEC, clock_tid);
SwitchSetDirection(15, SWITCH_STRAIGHT, switch_tid);
Printf("Switched sw15\r", TRAIN, curspeed);
Delay(80 SEC, clock_tid);
SwitchSetDirection(6, SWITCH_STRAIGHT, switch_tid);
SwitchSetDirection(9, SWITCH_STRAIGHT, switch_tid);
Printf("Switched sw6 and sw9\r", TRAIN, curspeed);
Delay(60 SEC, clock_tid);
TrainControllerSpeed(TRAIN, curspeed, train_controller_tid);
int sensor_node;
do {
SensorGetNextTriggered(&sensor_node, &time, sensor_tid);
} while (sensor_node != A4);
TrainControllerSpeed(TRAIN, 0, train_controller_tid);
SwitchSetDirection(16, SWITCH_CURVED, switch_tid);
SwitchSetDirection(15, SWITCH_CURVED, switch_tid);
SwitchSetDirection(6, SWITCH_CURVED, switch_tid);
SwitchSetDirection(9, SWITCH_CURVED, switch_tid);
Delay(5 SEC, clock_tid);
}
// TrainControllerSpeed(TRAIN, 14, train_controller_tid);
// Delay(5 SEC, clock_tid);
//
// curspeed = 13;
// for ( ; curspeed >= 8 ; --curspeed) {
// Printf("Running train %d at speed %d (decelerating)\r", TRAIN, curspeed);
//
// TrainControllerSpeed(TRAIN, curspeed, train_controller_tid);
//
// Delay(60 SEC, clock_tid);
// SwitchSetDirection(16, SWITCH_STRAIGHT, switch_tid);
// Printf("Switched sw16\r", TRAIN, curspeed);
//
// Delay(45 SEC, clock_tid);
// SwitchSetDirection(15, SWITCH_STRAIGHT, switch_tid);
// Printf("Switched sw15\r", TRAIN, curspeed);
//
// Delay(60 SEC, clock_tid);
// SwitchSetDirection(6, SWITCH_STRAIGHT, switch_tid);
// SwitchSetDirection(9, SWITCH_STRAIGHT, switch_tid);
// Printf("Switched sw6 and sw9\r", TRAIN, curspeed);
//
// Delay(45 SEC, clock_tid);
// TrainControllerSpeed(TRAIN, curspeed, train_controller_tid);
//
// int sensor_node;
// do {
// SensorGetNextTriggered(&module, &sensor, &time, sensor_tid);
// sensor_node = SENSOR_TO_NODE(module,sensor);
// } while (sensor_node != A4);
//
// SwitchSetDirection(16, SWITCH_CURVED, switch_tid);
// SwitchSetDirection(15, SWITCH_CURVED, switch_tid);
// SwitchSetDirection(6, SWITCH_CURVED, switch_tid);
// SwitchSetDirection(9, SWITCH_CURVED, switch_tid);
//
// }
//
// TrainControllerSpeed(TRAIN, 0, train_controller_tid);
Printf("DONE!\r");
Exit();
}
