//------------------------------------------------------------------------------
// Loop Function - Arduino Program Loop
//------------------------------------------------------------------------------
void loop() {
digitalWrite(13, HIGH);
delay(1000);
digitalWrite(13, LOW);
delay(100);
Serial.print(itg.x());
Serial.print(", ");
Serial.print(itg.y());
Serial.print(", ");
Serial.println(itg.z());
}
void getMotionData(Datum *datum) {
acc.getAcceleration(&datum->acc.x, &datum->acc.y, &datum->acc.z);
mag.getHeading(&datum->mag.x, &datum->mag.y, &datum->mag.z);
gyr.getRotation(&datum->gyr.x, &datum->gyr.y, &datum->gyr.z);
}
void getGyroValues(){
float xyz[3];
gyro.readGyro(xyz);
gx = xyz[0];
gy = xyz[1];
gz = xyz[2];
}
void setup() {
debug("Starting setup");
Serial.begin(9600);
mySerial.begin(9600);
// Initialize classes
escs.x = &esc_x;
escs.nx = &esc_nx;
escs.y = &esc_y;
escs.ny = &esc_ny;
quad.escs = &escs;
quad.controller = &controller;
quad.gyro = &gyro;
quad.acc = &acc;
quad.alt = &alt;
Wire.begin();
acc.init();
gyro.init();
attachInterrupt(PING_INT, pingInterrupt, FALLING);
pinMode(ESC_X_PIN, OUTPUT);
pinMode(ESC_NX_PIN, OUTPUT);
pinMode(ESC_Y_PIN, OUTPUT);
pinMode(ESC_NY_PIN, OUTPUT);
// Set all motor speeds to ESC min
escs.write(OUTPUT_MIN);
debug("Entering main loop");
}
//------------------------------------------------------------------------------
// Setup Function - Initializes Arduino
//------------------------------------------------------------------------------
void setup() {
pinMode(13, OUTPUT);
Serial.begin(9600);
Serial.println("Initializing... do not move chip!");
itg.initialize();
}
void Cfilterbegin(){
//Turning on the accelerometer
Accel.init(ADXL345_ADDR_ALT_LOW);
Accel.set_bw(ADXL345_BW_12);
gyro.reset();
// Use ITG3200_ADDR_AD0_HIGH or ITG3200_ADDR_AD0_LOW as the ITG3200 address
// depending on how AD0 is connected on your breakout board, check its schematics for details
gyro.init(ITG3200_ADDR_AD0_LOW);
Serial.print("zeroCalibrating...");
gyro.zeroCalibrate(2500,2);
Serial.println("done.");
alpha = float(TIME_CONSTANT) / (float(TIME_CONSTANT) + float(SAMPLE_RATE)); // calculate the scaling coefficent
Serial.print("Scaling Coefficent: ");
Serial.println(alpha);
delay(100);
}
void ITG3200_sample(){
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Starting");
#endif
float x,y,z;
if (itg.isRawDataReady()){
itg.readGyro(&x,&y,&z);
logMessage("Gyro.ITG3200.X",x, "Degrees*1000/Second");
logMessage("Gyro.ITG3200.Y",y, "Degrees*1000/Second");
logMessage("Gyro.ITG3200.Z",z, "Degrees*1000/Second");
}
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Finished");
#endif
}
void itg3200poll(){
m.units="Degrees*1000/Second";
m.nameSpace="Gyro.ITG3200.X";
m.value=String(int(gyro.getX()*1000));
log_message();
m.nameSpace="Gyro.ITG3200.Y";
m.value=String(int(gyro.getY()*1000));
log_message();
m.nameSpace="Gyro.ITG3200.Z";
m.value=String(int(gyro.getZ()*1000));
log_message();
m.units="Degrees Celsius*1000";
m.nameSpace="Gyro.ITG3200.Temperature";
m.value=String(int(gyro.getTemperature()*1000));
log_message();
return;
}
void ITG3200_init(){
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Starting");
#endif
Wire.begin();
delay(1000);
itg.init(ITG3200_ADDR_AD0_HIGH);
#ifdef DEBUG_ITG3200_POLLER
DEBUG_1("Finished");
#endif
}
void ExtendoHand::setupOther() {
if (nineAxis) {
// adjust the power settings after you call this method if you want the accelerometer
// to enter standby mode, or another less demanding mode of operation
accel.setRange(1); // 4g
accel.setFullResolution(1); // maintain 4mg/LSB scale factor (irrespective of range)
accel.initialize();
if (!accel.testConnection()) {
osc.sendError("ADXL345 connection failed");
} else {
randomSeed(accel.getAccelerationX() - accel.getAccelerationY() + accel.getAccelerationZ());
}
#ifdef ENABLE_GYRO
gyro.initialize();
if (!gyro.testConnection()) {
osc.sendError("ITG3200 connection failed");
}
#endif // ENABLE_GYRO
#ifdef ENABLE_MAGNETOMETER
magnet.initialize();
if (!magnet.testConnection()) {
osc.sendError("HMC5883L connection failed");
}
#endif // ENABLE_MAGNETOMETER
} else {
#ifdef THREE_AXIS
randomSeed(motionSensor.rawX() + motionSensor.rawY() + motionSensor.rawZ());
// 1.5g constants, sampled 2014-06-21
motionSensor.calibrateX(272, 794);
motionSensor.calibrateY(332, 841);
motionSensor.calibrateZ(175, 700);
#endif // THREE_AXIS
}
vibrateStart = 0;
alertStart = 0;
}
void ExtendoHand::getRotation(Vector3D &g) {
if (nineAxis) {
#ifdef ENABLE_GYRO
int16_t gx, gy, gz;
gyro.getRotation(&gx, &gy, &gz);
g.set(gx, gy, gz);
#else
g.set(0, 0, 0);
#endif // ENABLE_GYRO
} else {
g.set(0, 0, 0);
}
}
int setupMotionSensors() {
int error = 0;
// Initialization
Wire.begin();
acc.initialize();
mag.initialize();
gyr.initialize();
// Verification
if (!acc.testConnection() ||
!mag.testConnection() ||
!gyr.testConnection())
error = 1;
// Configuration
acc.setRange(ADXL345_RANGE_16G);
mag.setMode(HMC5883L_MODE_CONTINUOUS);
return error;
}
void loop() {
unsigned char data[8];
if (mySerial.available() > 0) {
if (readFrame(&mySerial, data) == FRAME_COMPLETE)
controller.updateFromDataArray(data);
}
acc.read();
gyro.read();
alt.start();
quad.computePIDs();
quad.setESCs();
}
void ExtendoHand::onLoopTimeUpdated(double loopTime) {
if (nineAxis) {
// the sensor's sampling rate should exceed Extend-o-Hand's loop rate
uint8_t sampleRate = loopTime <= 0.00125
? 0xe // 1600 Hz
: loopTime <= 0.0025
? 0xd // 800 Hz
: 0xc; // 400 Hz
// uncomment only for development
//osc.sendInfo("setting sensor sampling rate to %d based on loop time of %d micros",
// sampleRate, (int)(loopTime*1000000));
// adjust the power settings after you call these methods if you want the sensors
// to enter standby mode, or another less demanding mode of operation
accel.setRate(sampleRate);
#ifdef ENABLE_GYRO
gyro.setRate(sampleRate);
#endif // ENABLE_GYRO
#ifdef ENABLE_MAGNETOMETER
magnet.setDataRate(sampleRate);
#endif // ENABLE_MAGNETOMETER
}
}
void itg3200begin(){
gyro.begin(ITG3200_ADDRESS);
return;
}
