void setup() {
Wire.begin();
Serial.begin(57600);
I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG
accelgyro.setRate(4);
accelgyro.setDLPFMode(0x03);
accelgyro.setFIFOEnabled(true);
I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
accelgyro.setDMPEnabled(false);
I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
attachInterrupt(0, mpu_interrupt, RISING);
attachInterrupt(1, compass_interrupt, FALLING);
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(0x1E); //open communication with HMC5883
Wire.write(0x00); //select Config_Register_A:
Wire.write(0x58); //4-point avg. and 75Hz rate
Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
Wire.write(0x00); //In Mode_Register: continuous measurement mode
Wire.endTransmission();
pinMode(13, INPUT);
#ifdef CAL_DEBUG
Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
#endif
calibrate_gyros();
calibrate_accel();
accelgyro.resetFIFO();
}
void main(void)
{
char counting_loops = 0;
// initialize board
Sys_Init();
putchar(' ');
All_Init();
Accel_Init(); // Provided function in i2c.h for accelerometer use
PCA0CP2 = 0xFFFF - PW_NEUT;
setup_count = 25;
while (setup_count);
LCD_calibrate_steering();
printf("\rSteering Calibrated\n");
lcd_print("Steering Calibrated\n");
LCD_prompts();
setup_count = 20;
prev_h_counts = h_counts; //start the checking flag
while(1)
{
if(!SSgain && calibrate_flag)
{
calibrate_accel();
calibrate_flag = 0;
//printf("calibrated once\n\r");
}
else if (SSgain){
get_four_readings(); //if the flag is down then we update the car
//printf("running damn car");
Run_Car();
}
counting_loops = counting_loops ? counting_loops-1 : 5;
//printf("once through loop %d \n\r",counting_loops);
if(!counting_loops)
{
Info_LCD_print();
//X accel. Y accel. DPW, SPW, total_time
printf("%d, %d, %d, %d, %d\n\r", gx, gy, motor_pw, r, total_time);
}
}
}
