void MyRobot::run(){
double compass_angle;
double x = 0, z = 0;
double distance = 0;
while (step(_time_step) != -1) {
// Vector to initial position and actual position
vector <double> position, actual_position;
// Read the compass sensor
const double *compass_val = _my_compass->getValues();
compass_angle = convert_bearing_to_degrees(compass_val);
cout << "Compass angle: " << compass_angle << endl;
// Initialization for initial position
position.push_back(x);
position.push_back(z);
position.push_back(compass_angle*M_PI/180);
// If crossed distance (distance) is smaller than desired distance (18.3)
// robot moves depend of encoders position
// gets actual position
if (distance < 18.3 ){ // Estimate error 1.3 m, yellow line distance is 17 m
mode_selection(move());
actual_position = calc_distance(position);
x = actual_position.at(0);
z = actual_position.at(1);
distance = actual_position.at(2);
cout << "x: " << x << ", z: " << z << ", theta: " << compass_angle*M_PI/180 << endl;
cout << "distance: " << distance << endl;
}
// Else, desired distance achivied and robot stops
else {
mode_selection(3);
}
}
}
void display(void) {
if(handPointList == NULL){
printf("error. can't allocate memory for handPointList");
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if(!preview){
draw_background();
glLoadIdentity();
UIhandler(); //check ui touch
//display
//printscr();
mode_selection(handPointList, rhand);
}else{
preview_scene();
}
glFlush();
}
