void setCompassOffset()
{
#ifdef DEBUG
debugSerial.println("setCompassOffset");
#endif
Wire.begin();
compass.setRange(HMC5883L_RANGE_1_3GA);
compass.setMeasurementMode(HMC5883L_CONTINOUS);
compass.setDataRate(HMC5883L_DATARATE_30HZ);
compass.setSamples(HMC5883L_SAMPLES_8);
int minX = INT_MAX,maxX = INT_MIN,minY = INT_MAX,maxY = INT_MIN;
int offsetX,offsetY;
motor.rotateRun(FORWORD, 128);
delay(200);
#ifdef DEBUG
debugSerial.println("setCompassOffset2");
#endif
for(int i = 0; i < 500; ++i)
{
Vector mag = compass.readRaw();
maxX = max(maxX,mag.XAxis);
maxY = max(maxY,mag.XAxis);
minX = min(minX,mag.XAxis);
minY = min(minY,mag.XAxis);
delay(20);
#ifdef DEBUG
debugSerial.println(i);
#endif // DEBUG
}
motor.stop();
offsetX = (maxX + minX) / 2;
offsetY = (maxY + minY) / 2;
uint addr = CompassStorageAddr;
EEPROM_writeInt(addr,offsetX);
addr += 2;
EEPROM_writeInt(addr,offsetY);
addr += 2;
compass.setOffset(offsetX,offsetY);
#ifdef DEBUG
debugSerial.print("Compass Offset:");
debugSerial.print(offsetX);
debugSerial.print(" | ");
debugSerial.print(offsetY);
debugSerial.print("\n");
#endif // DEBUG
}
void EEPROM_writeAccelTable(AccelTableStruct* table, byte elements, unsigned int address){
for(byte i = 0; i < elements; i++) {
EEPROM_writeInt(table[i].speed,address);
address = address + sizeof(unsigned int);
EEPROM_writeByte(table[i].repeats,address);
address = address + sizeof(byte);
}
}
void EEPROM_writeLong(unsigned long val, unsigned int address) {
FourBytes storer = {val};
EEPROM_writeInt(storer.array[0], address);
EEPROM_writeInt(storer.array[1], address+2);
}