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; } }