task main()
{
setupGyro();
resetAll(); //Stop all motors and reset all sensors
//1.Preload
//Pick up First Buckyball
intake(127, 200);
//Rotate 90 degrees
spin(900, 5);
wait1Msec(500);
//Dump buckyball
intake(-127, 1000); //Spin intake for 1 second
wait1Msec(500);
//2. Corner Buckys
//Turn around to collect other buckyballs
clearGyro();
spin(1800,5);
wait1Msec(500);
//Move toward buckys
clearDriveEncoders();
motor[intakeMotor] = 127;
sonicMove(80, 15); //Move to 15cm away from buckyball using sonar
motor[intakeMotor] = 0;
//3. Deposit Buckys in goal
resetAll();
//Turn around 180 degrees
spin(1800, 5);
wait1Msec(500);
//Drive to other side
clearDriveEncoders();
drive(100);
wait1Msec(7000); //Time based driving at the beginning to get over bump
clearDriveEncoders();
sonicMove(100, 30); //Ultra-sonic takes over
//Turn to face tube
clearGyro();
spin(3150, 2);
wait1Msec(500);
//Drive to tube
sonicMove(50, 15);
}
bool Adafruit_LSM9DS0::begin()
{
// Enable I2C
Wire.begin();
uint8_t id = read8(XMTYPE, LSM9DS0_REGISTER_WHO_AM_I_XM);
// Serial.print ("XM whoami: 0x");
// Serial.println(id, HEX);
if (id != LSM9DS0_XM_ID)
return false;
id = read8(GYROTYPE, LSM9DS0_REGISTER_WHO_AM_I_G);
// Serial.print ("G whoami: 0x");
// Serial.println(id, HEX);
if (id != LSM9DS0_G_ID)
return false;
// Enable the accelerometer continous
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG1_XM, 0x67); // 100hz XYZ
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM, 0b11110000);
// enable mag continuous
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG7_XM, 0b00000000);
// enable gyro continuous
write8(GYROTYPE, LSM9DS0_REGISTER_CTRL_REG1_G, 0x0F); // on XYZ
// enable the temperature sensor (output rate same as the mag sensor)
uint8_t tempReg = read8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM);
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM, tempReg | (1<<7));
/*
for (uint8_t i=0; i<0x30; i++) {
Serial.print("$"); Serial.print(i, HEX);
Serial.print(" = 0x");
Serial.println(read8(XMTYPE, i), HEX);
}
*/
// Set default ranges for the various sensors
setupAccel(LSM9DS0_ACCELRANGE_2G);
setupGyro(LSM9DS0_GYROSCALE_245DPS);
return true;
}
void gyro_setup() {
//Serial.begin(115200);
Wire.begin();
setupGyro();
calibrateGyro();
}
bool Adafruit_LSM9DS0::begin()
{
if (_i2c) {
Wire.begin();
} else if (_clk == -1) {
// Hardware SPI
pinMode(_csxm, OUTPUT);
pinMode(_csg, OUTPUT);
digitalWrite(_csxm, HIGH);
digitalWrite(_csg, HIGH);
SPCRback = SPCR;
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV8);
SPI.setDataMode(SPI_MODE0);
mySPCR = SPCR;
SPCR = SPCRback;
} else {
// Sofware SPI
pinMode(_clk, OUTPUT);
pinMode(_mosi, OUTPUT);
pinMode(_csxm, OUTPUT);
pinMode(_csg, OUTPUT);
digitalWrite(_csxm, HIGH);
digitalWrite(_csg, HIGH);
digitalWrite(_clk, HIGH);
}
uint8_t id = read8(XMTYPE, LSM9DS0_REGISTER_WHO_AM_I_XM);
//Serial.print ("XM whoami: 0x");
// Serial.println(id, HEX);
if (id != LSM9DS0_XM_ID)
return false;
id = read8(GYROTYPE, LSM9DS0_REGISTER_WHO_AM_I_G);
// Serial.print ("G whoami: 0x");
// Serial.println(id, HEX);
if (id != LSM9DS0_G_ID)
return false;
// Enable the accelerometer continous
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG1_XM, 0x67); // 100hz XYZ
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM, 0b11110000);
// enable mag continuous
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG7_XM, 0b00000000);
// enable gyro continuous
write8(GYROTYPE, LSM9DS0_REGISTER_CTRL_REG1_G, 0x0F); // on XYZ
// enable the temperature sensor (output rate same as the mag sensor)
uint8_t tempReg = read8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM);
write8(XMTYPE, LSM9DS0_REGISTER_CTRL_REG5_XM, tempReg | (1<<7));
/*
for (uint8_t i=0; i<0x30; i++) {
Serial.print("$"); Serial.print(i, HEX);
Serial.print(" = 0x");
Serial.println(read8(XMTYPE, i), HEX);
}
*/
// Set default ranges for the various sensors
setupAccel(LSM9DS0_ACCELRANGE_2G);
setupMag(LSM9DS0_MAGGAIN_2GAUSS);
setupGyro(LSM9DS0_GYROSCALE_245DPS);
return true;
}
void setup() {
Serial.begin(115200);
Wire.begin();
setupGyro();
calibrateGyro();
}
