int main(void) { // initialization initIO(); startupSequence(); startupSequence(); // _delay_ms(100); // TOGGLE_STATUS; // _delay_ms(100); // TOGGLE_STATUS; // spin until position received from connected block (or BBB) uint8_t addr = waitForVector(); // assign function pointer to use custom data-collector function getData_ptr gData = getMyData; sendHorizontal_ptr sHoriz = sendRight; sendHorizontal_ptr sVert = sendUp; // assign received vector to i2c initialization setup_i2c(addr, gData, sHoriz, sVert, 0, 0); forwardChain(); STATUS_PORT &= ~_BV(STATUS_LED); sei(); // continuously poll i2c for commands while (loop_i2c()) { advanceVector(); // _delay_ms(200); // TOGGLE_STATUS; } return 0; // never reached }
// one-time initialization routine void Quadrotor::initialize() { // wait 1 sec for devices to power on delay(1000); // set up Serial output via USB Serial.begin(115200); delay(5); // initialize i2c bus i2c.initialize(); // initialize bluetooth bluetooth->initialize(); // initialize Motors motor1->initialize(CLOCKWISE, ONE); motor2->initialize(COUNTER_CLOCKWISE, TWO); motor3->initialize(CLOCKWISE, THREE); motor4->initialize(COUNTER_CLOCKWISE, FOUR); // attach the motors to the controller so that it can adjust speeds for stability and control // controller->attachMotors(motor1, motor2, motor3, motor4); // initialize orientationManager // orientationManager->initialize(imu, altitudeSensor); // if not testing then finish initialization #ifndef TESTING // wait for start command waitForStartCommand(); // start up the quadrotor motors float startingSpeed = 10.0; startupSequence(startingSpeed); // start keeping track of the timing previousTime = millis(); // last task is to setup the orientation manager with the orientation of the craft before takeoff orientationManager->initialRead(orientation); // initialize the control system with the initial data controller->initialize(orientation, startingSpeed); #endif }