void turn(float degree, char side, int speed){
float distance_remaining = get_circle_arc_length(degree, __DISTANCE_BETWEEN_WHEELS__/2);
int motor, reverse_motor, encoder;
if(side=='L'){
motor = MOTOR_RIGHT;
reverse_motor = MOTOR_LEFT;
encoder = ENCODER_RIGHT;
}else if(side=='R'){
motor = MOTOR_LEFT;
reverse_motor = MOTOR_RIGHT;
encoder = ENCODER_LEFT;
}
MOTOR_SetSpeed(motor, speed);
MOTOR_SetSpeed(reverse_motor, -speed);
while(distance_remaining >= 0){
THREAD_MSleep(250);
int encoder_read = ENCODER_Read(encoder);
distance_remaining = distance_remaining - (encoder_read * __TURN_DISTANCE_PER_ENCODER__);
}
stop_and_wait(10);
reset_encoders();
}
void move(float distance, int motor_speed){
float distance_remaining = distance;
int encoder_error_right = 0, encoder_error_left = 0, total_error_right = 0, total_error_left = 0;
bool move_is_done = false;
set_motors_speed(motor_speed);
while(!move_is_done){
if(read_bumpers()){
stop_and_wait(250);
}else{
THREAD_MSleep(__PID_TIME_INTERVAL__);
int encoder_read_right = ENCODER_Read(ENCODER_RIGHT);
int encoder_read_left = ENCODER_Read(ENCODER_LEFT);
total_error_right += encoder_error_right;
total_error_left += encoder_error_left;
encoder_error_right = pid(encoder_read_right, MOTOR_RIGHT, EXPECTED_ENCODER_READ_RIGHT, encoder_error_right, total_error_right, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
encoder_error_left = pid(encoder_read_left, MOTOR_LEFT, EXPECTED_ENCODER_READ_LEFT, encoder_error_left, total_error_left, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__);
if(distance_remaining < 0){
move_is_done = true;
}
}
}
}
int main(int argc, char * argv[]){
if(geteuid() != 0){
fprintf(stderr, "This program requires root.\n");
exit(1);
}
flags f = {0};
f.heap_tree_height = 8;
int opt;
while((opt = getopt(argc, argv, "p:ti:d:hv")) != -1){
switch(opt){
case 'p':
f.process = (pid_t) atoi(optarg);
break;
case 't':
f.build_heap_tree = 1;
break;
case 'i':
f.heap_tree_height = (size_t) atoi(optarg);
break;
case 'd':
f.dump_file_name = optarg;
break;
case 'h':
print_help();
exit(0);
break;
case 'v':
f.verbose = 1;
break;
default:
print_help();
exit(0);
}
}
if(optind == 1){
print_help();
exit(0);
}
if(f.process == 0){
puts("Missing option -p (process ID)");
exit(1);
}
//get maps and mem files from /proc
char * proc_map_path;
char * proc_mem_path;
asprintf(&proc_map_path, "/proc/%d/maps", f.process);
asprintf(&proc_mem_path, "/proc/%d/mem" , f.process);
FILE * proc_map_file = fopen(proc_map_path, "r");
FILE * proc_mem_file = fopen(proc_mem_path, "r");
//hash tree section
if(f.build_heap_tree == 1){
char * proc_map_heap_line;
hash_tree_node * first_hash_tree = NULL;
hash_tree_node * second_hash_tree = NULL;
proc_map_heap_info heap_info_first ={0};
proc_map_heap_info heap_info_second ={0};
uint8_t * first_chunk = NULL;
uint8_t * second_chunk = NULL;
//take the first snapshot of the process
stop_and_wait(f.process);
fflush(proc_map_file);
proc_map_heap_line = proc_map_find_heap(proc_map_file);
parse_proc_map_heap(proc_map_heap_line, &heap_info_first);
first_chunk = load_heap_from_file(proc_mem_file, heap_info_first.start_address, heap_info_first.size);
printf("%sSnapshot 1:%s\n", RED, NO_COLOR);
print_heap_info(&heap_info_first);
//unless we close the files we will get old data (there's probably a better solution to this)
fclose(proc_mem_file);
fclose(proc_map_file);
free(proc_map_heap_line);
proc_map_heap_line = NULL;
countinue_stopped_process(f.process);
//prompt user for second snapshot
printf("\n%sPress enter to take second snapshot.%s\n", GREEN, NO_COLOR);
getchar();
//take the second snapshot of the process
proc_map_file = fopen(proc_map_path, "r");
proc_mem_file = fopen(proc_mem_path, "r");
stop_and_wait(f.process);
proc_map_heap_line = proc_map_find_heap(proc_map_file);
parse_proc_map_heap(proc_map_heap_line, &heap_info_second);
second_chunk = load_heap_from_file(proc_mem_file,heap_info_second.start_address, heap_info_second.size);
printf("%sSnapshot 2:%s\n", RED, NO_COLOR);
print_heap_info(&heap_info_second);
free(proc_map_heap_line);
proc_map_heap_line = NULL;
countinue_stopped_process(f.process);
//If the first snapshot used less memory than the first resize the first chunk
//so our hashing algorythem still works. The else if probably won't happen.
if(heap_info_second.size > heap_info_first.size){
heap_info_first.size = heap_info_second.size;
first_chunk = realloc(first_chunk, heap_info_second.size);
}
else if(heap_info_second.size < heap_info_first.size){
heap_info_second.size = heap_info_first.size;
second_chunk = realloc(second_chunk, heap_info_first.size);
}
//generate the hash_trees
first_hash_tree = generate_hash_tree(first_chunk,
heap_info_first.size,
0,
f.heap_tree_height);
free(first_chunk);
first_chunk = NULL;
second_hash_tree = generate_hash_tree(second_chunk,
heap_info_second.size,
0,
f.heap_tree_height);
free(second_chunk);
second_chunk = NULL;
int same_tree = diff_hash_tree(first_hash_tree, second_hash_tree);
//handle verbose hash printing
if(f.verbose){
printf("\n%sFIRST%s\n", YELLOW, NO_COLOR);
print_hash_tree(first_hash_tree,0);
printf("\n%sSECOND%s\n", YELLOW, NO_COLOR);
print_hash_tree(second_hash_tree,0);
//diff the hash trees and print the solution
if(same_tree!=1){
printf("\n%sDIFF%s\n", YELLOW, NO_COLOR);
print_hash_tree(second_hash_tree, 0);
}else{
second_hash_tree = NULL;
puts("SAME TREE!!!");
}
}
//start visualization
draw_heap_visualization(second_hash_tree, heap_info_second.size, f.heap_tree_height);
}
//dump a single heap snapshot to a file
if(f.dump_file_name != NULL){
proc_map_heap_info heap_info = {0};
char * proc_map_heap_line = proc_map_find_heap(proc_map_file);
parse_proc_map_heap(proc_map_heap_line, &heap_info);
FILE * heap_dump_file = fopen(f.dump_file_name, "w");
stop_and_wait(f.process);
dump_to_file(proc_mem_file, heap_dump_file, &heap_info);
countinue_stopped_process(f.process);
}
return 0;
}
