//###########################################3
void rotateRel_naive(double deltaAngle)
{
double x, y, t;
double targetAngle;
double error;
int cmdVel;
getRobotPos(&x, &y, &t);
targetAngle = normalizeAngle(t + deltaAngle);
error = normalizeAngle(targetAngle - t);
if(error < 0)
cmdVel = -30;
else
cmdVel = 30;
setVel2(-cmdVel, cmdVel);
do
{
getRobotPos(&x, &y, &t);
error = normalizeAngle(targetAngle - t);
} while (fabs(error) > 0.01 && (error * cmdVel) > 0);
setVel2(0, 0);
}
/*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);
}
//################3
void ANDAR2 ()
{
if(sensor_dir < LIMIAR)
{
Vira_Dir();
}
if(sensor_esq > LIMIAR)
{
Vira_esq();
}
if(sensor_frente < LIMIAR && sensor_dir < LIMIAR&& sensor_esq < LIMIAR)
{
andar_frente();
}
if(sensor_frente > LIMIAR && sensor_dir > LIMIAR && sensor_esq > LIMIAR)
{
setVel2(0,0);
Rodar_Sobre_Si();
wait(1);
}
}
void sab_execute() {
if(!followPoints){
followPoints = 1;
firstTime=1;
visible=1;
setVel2(0,0);
printf("Beacon found at %2.0f %2.0f\n", x, y);
while (!startButton());
}else {
if(beaconDir > 0) {
setVel2(30, -30);
} else {
setVel2(-30, 30);
}
}
}
int main(void){
initPIC32();
closedLoopControl( false );
setVel2(0, 0); //Stop Engines
int start, oldStart = 2;
resetCoreTimer();
while(1){
start = startButton();
if(start != oldStart)
printf("%10d %d\n",readCoreTimer()/1000, start);
oldStart = start;
}
return 0;
}
int main(void){
initPIC32();
closedLoopControl( false );
setVel2(0, 0); //Stop Engines
while(1){
printf("Press start to continue\n");
while(!startButton());
do{
waitTick40ms();
int ground = readLineSensors(0);
if(NEAR_LEFT_BLACK(ground))
setVel2(0, 50);
else if(NEAR_RIGHT_BLACK(ground))
setVel2(50, 0);
else if(FAR_LEFT_BLACK(ground))
setVel2(-50, 50);
else if (FAR_RIGHT_BLACK(ground))
setVel2(50, -50);
else if(GROUND_CENTER_BLACK(ground))
setVel2(55, 60);
else
setVel2(50, -50);
}while(!stopButton());
}
return 0;
}
//#############################################################################
void Run_Beacon ()
{
disableObstSens();
enableObstSens();
if(obstacleSensor(OBST_SENSOR_FRONT) > LIMIAR)
{
stop_Motors();
if(randomDecisionRotate() == 0)
{
Rodar_Sobre_Si_Dir();
wait(2);
}
else
{
Rodar_Sobre_Si_Esq();
wait(2);
}
}
if(obstacleSensor(OBST_SENSOR_RIGHT) > LIMIAR)
{
Vira_Dir();
}
if(obstacleSensor(OBST_SENSOR_LEFT) > LIMIAR)
{
Vira_esq();
}
if(obstacleSensor(OBST_SENSOR_FRONT) < LIMIAR && obstacleSensor(OBST_SENSOR_RIGHT) < LIMIAR && obstacleSensor(OBST_SENSOR_LEFT)< LIMIAR)
{
andar_frente();
}
if(obstacleSensor(OBST_SENSOR_FRONT) > LIMIAR && obstacleSensor(OBST_SENSOR_RIGHT) > LIMIAR && obstacleSensor(OBST_SENSOR_LEFT)> LIMIAR)
{
setVel2(0,0);
Rodar_Sobre_Si();
wait(1);
}
}
//################3
void Vira_esq ()
{
setVel2 (Velocidade, Velocidade-40); // ALTERAR ISTO
}
//################3
void Vira_Dir ()
{
setVel2 (Velocidade-40, Velocidade); // ALTERAR ISTO
}
//################3
void Ajusta_Dir ()
{
setVel2 (Velocidade,Velocidade-20);
}
//################3
void Ajusta_Esq ()
{
setVel2 (Velocidade-20,Velocidade);
}
void Stop_robot() //serve para quando se carrega no botao de desligar
{
setVel2(0,0); // desliga os metores
estado = 0;
}
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);
}
}
//################
void Vira_esq ()
{
setVel2 (Velocidade+10, Velocidade-30); // Velocidade+10, Velocidade-30
}
void Rodar_Sobre_Si_Esq()
{
setVel2 (-Velocidade, Velocidade);
}
//################3
void andar_frente ()
{
setVel2 (Velocidade, Velocidade); // ALTERAR ISTO
}
void stop_Motors()
{
setVel2(0,0);
}
int main (void)
{
int countCiclos = 100;
//iniciçao da pic
initPIC32 ();
closedLoopControl( true );
setVel2(0, 0);
printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot
printf("\r battery: %d ", batteryVoltage());
if(batteryVoltage() <94){
printf("_Bateria fraca, MUDAR Bateria\n");
}
printStr("\n");
while(1)
{
while(!tick40ms);
tick40ms=0;
readAnalogSensors();
//state buttons
if(startButton() == 1) // Botao start(preto) primido
{
estado = 1;
enableObstSens();
leds(0x0); //leds off
countCiclos = 100;
}
else if(stopButton() == 1) //Botao stop(vermelho) primido
{
printf("Red button pressed!!! \n");
estado = 0;
disableObstSens();
}
if(estado == 1)
{
TimeOut(); // timeOut => tb devia ir para uma inturrupcao
Chegada_Farol();
if(countCiclos++ >= 100)
{
Ver_Farol();
countCiclos = 0;
}
Run_Beacon();
}
if(estado == 2)
{
leds(0x1);
Stop_robot();
}
if(estado == 3)
Fim();
else if(stopButton() == 1 || estado == 0) // deslica o funcionamento, nenhum led activo
{
Stop_robot();
}
}
return (0);
}
//################3
void Rodar_Sobre_Si ()
{
setVel2 (Velocidade, -Velocidade);
}
void PhysicsAMotorJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0);
if(!(whichField & VelFieldMask))
{
setVel(dJointGetAMotorParam(_JointID,dParamVel));
}
if(!(whichField & FMaxFieldMask))
{
setFMax(dJointGetAMotorParam(_JointID,dParamFMax));
}
if(!(whichField & FudgeFactorFieldMask))
{
setFudgeFactor(dJointGetAMotorParam(_JointID,dParamFudgeFactor));
}
if(!(whichField & Vel2FieldMask))
{
setVel2(dJointGetAMotorParam(_JointID,dParamVel2));
}
if(!(whichField & FMax2FieldMask))
{
setFMax2(dJointGetAMotorParam(_JointID,dParamFMax2));
}
if(!(whichField & FudgeFactor2FieldMask))
{
setFudgeFactor2(dJointGetAMotorParam(_JointID,dParamFudgeFactor2));
}
if(!(whichField & Vel3FieldMask))
{
setVel3(dJointGetAMotorParam(_JointID,dParamVel3));
}
if(!(whichField & FMax3FieldMask))
{
setFMax3(dJointGetAMotorParam(_JointID,dParamFMax3));
}
if(!(whichField & FudgeFactor3FieldMask))
{
setFudgeFactor3(dJointGetAMotorParam(_JointID,dParamFudgeFactor3));
}
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetAMotorParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetAMotorParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetAMotorParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetAMotorParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetAMotorParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetAMotorParam(_JointID,dParamStopERP));
}
if(!(whichField & HiStop2FieldMask))
{
setHiStop2(dJointGetAMotorParam(_JointID,dParamHiStop2));
}
if(!(whichField & LoStop2FieldMask))
{
setLoStop2(dJointGetAMotorParam(_JointID,dParamLoStop2));
}
if(!(whichField & Bounce2FieldMask))
{
setBounce2(dJointGetAMotorParam(_JointID,dParamBounce2));
}
if(!(whichField & CFM2FieldMask))
{
setCFM2(dJointGetAMotorParam(_JointID,dParamCFM2));
}
if(!(whichField & StopCFM2FieldMask))
{
setStopCFM2(dJointGetAMotorParam(_JointID,dParamStopCFM2));
}
if(!(whichField & StopERP2FieldMask))
{
setStopERP2(dJointGetAMotorParam(_JointID,dParamStopERP2));
}
if(!(whichField & HiStop3FieldMask))
{
setHiStop3(dJointGetAMotorParam(_JointID,dParamHiStop3));
}
if(!(whichField & LoStop3FieldMask))
{
setLoStop3(dJointGetAMotorParam(_JointID,dParamLoStop3));
}
if(!(whichField & Bounce3FieldMask))
{
setBounce3(dJointGetAMotorParam(_JointID,dParamBounce3));
}
if(!(whichField & CFM3FieldMask))
{
setCFM3(dJointGetAMotorParam(_JointID,dParamCFM3));
}
if(!(whichField & StopCFM3FieldMask))
{
setStopCFM3(dJointGetAMotorParam(_JointID,dParamStopCFM3));
}
if(!(whichField & StopERP3FieldMask))
{
setStopERP3(dJointGetAMotorParam(_JointID,dParamStopERP3));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & NumAxesFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorNumAxes(_JointID,getNumAxes());
}
if((whichField & Axis1FieldMask) ||
(whichField & Axis1ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z());
}
if((whichField & Axis2FieldMask) ||
(whichField & Axis2ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z());
}
if((whichField & Axis3FieldMask) ||
(whichField & Axis3ReferenceFrameFieldMask) ||
(whichField & WorldFieldMask))
{
dJointSetAMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z());
}
if((whichField & VelFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel, getVel());
}
if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax, getFMax());
}
if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor());
}
if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel2, getVel2());
}
if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax2, getFMax2());
}
if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2());
}
if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamVel3, getVel3());
}
if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFMax3, getFMax3());
}
if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM, getStopCFM());
}
if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop2, getHiStop2());
}
if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop2, getLoStop2());
}
if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce2, getBounce2());
}
if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM2, getCFM2());
}
if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP2, getStopERP2());
}
if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM2, getStopCFM2());
}
if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamHiStop3, getHiStop3());
}
if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamLoStop3, getLoStop3());
}
if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamBounce3, getBounce3());
}
if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamCFM3, getCFM3());
}
if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopERP3, getStopERP3());
}
if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask))
{
dJointSetAMotorParam(_JointID, dParamStopCFM3, getStopCFM3());
}
}
int main (void)
{
//iniciçao da pic
initPIC32 ();
closedLoopControl( true );
setVel2(0, 0);
T4CONbits.TCKPS=5;
PR4=(PBCLK/32/3)-1;//fout 20MHz/(32*(62499+1))*10Hz,freq a 1 hz
TMR4=0;//Reset timer T2
T4CONbits.TON=1;
//Interrupts
IFS0bits.T4IF=0;//reset do timer
IPC4bits.T4IP=2;//priority
IEC0bits.T4IE=1;//enable timer3
EnableInterrupts();
printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot
printf("\r battery: %d ", batteryVoltage());
printStr("\n");
//desligar os leds para o inicio
leds(0b1111); //leds off
#if 0
while(1)
{
while(!tick40ms);
tick40ms=0;
readAnalogSensors();
}
#endif
while (TRUE)
{
TimeOut(); // timeOut => tb devia ir para uma inturrupcao
//Sensor(); //leitura os sensores tem de se por numa interropecao
//botao de estado
if(startButton() == 1) // Botao start(preto) primido
{
estado = 1;
resetCoreTimer();
leds(0x1);
}
else if(stopButton() == 1) //Botao stop(vermelho) primido
{
estado = 0;
leds(0x0);
}
//funcoa de funcionameneto
if(estado == 1) // robot fica em presesamento
{
Ver_Farol();
printf("D: %d F: %d E: %d LINHA: %d Farol: %d \r", sensor_dir,sensor_frente,sensor_esq,linha ,farolsen );
ANDAR2();
}
//quando e primido o botao stop
else if(stopButton() == 1) // deslica o funcionamento, nenhum led activo
{
Stop_robot();
}
}
return (0);
}
//################
void Vira_Dir ()
{
setVel2 (Velocidade-30, Velocidade+10); // Velocidade-40, Velocidade
}
