int main(void) { BYTE stBtn1; BYTE stBtn2; DeviceInit(); AppInit(); while(stBtn1!=stPressed && stBtn2!=stPressed) { mT5IntEnable(fFalse); stBtn1 = btnBtn1.stBtn; stBtn2 = btnBtn2.stBtn; mT5IntEnable(fTrue); } RightReverse; LeftReverse; SetLeftSpeed(dtcMtrMedium); SetRightSpeed(dtcMtrMedium); mCNIntEnable(fTrue); //Sensors will trigger while(fTrue); }
Agent::Agent(double InitialXCoordinate, double InitialYCoordinate, double InitialAngle): myColor(RobotColor), myRadius(RobotRadius), myEnkiEpuck(new Enki::EPuck(Enki::EPuck::CAPABILITY_CAMERA | Enki::EPuck::CAPABILITY_BASIC_SENSORS)) { mySensorReading = 0; basketSeenFlag = 0; canPickedFlag = 0; SetXCoordinate(InitialXCoordinate); SetYCoordinate(InitialYCoordinate); SetAngle(InitialAngle); SetLeftSpeed(0); SetRightSpeed(0); myEnkiEpuck->camera.setRange(CameraRange); //Camera range; default: unlimited myEnkiEpuck->camera.halfFieldOfView = CameraFieldOfView/2; // Camera half field of view }
void __ISR(_CHANGE_NOTICE_VECTOR, ipl2) ChangeNotice_Handler(void) { if(InTheMiddleOfSomething) return; BYTE bVal; bVal = PORTReadBits(IOPORT_B, BIT_0 | BIT_3); Led2; /******************A front wall is detected******************/ // if(!FrontSensor && LeftSensor) { InTheMiddleOfSomething = fTrue; Led1; Led2Clr; //Reverse the motor SetRightSpeed(dtcMtrStopped); RightReverse; //SetRightDir(prtMtrRightDirClr); //Reverse SetRightSpeed(dtcMtrMedium); //mCNClearIntFlag(); //Wait until the turn is executed Wait_ms(TURN90); //Continue going straight SetRightSpeed(dtcMtrStopped); RightForward; //SetRightDir(prtMtrRightDirSet); //Forward SetRightSpeed(dtcMtrMedium); Led1Clr; InTheMiddleOfSomething = fFalse; mCNClearIntFlag(); return; } if(LeftSensor) { InTheMiddleOfSomething = fTrue; Led4; Led2Clr; //Run for a few moments to get around the corner Wait_ms(TURN90/2); //Stop it until the turn is completed SetLeftSpeed(dtcMtrStopped); Wait_ms(TURN90); //Continue going straight and wait to mare sure it can detect the new wall SetLeftSpeed(dtcMtrMedium); Wait_ms(TURN90/4); Led4Clr; InTheMiddleOfSomething = fFalse; } /* if(LeftSensor && FrontSensor) { SetLeftSpeed(dtcMtrStopped); SetRightSpeed(dtcMtrStopped); } else { if(LeftSensor) //Far left sensor -> hard left { SetLeftSpeed(dtcMtrMedium); } if(FrontSensor) //Far right sensor -> hard right { SetRightSpeed(dtcMtrMedium); } } */ mCNClearIntFlag(); }