void elevator_init_pos(){
if(elev_get_floor_sensor_signal()==BETWEEN_FLOORS){
control_down();
while(elev_get_floor_sensor_signal()==BETWEEN_FLOORS)
; // wait
}
int floor = elev_get_floor_sensor_signal();
set_last_floor(floor);
control_setcurpos(floor);
control_stop();
}
void CScrView::OnKeyDown(UINT nChar, UINT nRepCnt, UINT nFlags)
{
switch(nChar)
{
case VK_LEFT:
case VK_RIGHT:
case VK_UP:
case VK_DOWN:
case VK_HOME:
case VK_END:
case VK_PRIOR:
case VK_NEXT:
if (MovePos(nChar, nRepCnt, control_down(), shift_down(), caret_mode_))
{
DisplayCaret();
return;
}
break;
}
CView::OnKeyDown(nChar, nRepCnt, nFlags);
}
void MyRobot::run()
{
double sum;
gps_initial[2] = 1000.0;
x = 0;
y = 0;
z = 0;
counter = 0;
_compass_angle_green[0]= 1000.0;
_compass_angle_green[1]= 1000.0;
end = 0;
metres =0;
num_person = 0;
turn = false;
back = false;
inside = false;
person = false;
following = false;
while (step(_time_step) != -1)
{
get_distances();
sum = 0;
for (int i = 0; i < NUM_DISTANCE_SENSOR; i++)
{
sum = sum + _dist_val[i];
}
//First we calculate initial gps position, the robot wont start until we know it
if(gps_initial[2] != 1000.0){
if (((gps[2]- gps_initial[2])>17) && (num_person < 2)){
//Start the identification
if ((_compass_angle_green[0] == 1000.0) || (_compass_angle_green[1] == 1000.0))
{
scaner_turn_around();
}
else
{
gps[2] = gps_initial[2] + 18;
_forward_camera->disable();
if (person == false)
{
rescue_person(_compass_angle_green[1]);
}
else
{
rescue_person(_compass_angle_green[0]);
}
}
}
else
{
//Start the going down
if (num_person == 2){
/*Checks if there is any distance sensor detecting a wall
and if this wall is close enought (200) to change the mode
from follow_compass to control */
if (sum <= 200)
{
//To follow down direction of the map
follow_compass_down(-135);
}
else
{
control_down();
}
}
else
{
//Start the going up
gps_average();
if (sum <= 200)
{
follow_compass(45);
}
else
{
control_up();
}
}
}
}else{
gps_average();
// Read the sensors
const double *compass_val = _my_compass->getValues();
// Convert compass bearing vector to angle, in degrees
_compass_angle = convert_bearing_to_degrees(compass_val);
//We placed the robot in the correct direction.
if(_compass_angle >= 35 && _compass_angle < 55){
_mode = STOP;
}else{
_mode = FAST_TURN_AROUND;
}
}
// Set the mode
mode();
// Set the motor speeds
setSpeed(_left_speed, _right_speed);
}
}
