Chad Conway / Mbed 2 deprecated GyroMouse1

tttttt

Dependencies:   mbed X_NUCLEO_IKS01A2

main.cpp@0:26594709b159, 2020-03-20 (annotated)

Committer:
Nextdaycode
Date:
Fri Mar 20 22:45:29 2020 +0000
Revision:
0:26594709b159
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nextdaycode 0:26594709b159 1 /**
Nextdaycode 0:26594709b159 2 ******************************************************************************
Nextdaycode 0:26594709b159 3 * @file main.cpp
Nextdaycode 0:26594709b159 4 * @author CLab
Nextdaycode 0:26594709b159 5 * @version V1.0.0
Nextdaycode 0:26594709b159 6 * @date 2-December-2016
Nextdaycode 0:26594709b159 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
Nextdaycode 0:26594709b159 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
Nextdaycode 0:26594709b159 9 ******************************************************************************
Nextdaycode 0:26594709b159 10 * @attention
Nextdaycode 0:26594709b159 11 *
Nextdaycode 0:26594709b159 12 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
Nextdaycode 0:26594709b159 13 *
Nextdaycode 0:26594709b159 14 * Redistribution and use in source and binary forms, with or without modification,
Nextdaycode 0:26594709b159 15 * are permitted provided that the following conditions are met:
Nextdaycode 0:26594709b159 16 * 1. Redistributions of source code must retain the above copyright notice,
Nextdaycode 0:26594709b159 17 * this list of conditions and the following disclaimer.
Nextdaycode 0:26594709b159 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Nextdaycode 0:26594709b159 19 * this list of conditions and the following disclaimer in the documentation
Nextdaycode 0:26594709b159 20 * and/or other materials provided with the distribution.
Nextdaycode 0:26594709b159 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Nextdaycode 0:26594709b159 22 * may be used to endorse or promote products derived from this software
Nextdaycode 0:26594709b159 23 * without specific prior written permission.
Nextdaycode 0:26594709b159 24 *
Nextdaycode 0:26594709b159 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Nextdaycode 0:26594709b159 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Nextdaycode 0:26594709b159 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Nextdaycode 0:26594709b159 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Nextdaycode 0:26594709b159 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Nextdaycode 0:26594709b159 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Nextdaycode 0:26594709b159 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Nextdaycode 0:26594709b159 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Nextdaycode 0:26594709b159 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Nextdaycode 0:26594709b159 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Nextdaycode 0:26594709b159 35 *
Nextdaycode 0:26594709b159 36 ******************************************************************************
Nextdaycode 0:26594709b159 37 */
Nextdaycode 0:26594709b159 38
Nextdaycode 0:26594709b159 39 /* Includes */
Nextdaycode 0:26594709b159 40 #include "mbed.h"
Nextdaycode 0:26594709b159 41 #include "XNucleoIKS01A2.h"
Nextdaycode 0:26594709b159 42
Nextdaycode 0:26594709b159 43 /* Instantiate the expansion board */
Nextdaycode 0:26594709b159 44 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
Nextdaycode 0:26594709b159 45
Nextdaycode 0:26594709b159 46 /* Retrieve the composing elements of the expansion board */
Nextdaycode 0:26594709b159 47 //static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
Nextdaycode 0:26594709b159 48 //static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
Nextdaycode 0:26594709b159 49 //static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
Nextdaycode 0:26594709b159 50 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
Nextdaycode 0:26594709b159 51 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
Nextdaycode 0:26594709b159 52
Nextdaycode 0:26594709b159 53 /* Helper function for printing floats & doubles */
Nextdaycode 0:26594709b159 54 static char *print_double(char* str, double v, int decimalDigits=2)
Nextdaycode 0:26594709b159 55 {
Nextdaycode 0:26594709b159 56 int i = 1;
Nextdaycode 0:26594709b159 57 int intPart, fractPart;
Nextdaycode 0:26594709b159 58 int len;
Nextdaycode 0:26594709b159 59 char *ptr;
Nextdaycode 0:26594709b159 60
Nextdaycode 0:26594709b159 61 /* prepare decimal digits multiplicator */
Nextdaycode 0:26594709b159 62 for (;decimalDigits!=0; i*=10, decimalDigits--);
Nextdaycode 0:26594709b159 63
Nextdaycode 0:26594709b159 64 /* calculate integer & fractinal parts */
Nextdaycode 0:26594709b159 65 intPart = (int)v;
Nextdaycode 0:26594709b159 66 fractPart = (int)((v-(double)(int)v)*i);
Nextdaycode 0:26594709b159 67
Nextdaycode 0:26594709b159 68 /* fill in integer part */
Nextdaycode 0:26594709b159 69 sprintf(str, "%i.", intPart);
Nextdaycode 0:26594709b159 70
Nextdaycode 0:26594709b159 71 /* prepare fill in of fractional part */
Nextdaycode 0:26594709b159 72 len = strlen(str);
Nextdaycode 0:26594709b159 73 ptr = &str[len];
Nextdaycode 0:26594709b159 74
Nextdaycode 0:26594709b159 75 /* fill in leading fractional zeros */
Nextdaycode 0:26594709b159 76 for (i/=10;i>1; i/=10, ptr++) {
Nextdaycode 0:26594709b159 77 if (fractPart >= i) {
Nextdaycode 0:26594709b159 78 break;
Nextdaycode 0:26594709b159 79 }
Nextdaycode 0:26594709b159 80 *ptr = '0';
Nextdaycode 0:26594709b159 81 }
Nextdaycode 0:26594709b159 82
Nextdaycode 0:26594709b159 83 /* fill in (rest of) fractional part */
Nextdaycode 0:26594709b159 84 sprintf(ptr, "%i", fractPart);
Nextdaycode 0:26594709b159 85
Nextdaycode 0:26594709b159 86 return str;
Nextdaycode 0:26594709b159 87 }
Nextdaycode 0:26594709b159 88
Nextdaycode 0:26594709b159 89 /* Simple main function */
Nextdaycode 0:26594709b159 90 int main() {
Nextdaycode 0:26594709b159 91 uint8_t id;
Nextdaycode 0:26594709b159 92 int32_t offsets[3];
Nextdaycode 0:26594709b159 93 int32_t lastAxes[3];
Nextdaycode 0:26594709b159 94 int32_t currAxes[3];
Nextdaycode 0:26594709b159 95 char buffer1[32], buffer2[32];
Nextdaycode 0:26594709b159 96 int32_t axes[3];
Nextdaycode 0:26594709b159 97
Nextdaycode 0:26594709b159 98 /* Enable all sensors */
Nextdaycode 0:26594709b159 99 // hum_temp->enable();
Nextdaycode 0:26594709b159 100 // press_temp->enable();
Nextdaycode 0:26594709b159 101 // magnetometer->enable();
Nextdaycode 0:26594709b159 102 accelerometer->enable();
Nextdaycode 0:26594709b159 103 acc_gyro->enable_x();
Nextdaycode 0:26594709b159 104 acc_gyro->enable_g();
Nextdaycode 0:26594709b159 105
Nextdaycode 0:26594709b159 106 printf("\r\n--- Starting new run ---\r\n");
Nextdaycode 0:26594709b159 107
Nextdaycode 0:26594709b159 108 // wait for sensor to stabilize
Nextdaycode 0:26594709b159 109 wait(2);
Nextdaycode 0:26594709b159 110 // hum_temp->read_id(&id);
Nextdaycode 0:26594709b159 111 // printf("HTS221 humidity & temperature = 0x%X\r\n", id);
Nextdaycode 0:26594709b159 112 // press_temp->read_id(&id);
Nextdaycode 0:26594709b159 113 // printf("LPS22HB pressure & temperature = 0x%X\r\n", id);
Nextdaycode 0:26594709b159 114 // magnetometer->read_id(&id);
Nextdaycode 0:26594709b159 115 // printf("LSM303AGR magnetometer = 0x%X\r\n", id);
Nextdaycode 0:26594709b159 116 accelerometer->read_id(&id);
Nextdaycode 0:26594709b159 117 // printf("LSM303AGR accelerometer = 0x%X\r\n", id);
Nextdaycode 0:26594709b159 118 acc_gyro->read_id(&id);
Nextdaycode 0:26594709b159 119
Nextdaycode 0:26594709b159 120 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
Nextdaycode 0:26594709b159 121
Nextdaycode 0:26594709b159 122 // Read and display initial values
Nextdaycode 0:26594709b159 123 acc_gyro->get_g_axes(axes);
Nextdaycode 0:26594709b159 124 printf("Initial Gyro Readings: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
Nextdaycode 0:26594709b159 125
Nextdaycode 0:26594709b159 126 // Calculate offset for each axes and store in offsets
Nextdaycode 0:26594709b159 127 // Then initialize lastAxes to calibrated values
Nextdaycode 0:26594709b159 128 for(int i = 0; i < 3;i++){
Nextdaycode 0:26594709b159 129 offsets[i] = axes[i] * (-1);
Nextdaycode 0:26594709b159 130 lastAxes[i] = axes[i] + offsets[i];
Nextdaycode 0:26594709b159 131 }
Nextdaycode 0:26594709b159 132 // Display offsets
Nextdaycode 0:26594709b159 133 printf("Offsets set to: %6ld, %6ld, %6ld\r\n", offsets[0], offsets[1], offsets[2]);
Nextdaycode 0:26594709b159 134
Nextdaycode 0:26594709b159 135 // Display initial calibrated values should be at or near 0 for all
Nextdaycode 0:26594709b159 136 printf("Calibrated Gyro Settings: %6ld, %6ld, %6ld\r\n", lastAxes[0], lastAxes[1], lastAxes[2]);
Nextdaycode 0:26594709b159 137
Nextdaycode 0:26594709b159 138
Nextdaycode 0:26594709b159 139 while(1) {
Nextdaycode 0:26594709b159 140 // printf("\r\n");
Nextdaycode 0:26594709b159 141
Nextdaycode 0:26594709b159 142 // hum_temp->get_temperature(&value1);
Nextdaycode 0:26594709b159 143 // hum_temp->get_humidity(&value2);
Nextdaycode 0:26594709b159 144 // printf("HTS221: [temp] %7s C, [hum] %s%%\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
Nextdaycode 0:26594709b159 145 //
Nextdaycode 0:26594709b159 146 // press_temp->get_temperature(&value1);
Nextdaycode 0:26594709b159 147 // press_temp->get_pressure(&value2);
Nextdaycode 0:26594709b159 148 // printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
Nextdaycode 0:26594709b159 149
Nextdaycode 0:26594709b159 150 printf("---\r\n");
Nextdaycode 0:26594709b159 151
Nextdaycode 0:26594709b159 152 // magnetometer->get_m_axes(axes);
Nextdaycode 0:26594709b159 153 // printf("LSM303AGR [mag/mgauss]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
Nextdaycode 0:26594709b159 154
Nextdaycode 0:26594709b159 155 // accelerometer->get_x_axes(axes);
Nextdaycode 0:26594709b159 156 // printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
Nextdaycode 0:26594709b159 157
Nextdaycode 0:26594709b159 158 // acc_gyro->get_x_axes(axes);
Nextdaycode 0:26594709b159 159 // printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
Nextdaycode 0:26594709b159 160
Nextdaycode 0:26594709b159 161 acc_gyro->get_g_axes(axes);
Nextdaycode 0:26594709b159 162 for(int i = 0; i < 3;i++){
Nextdaycode 0:26594709b159 163 currAxes[i] = axes[i] + offsets[i];
Nextdaycode 0:26594709b159 164 }
Nextdaycode 0:26594709b159 165 // print current calibrated readings
Nextdaycode 0:26594709b159 166 printf("Axes Readings: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
Nextdaycode 0:26594709b159 167 printf("Offsets: %6ld, %6ld, %6ld\r\n", offsets[0], offsets[1], offsets[2]);
Nextdaycode 0:26594709b159 168 printf("Adjusted Readings: %6ld, %6ld, %6ld\r\n", currAxes[0], currAxes[1], currAxes[2]);
Nextdaycode 0:26594709b159 169
Nextdaycode 0:26594709b159 170 // Set lastAxes to current readings
Nextdaycode 0:26594709b159 171 for(int i = 0; i < 3;i++){
Nextdaycode 0:26594709b159 172 lastAxes[i] = axes[i] + offsets[i];
Nextdaycode 0:26594709b159 173 }
Nextdaycode 0:26594709b159 174 wait(1.5);
Nextdaycode 0:26594709b159 175 }
Nextdaycode 0:26594709b159 176 }