/*esta funcao tem de ser alterada pois ele tem de ver se encotra o farol a 360º */ void Ver_Farol() { stop_Motors(); int pos = -15, cont=0; readAnalogSensors(); do{ setServoPos(pos); pos++; if(pos== (15)){ pos= -15; cont++; } wait(1); }while((readBeaconSens() ==0 ) && cont <= 1); printf("%d \n", pos); if(pos<0 && cont <2){ printf("1\n"); do{ setVel2 (-Velocidade, Velocidade); pos++; setServoPos(pos); wait(0.8); setVel2(0,0); // rotateRel_naive((double) pos ); }while((readBeaconSens())==0 && pos != 0); }else if(pos>0 && cont <2){ printf("2\n"); do{ setVel2 (Velocidade, -Velocidade); pos--; setServoPos(pos); wait(0.8); setVel2(0,0); // rotateRel_naive((double) (pos* (-1) ) ); }while((readBeaconSens())==0 && pos != 0); } setServoPos(0); }
void readBeaconSensor(){ if(readBeaconSens()){ sensor_sensorReadings.beaconSensor.isVisible = true; sensor_sensorReadings.beaconSensor.relative_direction = (beacon_servo_pos/15.0*M_PI/2);//normalizing angle in radians beacon_servo_counter = 0; beacon_servo_sensor_limit_left = ((beacon_servo_pos - BEACON_SENSOR_SERVO_BUFFER) >= -BEACON_SENSOR_SERVO_LIMIT) ? (beacon_servo_pos - BEACON_SENSOR_SERVO_BUFFER) : -BEACON_SENSOR_SERVO_LIMIT; beacon_servo_sensor_limit_right = ((beacon_servo_pos + BEACON_SENSOR_SERVO_BUFFER) <= BEACON_SENSOR_SERVO_LIMIT) ? (beacon_servo_pos + BEACON_SENSOR_SERVO_BUFFER) : BEACON_SENSOR_SERVO_LIMIT; } if(sensor_sensorReadings.beaconSensor.isVisible == true && beacon_servo_counter > (BEACON_SENSOR_SERVO_BUFFER*3)){ beacon_servo_sensor_limit_left = -BEACON_SENSOR_SERVO_LIMIT; beacon_servo_sensor_limit_right = BEACON_SENSOR_SERVO_LIMIT; sensor_sensorReadings.beaconSensor.isVisible = false; beacon_servo_counter = 0; }else{ beacon_servo_counter++; } if(beacon_servo_pos <= beacon_servo_sensor_limit_left || beacon_servo_pos >= beacon_servo_sensor_limit_right){ beacon_servo_dir = !beacon_servo_dir;//change direction } if(beacon_servo_dir == LEFT){ beacon_servo_pos-=2; }else{ beacon_servo_pos+=2; } setServoPos(beacon_servo_pos); }
/*esta funcao tem de ser alterada pois ele tem de ver se encotra o farol a 360º */ void Ver_Farol() { setServoPos(0); readAnalogSensors(); if( (farolsen = readBeaconSens())==1) { andar_frente(); } }
void servoControl(){ //printf("BeaconVisible=%d beaconDir=%d left=%d right=%d ServoPos=%d\n", beaconVisible, beaconDir, left, right, currServoPos); //led(3,visible); if(followPoints){ //printf("FOLLOW POINT\n"); getLastPoint(); targetX=lastPointX; targetY=lastPointY; if( checkPointsRadius() ){ if(removePoint()){ // update point printf("Found home!\n"); setVel2(0,0); while (!startButton()); } } //double myNorm, targetNorm; double meToTargetVector [2]; double myDirectionVector [2]; double myX=x, myY=y, myDir=t; // 0.35 radians meToTargetVector[0]=(targetX-myX); meToTargetVector[1]=(targetY-myY); myDirectionVector[0]=cos(myDir); myDirectionVector[1]=sin(myDir); double radianBeaconDir = atan2(meToTargetVector[1], meToTargetVector[0]) - atan2(myDirectionVector[1], myDirectionVector[0]); beaconDir=-normalizeAngle(radianBeaconDir)*(180 / PI); int servoPos = beaconDir/6; printf("My position %2.1f,%2.1f; Target Position %d,%d; MyAngle %2.0f AngleToNextPoint %d\n", myX, myY, targetX, targetY, (myDir*180/PI), beaconDir); //NEGATIVO PRA ESQUERDA if(servoPos > POS_RIGHT) setServoPos(POS_RIGHT); else if( servoPos < POS_LEFT ) setServoPos(POS_LEFT); else setServoPos(servoPos); }else{ bVis=readBeaconSens(); if(bVis && !oldBVis){ // Started seeing beacon! if(rotate_right){ left = currServoPos; //if(left<0 && right>=POS_RIGHT) // right=left+15; } else { right = currServoPos; //if(right>0 && left<=POS_LEFT) // left=right-15; } if(currServoPos==POS_RIGHT || currServoPos==POS_LEFT){ // Got to the end of the line rotate_right=!rotate_right; } } else if(!bVis && oldBVis){ // Stopped seeing beacon! if(rotate_right){ right = currServoPos-1; } else { left = currServoPos+1; } rotate_right=!rotate_right; } else if(currServoPos==POS_RIGHT || currServoPos==POS_LEFT){ // Got to the end of the line if(currServoPos==POS_RIGHT){ right = POS_RIGHT+1; } else { left = POS_LEFT-1; } rotate_right=!rotate_right; } visible = bVis||oldBVis||oldBVis2; //led(2, visible); oldBVis2=oldBVis; oldBVis=bVis; beaconDir = (left + right)/2*6; //printf("Beacon at %d\n",beaconDir); modder = rotate_right ? 1 : -1; //currServoPos+=modder; currServoPos = visible ? currServoPos+modder*1 : currServoPos+modder*2; if(currServoPos>POS_RIGHT){ currServoPos=POS_RIGHT; } else if(currServoPos<POS_LEFT){ currServoPos=POS_LEFT; } setServoPos(currServoPos); } }