void loop() { printGyro(); // Print "G: gx, gy, gz" printAccel(); // Print "A: ax, ay, az" printMag(); // Print "M: mx, my, mz" // Print the heading and orientation for fun! // Call print attitude. The LSM9DS1's magnetometer x and y // axes are opposite to the accelerometer, so my and mx are // substituted for each other. printAttitude(imu.ax, imu.ay, imu.az, -imu.my, -imu.mx, imu.mz); Serial.println(); delay(PRINT_SPEED); }
/** * Main loop. */ int main(void) { int32_t lastSpeedCalc = 0; int32_t lastDebug = 0; int32_t lastDebug2 = 0; // initalize all modules initAll(); // enable interrupts (i.e timers start from here) sei(); // The LOOP while(1) { // read serial port readCommand(); // communication.c // apply speed calculation and run odometer if (uptime() >= lastSpeedCalc + SPEED_CALC_PERIOD) { lastSpeedCalc += SPEED_CALC_PERIOD; calculateSpeeds(); // motors.c if (pidOn) { runPID(); // motors.c } //if (debug1) printSpeed(CPUCHAR); runOdometer(); // odometer.c navigator(); } // read sensors and apply position correction positionCorrection(); // odometer.c // print debug loop 2 if (uptime() >= (lastDebug2 + 20)) { lastDebug2 = uptime(); if (debug6) printf("%d, %d\r\n", p_LsensVal, p_RsensVal); } // print debug if ((debugPeriod) && uptime() >= (lastDebug + debugPeriod)) { lastDebug += debugPeriod; //if (debug1) printf(">21%02x\r", speed0 >> 4); //if (debug1) printf(">21%04x\r>22%04x\r", readADC(0), readADC(1)); //if (debug2) printf("%ld\t%ld\r\n", timer, timer20); //if (debug2) printf_P(PSTR("%d\t%d\r\n"), p_L, p_R); if (debug3) printf_P(PSTR("%d, %d / %d, %d / %d, %d / %u, %u\r\n"), p_Ltrans, p_Lrel, p_Rtrans, p_Rrel, p_Lerr, p_Rerr, posCorrLeftFailed, posCorrRightFailed); //if (debug4) printf_P(PSTR("%ld\t%ld\r\n"), ticks0, ticks1); //if (debug5) printSpeed(CPUCHAR); //if (debug5) printf_P(PSTR("%d\t%d\r\n"), speed0, speed1); //if (debug6) printf_P(PSTR("%d\t%d\r\n"), accel0, accel1); if (debug7) printf_P(PSTR("%u\t%u\r\n"), power0, power1); // debug for GUI printTicks(CPUCHAR); printSpeed(CPUCHAR); printAccel(CPUCHAR); printAbsDist(CPUCHAR); printRelDist(CPUCHAR); printSensors(CPUCHAR); } } while(1); }