compassTest::compassTest(struct Motors *motors, OrientationManager *orientationManager, ControllerManager *controllerManager, Bluetooth *bluetooth, HMC5883L *mag)
: motors(motors), orientationManager(orientationManager), controllerManager(controllerManager), bluetooth(bluetooth), mag(mag)
{
XYZData compassBearing;
int xComp, yComp, zComp;
mag->initialize();
mag->selfTest();
waitForStartCommand();
for(int i =0; i < 11; i++)
{
setSpeed(10*i);
for(int j=0; j<30; j++)
{
mag->readData(compassBearing);
delay(10);
xComp = compassBearing.X;
yComp = compassBearing.Y;
zComp = compassBearing.Z;
Serial.write(xComp);
Serial.write(yComp);
Serial.write(zComp);
}
}
shutdownSequence();
}
// 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
}
