Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //INSTITUTO POLITECNICO NACIONAL
- //ESCUELA SUPERIOR DE INGENIERIA MECANICA Y ELECTRICA
- //UNIDAD ZACATENCO
- //
- //ING. EN CONTROL Y AUTOMATIZACION
- //ASIGNATURA: INTERFASES Y MICROCONTROLADORES
- //M.C LUIS ARTURO BENITEZ VELAZQUEZ
- //GRUPO 7AM1
- //
- //AUTORES:
- //ROSALES HERNANDEZ JOSE EDUARDO
- //RUIZ ESTUDILLO MARIA FERNANDA
- //***********************************************************************************
- //***********************************************************************************
- //PRACTICA NO. 5
- //PROGRAMA: CONTROL E INVERSION DE GIRO DE UN MOTOR CD CON SENSOR STICK 9DOF
- /***************************************************************************************************************
- * Razor AHRS Firmware v1.4.2
- * 9 Degree of Measurement Attitude and Heading Reference System
- * for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
- * and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
- *
- * Released under GNU GPL (General Public License) v3.0
- * Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
- * Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
- *
- * Infos, updates, bug reports, contributions and feedback:
- * https://github.com/ptrbrtz/razor-9dof-ahrs
- *
- *
- * History:
- * * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio,
- * based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you!
- *
- * * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com)
- * for new Sparkfun 9DOF Razor hardware (SEN-10125).
- *
- * * Updated and extended by Peter Bartz (peter-bartz@gmx.de):
- * * v1.3.0
- * * Cleaned up, streamlined and restructured most of the code to make it more comprehensible.
- * * Added sensor calibration (improves precision and responsiveness a lot!).
- * * Added binary yaw/pitch/roll output.
- * * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc.
- * * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible).
- * * Wrote new easier to use test program (using Processing).
- * * Added support for new version of "9DOF Razor IMU": SEN-10736.
- * --> The output of this code is not compatible with the older versions!
- * --> A Processing sketch to test the tracker is available.
- * * v1.3.1
- * * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away.
- * * Adjusted gyro low-pass filter and output rate settings.
- * * v1.3.2
- * * Adapted code to work with new Arduino 1.0 (and older versions still).
- * * v1.3.3
- * * Improved synching.
- * * v1.4.0
- * * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724).
- * * v1.4.1
- * * Added output modes to read raw and/or calibrated sensor data in text or binary format.
- * * Added static magnetometer soft iron distortion compensation
- * * v1.4.2
- * * (No core firmware changes)
- *
- * TODOs:
- * * Allow optional use of EEPROM for storing and reading calibration values.
- * * Use self-test and temperature-compensation features of the sensors.
- ***************************************************************************************************************/
- /*
- "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736
- ATMega328@3.3V, 8MHz
- ADXL345 : Accelerometer
- HMC5843 : Magnetometer on SEN-10125
- HMC5883L : Magnetometer on SEN-10736
- ITG-3200 : Gyro
- Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328"
- */
- /*
- "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724
- ADXL345 : Accelerometer
- HMC5843 : Magnetometer on SEN-10183 and SEN-10321
- HMC5883L : Magnetometer on SEN-10724
- ITG-3200 : Gyro
- */
- /*
- Axis definition (differs from definition printed on the board!):
- X axis pointing forward (towards the short edge with the connector holes)
- Y axis pointing to the right
- and Z axis pointing down.
- Positive yaw : clockwise
- Positive roll : right wing down
- Positive pitch : nose up
- Transformation order: first yaw then pitch then roll.
- */
- /*
- Serial commands that the firmware understands:
- "#o<params>" - Set OUTPUT mode and parameters. The available options are:
- // Streaming output
- "#o0" - DISABLE continuous streaming output. Also see #f below.
- "#o1" - ENABLE continuous streaming output.
- // Angles output
- "#ob" - Output angles in BINARY format (yaw/pitch/roll as binary float, so one output frame
- is 3x4 = 12 bytes long).
- "#ot" - Output angles in TEXT format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
- followed by carriage return and line feed [\r\n]).
- // Sensor calibration
- "#oc" - Go to CALIBRATION output mode.
- "#on" - When in calibration mode, go on to calibrate NEXT sensor.
- // Sensor data output
- "#osct" - Output CALIBRATED SENSOR data of all 9 axes in TEXT format.
- One frame consist of three lines - one for each sensor: acc, mag, gyr.
- "#osrt" - Output RAW SENSOR data of all 9 axes in TEXT format.
- One frame consist of three lines - one for each sensor: acc, mag, gyr.
- "#osbt" - Output BOTH raw and calibrated SENSOR data of all 9 axes in TEXT format.
- One frame consist of six lines - like #osrt and #osct combined (first RAW, then CALIBRATED).
- NOTE: This is a lot of number-to-text conversion work for the little 8MHz chip on the Razor boards.
- In fact it's too much and an output frame rate of 50Hz can not be maintained. #osbb.
- "#oscb" - Output CALIBRATED SENSOR data of all 9 axes in BINARY format.
- One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
- "#osrb" - Output RAW SENSOR data of all 9 axes in BINARY format.
- One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
- "#osbb" - Output BOTH raw and calibrated SENSOR data of all 9 axes in BINARY format.
- One frame consist of 2x36 = 72 bytes - like #osrb and #oscb combined (first RAW, then CALIBRATED).
- // Error message output
- "#oe0" - Disable ERROR message output.
- "#oe1" - Enable ERROR message output.
- "#f" - Request one output frame - useful when continuous output is disabled and updates are
- required in larger intervals only. Though #f only requests one reply, replies are still
- bound to the internal 20ms (50Hz) time raster. So worst case delay that #f can add is 19.99ms.
- "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous
- binary stream or to see if tracker is present and answering. The tracker will send
- "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function).
- x and y are two mandatory but arbitrary bytes that can be used to find out which request
- the answer belongs to.
- ("#C" and "#D" - Reserved for communication with optional Bluetooth module.)
- Newline characters are not required. So you could send "#ob#o1#s", which
- would set binary output mode, enable continuous streaming output and request
- a synch token all at once.
- The status LED will be on if streaming output is enabled and off otherwise.
- Byte order of binary output is little-endian: least significant byte comes first.
- */
- /*****************************************************************/
- /*********** USER SETUP AREA! Set your options here! *************/
- /*****************************************************************/
- //LIBRARY LCD
- #include "LiquidCrystal.h"
- // HARDWARE OPTIONS
- /*****************************************************************/
- // Select your hardware here by uncommenting one line!
- //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
- //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
- //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
- //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
- #define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
- // OUTPUT OPTIONS
- /*****************************************************************/
- // Set your serial port baud rate used to send out data here!
- #define OUTPUT__BAUD_RATE 9600
- // Sensor data output interval in milliseconds
- // This may not work, if faster than 20ms (=50Hz)
- // Code is tuned for 20ms, so better leave it like that
- #define OUTPUT__DATA_INTERVAL 20 // in milliseconds
- // Output mode definitions (do not change)
- #define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration
- #define OUTPUT__MODE_ANGLES 1 // Outputs yaw/pitch/roll in degrees
- #define OUTPUT__MODE_SENSORS_CALIB 2 // Outputs calibrated sensor values for all 9 axes
- #define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes
- #define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes
- // Output format definitions (do not change)
- #define OUTPUT__FORMAT_TEXT 0 // Outputs data as text
- #define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float
- // Select your startup output mode and format here!
- int output_mode = OUTPUT__MODE_ANGLES;
- int output_format = OUTPUT__FORMAT_TEXT;
- // Select if serial continuous streaming output is enabled per default on startup.
- #define OUTPUT__STARTUP_STREAM_ON true // true or false
- // If set true, an error message will be output if we fail to read sensor data.
- // Message format: "!ERR: reading <sensor>", followed by "\r\n".
- boolean output_errors = true; // true or false
- // Bluetooth
- // You can set this to true, if you have a Rovering Networks Bluetooth Module attached.
- // The connect/disconnect message prefix of the module has to be set to "#".
- // (Refer to manual, it can be set like this: SO,#)
- // When using this, streaming output will only be enabled as long as we're connected. That way
- // receiver and sender are synchronzed easily just by connecting/disconnecting.
- // It is not necessary to set this! It just makes life easier when writing code for
- // the receiving side. The Processing test sketch also works without setting this.
- // NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect!
- #define OUTPUT__HAS_RN_BLUETOOTH false // true or false
- // SENSOR CALIBRATION
- /*****************************************************************/
- // How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
- // Put MIN/MAX and OFFSET readings for your board here!
- // Accelerometer
- // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
- #define ACCEL_X_MIN ((float) -138)
- #define ACCEL_X_MAX ((float) 58)
- #define ACCEL_Y_MIN ((float) -355)
- #define ACCEL_Y_MAX ((float) 272)
- #define ACCEL_Z_MIN ((float) -223)
- #define ACCEL_Z_MAX ((float) 370)
- // Magnetometer (standard calibration mode)
- // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
- #define MAGN_X_MIN ((float) -22)
- #define MAGN_X_MAX ((float) 96)
- #define MAGN_Y_MIN ((float) 155)
- #define MAGN_Y_MAX ((float) 303)
- #define MAGN_Z_MIN ((float) -263)
- #define MAGN_Z_MAX ((float) -114)
- // Magnetometer (extended calibration mode)
- // Uncommend to use extended magnetometer calibration (compensates hard & soft iron errors)
- //#define CALIBRATION__MAGN_USE_EXTENDED true
- //const float magn_ellipsoid_center[3] = {0, 0, 0};
- //const float magn_ellipsoid_transform[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
- // Gyroscope
- // "gyro x,y,z (current/average) = .../OFFSET_X .../OFFSET_Y .../OFFSET_Z
- #define GYRO_AVERAGE_OFFSET_X ((float) -28.32)
- #define GYRO_AVERAGE_OFFSET_Y ((float) 3.16)
- #define GYRO_AVERAGE_OFFSET_Z ((float) 5.08)
- /*
- // Calibration example:
- // "accel x,y,z (min/max) = -277.00/264.00 -256.00/278.00 -299.00/235.00"
- #define ACCEL_X_MIN ((float) -277)
- #define ACCEL_X_MAX ((float) 264)
- #define ACCEL_Y_MIN ((float) -256)
- #define ACCEL_Y_MAX ((float) 278)
- #define ACCEL_Z_MIN ((float) -299)
- #define ACCEL_Z_MAX ((float) 235)
- // "magn x,y,z (min/max) = -511.00/581.00 -516.00/568.00 -489.00/486.00"
- //#define MAGN_X_MIN ((float) -511)
- //#define MAGN_X_MAX ((float) 581)
- //#define MAGN_Y_MIN ((float) -516)
- //#define MAGN_Y_MAX ((float) 568)
- //#define MAGN_Z_MIN ((float) -489)
- //#define MAGN_Z_MAX ((float) 486)
- // Extended magn
- #define CALIBRATION__MAGN_USE_EXTENDED true
- const float magn_ellipsoid_center[3] = {91.5, -13.5, -48.1};
- const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.00354, 0.9, -0.00599}, {0.000636, -0.00599, 1}};
- // Extended magn (with Sennheiser HD 485 headphones)
- //#define CALIBRATION__MAGN_USE_EXTENDED true
- //const float magn_ellipsoid_center[3] = {72.3360, 23.0954, 53.6261};
- //const float magn_ellipsoid_transform[3][3] = {{0.879685, 0.000540833, -0.0106054}, {0.000540833, 0.891086, -0.0130338}, {-0.0106054, -0.0130338, 0.997494}};
- //"gyro x,y,z (current/average) = -40.00/-42.05 98.00/96.20 -18.00/-18.36"
- #define GYRO_AVERAGE_OFFSET_X ((float) -42.05)
- #define GYRO_AVERAGE_OFFSET_Y ((float) 96.20)
- #define GYRO_AVERAGE_OFFSET_Z ((float) -18.36)
- */
- // DEBUG OPTIONS
- /*****************************************************************/
- // When set to true, gyro drift correction will not be applied
- #define DEBUG__NO_DRIFT_CORRECTION false
- // Print elapsed time after each I/O loop
- #define DEBUG__PRINT_LOOP_TIME false
- /*****************************************************************/
- /****************** END OF USER SETUP AREA! *********************/
- /*****************************************************************/
- // Check if hardware version code is defined
- #ifndef HW__VERSION_CODE
- // Generate compile error
- #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.ino!
- #endif
- #include <Wire.h>
- // Sensor calibration scale and offset values
- #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
- #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
- #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
- #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
- #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
- #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
- #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
- #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
- #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
- #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
- #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
- #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
- // Gain for gyroscope (ITG-3200)
- #define GYRO_GAIN 0.06957 // Same gain on all axes
- #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
- // DCM parameters
- #define Kp_ROLLPITCH 0.02f
- #define Ki_ROLLPITCH 0.00002f
- #define Kp_YAW 1.2f
- #define Ki_YAW 0.00002f
- // Stuff
- #define STATUS_LED_PIN 13 // Pin number of status LED
- #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
- #define TO_RAD(x) (x * 0.01745329252) // *pi/180
- #define TO_DEG(x) (x * 57.2957795131) // *180/pi
- //LCD variables
- LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
- byte charGrado[8] = {//creando 'º'
- 0b01110,
- 0b01010,
- 0b01110,
- 0b00000,
- 0b00000,
- 0b00000,
- 0b00000,
- 0b00000
- };
- // Sensor variables
- float accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
- float accel_min[3];
- float accel_max[3];
- float magnetom[3];
- float magnetom_min[3];
- float magnetom_max[3];
- float magnetom_tmp[3];
- float gyro[3];
- float gyro_average[3];
- int gyro_num_samples = 0;
- // DCM variables
- float MAG_Heading;
- float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector
- float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros turn rate in a vector
- float Omega_Vector[3]= {0, 0, 0}; // Corrected Gyro_Vector data
- float Omega_P[3]= {0, 0, 0}; // Omega Proportional correction
- float Omega_I[3]= {0, 0, 0}; // Omega Integrator
- float Omega[3]= {0, 0, 0};
- float errorRollPitch[3] = {0, 0, 0};
- float errorYaw[3] = {0, 0, 0};
- float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
- float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
- // Euler angles
- float yaw;
- float pitch;
- float roll;
- // DCM timing in the main loop
- unsigned long timestamp;
- unsigned long timestamp_old;
- float G_Dt; // Integration time for DCM algorithm
- // More output-state variables
- boolean output_stream_on;
- boolean output_single_on;
- int curr_calibration_sensor = 0;
- boolean reset_calibration_session_flag = true;
- int num_accel_errors = 0;
- int num_magn_errors = 0;
- int num_gyro_errors = 0;
- void read_sensors() {
- Read_Gyro(); // Read gyroscope
- Read_Accel(); // Read accelerometer
- Read_Magn(); // Read magnetometer
- }
- // Read every sensor and record a time stamp
- // Init DCM with unfiltered orientation
- // TODO re-init global vars?
- void reset_sensor_fusion() {
- float temp1[3];
- float temp2[3];
- float xAxis[] = {1.0f, 0.0f, 0.0f};
- read_sensors();
- timestamp = millis();
- // GET PITCH
- // Using y-z-plane-component/x-component of gravity vector
- pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
- // GET ROLL
- // Compensate pitch of gravity vector
- Vector_Cross_Product(temp1, accel, xAxis);
- Vector_Cross_Product(temp2, xAxis, temp1);
- // Normally using x-z-plane-component/y-component of compensated gravity vector
- // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
- // Since we compensated for pitch, x-z-plane-component equals z-component:
- roll = atan2(temp2[1], temp2[2]);
- // GET YAW
- Compass_Heading();
- yaw = MAG_Heading;
- // Init rotation matrix
- init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
- }
- // Apply calibration to raw sensor readings
- void compensate_sensor_errors() {
- // Compensate accelerometer error
- accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
- accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
- accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
- // Compensate magnetometer error
- #if CALIBRATION__MAGN_USE_EXTENDED == true
- for (int i = 0; i < 3; i++)
- magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i];
- Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom);
- #else
- magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
- magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
- magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
- #endif
- // Compensate gyroscope error
- gyro[0] -= GYRO_AVERAGE_OFFSET_X;
- gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
- gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
- }
- // Reset calibration session if reset_calibration_session_flag is set
- void check_reset_calibration_session()
- {
- // Raw sensor values have to be read already, but no error compensation applied
- // Reset this calibration session?
- if (!reset_calibration_session_flag) return;
- // Reset acc and mag calibration variables
- for (int i = 0; i < 3; i++) {
- accel_min[i] = accel_max[i] = accel[i];
- magnetom_min[i] = magnetom_max[i] = magnetom[i];
- }
- // Reset gyro calibration variables
- gyro_num_samples = 0; // Reset gyro calibration averaging
- gyro_average[0] = gyro_average[1] = gyro_average[2] = 0.0f;
- reset_calibration_session_flag = false;
- }
- void turn_output_stream_on()
- {
- output_stream_on = true;
- digitalWrite(STATUS_LED_PIN, HIGH);
- }
- void turn_output_stream_off()
- {
- output_stream_on = false;
- digitalWrite(STATUS_LED_PIN, LOW);
- }
- // Blocks until another byte is available on serial port
- char readChar()
- {
- while (Serial.available() < 1) { } // Block
- return Serial.read();
- }
- void setup()
- {
- //inicializacion de la LCD
- lcd.begin(16, 2);
- lcd.setCursor(0,0);
- lcd.print("IPN ESIME ZAC");
- lcd.setCursor(0,1);
- lcd.print(" SEGWAY ");
- delay (3000);
- lcd.clear();
- lcd.createChar(0, charGrado);
- // Init serial output
- Serial.begin(OUTPUT__BAUD_RATE);
- // Init status LED
- pinMode (STATUS_LED_PIN, OUTPUT);
- digitalWrite(STATUS_LED_PIN, LOW);
- // Init sensors
- delay(50); // Give sensors enough time to start
- I2C_Init();
- Accel_Init();
- Magn_Init();
- Gyro_Init();
- // Read sensors, init DCM algorithm
- delay(20); // Give sensors enough time to collect data
- reset_sensor_fusion();
- // Init output
- #if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
- turn_output_stream_off();
- #else
- turn_output_stream_on();
- #endif
- pinMode(9,OUTPUT);
- pinMode(10,OUTPUT);
- }
- // Main loop
- void loop()
- {
- // Read incoming control messages
- if (Serial.available() >= 2)
- {
- if (Serial.read() == '#') // Start of new control message
- {
- int command = Serial.read(); // Commands
- if (command == 'f') // request one output _f_rame
- output_single_on = true;
- else if (command == 's') // _s_ynch request
- {
- // Read ID
- byte id[2];
- id[0] = readChar();
- id[1] = readChar();
- // Reply with synch message
- Serial.print("#SYNCH");
- Serial.write(id, 2);
- Serial.println();
- }
- else if (command == 'o') // Set _o_utput mode
- {
- char output_param = readChar();
- if (output_param == 'n') // Calibrate _n_ext sensor
- {
- curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
- reset_calibration_session_flag = true;
- }
- else if (output_param == 't') // Output angles as _t_ext
- {
- output_mode = OUTPUT__MODE_ANGLES;
- output_format = OUTPUT__FORMAT_TEXT;
- }
- else if (output_param == 'b') // Output angles in _b_inary format
- {
- output_mode = OUTPUT__MODE_ANGLES;
- output_format = OUTPUT__FORMAT_BINARY;
- }
- else if (output_param == 'c') // Go to _c_alibration mode
- {
- output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
- reset_calibration_session_flag = true;
- }
- else if (output_param == 's') // Output _s_ensor values
- {
- char values_param = readChar();
- char format_param = readChar();
- if (values_param == 'r') // Output _r_aw sensor values
- output_mode = OUTPUT__MODE_SENSORS_RAW;
- else if (values_param == 'c') // Output _c_alibrated sensor values
- output_mode = OUTPUT__MODE_SENSORS_CALIB;
- else if (values_param == 'b') // Output _b_oth sensor values (raw and calibrated)
- output_mode = OUTPUT__MODE_SENSORS_BOTH;
- if (format_param == 't') // Output values as _t_text
- output_format = OUTPUT__FORMAT_TEXT;
- else if (format_param == 'b') // Output values in _b_inary format
- output_format = OUTPUT__FORMAT_BINARY;
- }
- else if (output_param == '0') // Disable continuous streaming output
- {
- turn_output_stream_off();
- reset_calibration_session_flag = true;
- }
- else if (output_param == '1') // Enable continuous streaming output
- {
- reset_calibration_session_flag = true;
- turn_output_stream_on();
- }
- else if (output_param == 'e') // _e_rror output settings
- {
- char error_param = readChar();
- if (error_param == '0') output_errors = false;
- else if (error_param == '1') output_errors = true;
- else if (error_param == 'c') // get error count
- {
- Serial.print("#AMG-ERR:");
- Serial.print(num_accel_errors); Serial.print(",");
- Serial.print(num_magn_errors); Serial.print(",");
- Serial.println(num_gyro_errors);
- }
- }
- }
- #if OUTPUT__HAS_RN_BLUETOOTH == true
- // Read messages from bluetooth module
- // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
- else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
- turn_output_stream_on();
- else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
- turn_output_stream_off();
- #endif // OUTPUT__HAS_RN_BLUETOOTH == true
- }
- else
- { } // Skip character
- }
- // Time to read the sensors again?
- if((millis() - timestamp) >= OUTPUT__DATA_INTERVAL)
- {
- timestamp_old = timestamp;
- timestamp = millis();
- if (timestamp > timestamp_old)
- G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
- else G_Dt = 0;
- // Update sensor readings
- read_sensors();
- float roll2;
- float roll3;
- if(roll>0)
- {
- roll2=40+((roll/3.1416)*(255-40));
- analogWrite(9,roll2);
- // digitalWrite(9,HIGH);
- }
- else
- analogWrite(9,0);
- // digitalWrite(9,LOW);
- if(roll<0)
- {
- roll3=255+(((roll+3.1416)/3.1416)*(40-255));
- analogWrite(10,roll3);
- // digitalWrite(10,HIGH);
- }
- else
- analogWrite(10,0);
- //digitalWrite(10,LOW);
- LCD();
- if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) // We're in calibration mode
- {
- check_reset_calibration_session(); // Check if this session needs a reset
- if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
- }
- else if (output_mode == OUTPUT__MODE_ANGLES) // Output angles
- {
- // Apply sensor calibration
- compensate_sensor_errors();
- // Run DCM algorithm
- Compass_Heading(); // Calculate magnetic heading
- Matrix_update();
- Normalize();
- Drift_correction();
- Euler_angles();
- if (output_stream_on || output_single_on) output_angles();
- }
- else // Output sensor values
- {
- if (output_stream_on || output_single_on) output_sensors();
- }
- output_single_on = false;
- #if DEBUG__PRINT_LOOP_TIME == true
- Serial.print("loop time (ms) = ");
- Serial.println(millis() - timestamp);
- #endif
- }
- #if DEBUG__PRINT_LOOP_TIME == true
- else
- {
- Serial.println("waiting...");
- }
- #endif
- }
- void LCD()
- {
- float roll4;
- roll4=roll*180/3.1416;
- lcd.setCursor(0,0);
- lcd.print("SENSOR:");
- for(int i=7;i<=16;i++)
- {
- lcd.setCursor(i,0);
- lcd.print(" ");
- }
- lcd.setCursor(7,0);
- lcd.print(roll4);
- lcd.setCursor(15,0);
- lcd.write(byte(0)); //caracter 'º'
- lcd.setCursor(0,1);
- lcd.print("SENTIDO:");
- for(int i=9;i<=16;i++)
- {
- lcd.setCursor(i,1);
- lcd.print(" ");
- }
- if(roll<0)
- {
- lcd.setCursor(9,1);
- lcd.print("IZQ.");
- }else
- {
- lcd.setCursor(9,1);
- lcd.print("DERECHA");
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement