/*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);
}
}
