void StartRun(void) {
int i;
// calibrate gyroscope (if not done)
if (!bGyroCalibratedFlag)
CalibrateGyro();
// reset alignment values
linePosition = 0;
prevLinePosition = 0;
// reset side sensors variables
for (i = 0; i < NUM_SIDE_SENSORS; i++) {
markerCnt[i] = 0;
bSideMarkerFlag[i] = FALSE;
bSideSenDisableFlag[i] = FALSE;
posSideSen[i] = 0;
}
// reset speed profile
ResetSpeedProfileData();
// reset flags
bJunctionFlag = FALSE;
posJunction = 0;
// enable motor and assert run flag
EnableMotor();
bRunFlag = TRUE;
}
/**
* Runs the robot, based on whichever program was selected. Only runs if the start button isnt connected or is pressed.
* Returns true when it has completed said program.
*/
bool Robot::Run()
{
//Read startButtonPin. If nothing is connected digitalRead will return HIGH (because of INPUT_PULLUP), otherwise if the
//switch is connected and pressed digital read will return HIGH, when not pressed will return LOW.
if(digitalRead(config.startButtonPin))
{
//Only run if we haven't completed the program yet.
if(!completed)
{
//Run the program selected when the robot powers on.
switch(program_)
{
case 1:
if(FinalRun()) completed = true;
break;
case 2:
if(TestMotorsDemo()) completed = true;
break;
case 3:
if(TestMotorsRotate()) completed = true;
break;
case 4:
if(TestMotorsFollowHeading()) completed = true;
break;
case 5:
if(TestGoToVictim()) completed = true;
break;
case 6:
if(TestPixyGetBlock()) completed = true;
break;
case 7:
if(CalibrateGyro()) completed = true;
break;
case 8:
if(TestGyroOutput()) completed = true;
break;
case 9:
if(TestBrainFollowWallFront(LEFT)) completed = true;
break;
case 10:
if(TestBrainFollowWallFront(RIGHT)) completed = true;
break;
case 11:
if(TestBrainFollowWallGap(LEFT)) completed = true;
break;
case 12:
if(TestBrainFollowWallGap(RIGHT)) completed = true;
break;
case 13:
if(TestBrainFollowWallPixy(LEFT)) completed = true;
break;
case 14:
if(TestBrainFollowWallPixy(RIGHT)) completed = true;
break;
case 15:
if(TestGoToLocation()) completed = true;
break;
case 16:
if(TestAStarSearch()) completed = true;
break;
case 17:
if(TestVictimGrasp()) completed = true;
break;
case 18:
if(TestWallSensors()) completed = true;
break;
case 19:
if(TestGoStraight()) completed = true;
break;
case 20:
if(TestHasVictim()) completed = true;
break;
case 21:
if(TestBrainFollowWallNone(LEFT)) completed = true;
break;
case 22:
if(TestBrainFollowWallNone(RIGHT)) completed = true;
break;
case 23:
if(TestBrainTravelPastWall(LEFT)) completed = true;
break;
case 24:
if(TestBrainTravelPastWall(RIGHT)) completed = true;
break;
default:
Serial.print(F("ERROR- Invalid program choice: "));
Serial.println(program_);
completed = true;
break;
}
}
}
return completed;
}
int16_t main(void)
{
unsigned int buttonCounter = 20000;
int i;
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
//initSerial();
initSPI1();
initSPI2();
InitI2C();
__delay32(1600000); // allow for POR for all devices
initnRF();
ControlByte = 0x00D0; // mpu6050 address
InitMPU6050(ControlByte);
// InitHMC5883L();
__delay_ms(500);
/** Init control loop *****************************************************/
readSensorData();
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
AngleOffset[0] = accXangle;
AngleOffset[1] = accYangle;
CalibrateGyro(); // Finding the gyro zero-offset
SetupInterrupts();
////// int serStringN = 14;
////// char nRFstatus = 0;
//////// int i;
////// delaytime = 1;
////// bool mode = 0;
while(1)
{
/** Read Button RB7 for enable steppers *******************************/
if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 1;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter)
{
enableSteppers = 2;
__delay32(40000000);
buttonState = 2;
buttonCounter = 20000;
enableSteppers = 1;
}
if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 3;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter)
{
buttonState = 0;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonCounter)
{
buttonCounter--;
}
for (i=0;i<100;i++)
{
i=i;
}
// __delay32(100);
}
}
/**
* keyboard is a callback registered with GLUT.
* It handles (surprise!) keyboard input.
*
* @param key The key pressed by the user.
* @param x The x coordinate of the mouse when the key was pressed.
* @param y The y coordinate of the mouse when the key was pressed.
*/
void keyboard( unsigned char key, int x, int y ) {
Scene *theScene = Engine::instance()->rootScene();
Cameras *camList = Engine::instance()->cams();
#ifdef WII
// Hacky, for the wii reset, below.
Camera *camptr = dynamic_cast< Camera* >( (*_camList)["AutoCamera2"] );
#endif
switch ( key ) {
case 033: // Escape Key
/*
cleanup();
Disabled for now; not crucial.
Intend to fix later when I profile a bit more with valgrind.
*/
glutLeaveMainLoop();
break;
case ';': // Print Info
fprintf( stderr, "Active Object: %s\n",
theScene->active()->Name().c_str() );
break;
case '~':
#ifdef WII
CalibrateGyro( Wii );
if (camptr) camptr->resetRotation();
#endif
break;
case '+':
std::stringstream camName;
camName << "AutoCamera" << camList->numCameras() + 1;
camList->addCamera( camName.str() );
break;
}
if ( camList->numCameras() < 1 ) return;
/* A shorthand variable with local scope that refers to "The active Camera." */
Camera &cam = *(camList->active());
switch ( key ) {
case '-':
camList->popCamera();
break;
case ';':
fprintf( stderr, "Camera Position: (%f,%f,%f)\n", cam.x(), cam.y(),
cam.z() );
break;
case 'w':
cam.move( Camera::DIR_FORWARD );
break;
case 's':
cam.move( Camera::DIR_BACKWARD );
break;
case 'a':
cam.move( Camera::DIR_LEFT );
break;
case 'd':
cam.move( Camera::DIR_RIGHT );
break;
case 'q':
cam.move( Camera::DIR_UP );
break;
case 'e':
cam.move( Camera::DIR_DOWN );
break;
//Perspectives
case 'z':
cam.changePerspective( Camera::PERSPECTIVE );
break;
case 'x':
cam.changePerspective( Camera::ORTHO );
break;
case 'c':
cam.changePerspective( Camera::ORTHO2D );
break;
case 'v':
cam.changePerspective( Camera::FRUSTUM );
break;
case 'b':
cam.changePerspective( Camera::IDENTITY );
break;
}
}
