//###########################################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 }