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
}
