int main(int argc, char** argv)
{
// used to set execution rate to 60hz
clock_t prevtime;
clock_t curtime;
int delay = 0;
// initialize graphics
WINDOW *scrn; // pointer to screen object for ncurse (not sure if required)
scrn = initscr(); // initalized the screen
clear(); // clears the screen
timeout(3); // sets getch() to wait 50ms
srand(time(NULL)); // need random generator for one of the opcodes implemented
chip8 myChip8;
// initalize myChip8
initializeMyChip(&myChip8);
initializeScreen(&myChip8);
// load program into memory, exit if not loaded correctly
if(loadProgram(&myChip8, "./ROMS/PONG") == 1){
return 0;
}
noecho(); // curses function, turns off echo of keyboard input
cbreak(); // curses, reports keystrokes without waiting for return key
// should probably come up with a better end condition but loops until pc == 0
while(myChip8.pc != 0){
prevtime = clock(); // stores the time at start of execution cycle
executeCycle(&myChip8); // execute one cycle
// Checks if draw flag is set to 1, if not no changes need to be drawn
if(myChip8.draw != 0){
draw(&myChip8);
myChip8.draw = 0;
}
// gets any changes to input
getInput(&myChip8);
// slows down execution to 60hz
curtime = clock(); // gets clock at end of execution cycle
// if execution time was greater than 17 ms (1sec/60) than continue if less
// sleep so delay + execution time is equal to 17 ms
if(((curtime * 1000 / CLOCKS_PER_SEC) - (prevtime * 1000 / CLOCKS_PER_SEC)) < 17){
delay = 17 - ((curtime * 1000 / CLOCKS_PER_SEC) -
(prevtime * 1000 / CLOCKS_PER_SEC));
usleep(delay );
}
}
}
void MemoryControl::wakeup () {
// execute everything
executeCycle();
m_idleCount--;
if (m_idleCount <= 0) {
m_awakened = 0;
} else {
// Reschedule ourselves so that we run every memory cycle:
g_eventQueue_ptr->scheduleEvent(this, m_mem_bus_cycle_multiplier);
}
}
void TOea_Planner::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal_msg)
{
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Unable to compute path.");
return;
}
planner_state.data = hardware::BUSY; //PLANNING;
state_pub_.publish(planner_state);
ROS_DEBUG_NAMED(logger_name_, "New Goal received on topic");
// get world pose from msg
Astar_.goal_world_pose_.x = goal_msg->pose.position.x;
Astar_.goal_world_pose_.y = goal_msg->pose.position.y;
Astar_.goal_world_pose_.yaw = tf::getYaw(goal_msg->pose.orientation);
oea_msgs::Oea_path oea_path; // plan variable
// oea_path.path.poses.clear();
//check validity:
if (Astar_.goal_world_pose_.yaw!=Astar_.goal_world_pose_.yaw) //if nan
{
ROS_ERROR_STREAM_NAMED(logger_name_, "Invalig Goal: yaw is " << to_degrees(Astar_.goal_world_pose_.yaw));
//else just publish the blank path
oea_path_pub_.publish(oea_path); //publishing 0 poses will cause the controller to stop following the previous path
visual_path_pub_.publish(oea_path.path); // publish nav_msgs/Path to view path on rviz
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
return;
}
//convert it to grid coord
Astar_.ConvertWorlCoordToMatrix(Astar_.goal_world_pose_.x, Astar_.goal_world_pose_.y, Astar_.goal_world_pose_.yaw , Astar_.goal_grid_pose_.x, Astar_.goal_grid_pose_.y, Astar_.goal_grid_pose_.z);
std::string error_str;
// check if goal is valid
//error_str = "GoalCb: check grid pose val";
if (Astar_.is_valid_point(Astar_.goal_grid_pose_,error_str))
{
//get path from the Astar...
executeCycle(error_str, oea_path);
}
else
{
ROS_WARN_STREAM_NAMED(logger_name_, error_str);
// no need to clear Grid, because Astar was not called
// publish empty plan to stop the robot in case it's moving and received a new point...
oea_path_pub_.publish(oea_path);
visual_path_pub_.publish(oea_path.path);
}
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
}
void vm(void) {
//Declaring file pointers
FILE* ifp;
FILE* ofp;
ifp = fopen("parserout.txt", "r");
ofp = fopen("vmout.txt", "w");
ofp2 = fopen("vmout2.txt", "w");
ofp3 = fopen("vmout3.txt", "w");
//Declraing index and stack
int i=0;
int stack[MAX_STACK_HEIGHT] = {0};
//Assigning values from homework specifications
int SP = 0;
int BP = 1;
int PC = 0;
int IR = 0;
//arrayStruct is a filled array of instruction structs, containing the op, l, and m.
//irStruct is a single reference of the instruction struct
instruction arrayStruct[CODE_SIZE];
instruction *irStruct;
//Scans in the instructions line by line until end of file.
//Also prints out first part of program to screen:
//The Line, OP (by name not number), L, and M.
fprintf(ofp, "Line\tOP\tL\tM\n");
fprintf(ofp2, "Line\tOP\tL\tM\n");
while (fscanf(ifp, "%d %d %d", &arrayStruct[i].op, &arrayStruct[i].l, &arrayStruct[i].m) != EOF) {
fprintf(ofp, "%d\t%s\t%d\t%d\n", i, opcodes[arrayStruct[i].op], arrayStruct[i].l, arrayStruct[i].m);
fprintf(ofp2, "%d\t%s\t%d\t%d\n", i, opcodes[arrayStruct[i].op], arrayStruct[i].l, arrayStruct[i].m);
i++;
}
//Prints second part of program to screen, starting with
//the pc, bp, sp, and stack headers.
fprintf(ofp, "\n\n");
fprintf(ofp, "\t\t\t\tpc \tbp \tsp \tstack\n");
fprintf(ofp, "Initial values\t\t\t%d \t%d \t%d \n", PC, BP, SP);
//fprintf(ofp3, "\n\n");
fprintf(ofp3, "\t\t\t\tpc \tbp \tsp \tstack\n");
fprintf(ofp3, "Initial values\t\t\t%d \t%d \t%d \n", PC, BP, SP);
//Prints out the second part of the program. Starts and keeps going until BP = 0, representing the program stopping.
while (BP!=0) {
//Prints out the PC, OP, L, and M. This is also the "Fetch Cycle"
irStruct=&arrayStruct[PC];
fprintf(ofp, "%d\t%s \t%d \t%d",PC, opcodes[irStruct->op], irStruct->l, irStruct->m);
fprintf(ofp3, "%d\t%s \t%d \t%d",PC, opcodes[irStruct->op], irStruct->l, irStruct->m);
PC++;
//Sends to giant switch function, mimicking the execute cycle
executeCycle(irStruct, stack, &SP, &BP, &PC);
fprintf(ofp, "\t%d \t%d \t%d \t", PC, BP, SP);
fprintf(ofp3, "\t%d \t%d \t%d \t", PC, BP, SP);
//Calls function that prints out each stack frame and its values, separated by |
printStackFrame(stack, SP, BP, ofp);
fprintf(ofp, "\n");
fprintf(ofp3, "\n");
}
fclose(ifp);
fclose(ofp);
fclose(ofp2);
fclose(ofp3);
}
