Alex Block / Mbed 2 deprecated test

ergwrthsgfhrtxhs

Dependencies:   mbed weelio PID

main.cpp@19:29d3c7caea66, 2019-11-19 (annotated)

Committer:
mitch092
Date:
Tue Nov 19 03:14:08 2019 +0000
Revision:
19:29d3c7caea66
Parent:
18:9b9ec0b35b45
Child:
20:babcf777607b
Removed whitespace and commented out tilt.x, tilt.z and the rest of the serial io print statements.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cparata 0:69566eea0fba 1 /**
cparata 0:69566eea0fba 2 ******************************************************************************
cparata 0:69566eea0fba 3 * @file main.cpp
cparata 8:8f495e604424 4 * @author CLab
cparata 8:8f495e604424 5 * @version V1.0.0
cparata 8:8f495e604424 6 * @date 2-December-2016
mitch092 18:9b9ec0b35b45 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
cparata 0:69566eea0fba 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
cparata 0:69566eea0fba 9 ******************************************************************************
cparata 0:69566eea0fba 10 * @attention
cparata 0:69566eea0fba 11 *
cparata 0:69566eea0fba 12 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
cparata 0:69566eea0fba 13 *
cparata 0:69566eea0fba 14 * Redistribution and use in source and binary forms, with or without modification,
cparata 0:69566eea0fba 15 * are permitted provided that the following conditions are met:
cparata 0:69566eea0fba 16 * 1. Redistributions of source code must retain the above copyright notice,
cparata 0:69566eea0fba 17 * this list of conditions and the following disclaimer.
cparata 0:69566eea0fba 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
cparata 0:69566eea0fba 19 * this list of conditions and the following disclaimer in the documentation
cparata 0:69566eea0fba 20 * and/or other materials provided with the distribution.
cparata 0:69566eea0fba 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
cparata 0:69566eea0fba 22 * may be used to endorse or promote products derived from this software
cparata 0:69566eea0fba 23 * without specific prior written permission.
cparata 0:69566eea0fba 24 *
cparata 0:69566eea0fba 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
cparata 0:69566eea0fba 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
cparata 0:69566eea0fba 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
cparata 0:69566eea0fba 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
cparata 0:69566eea0fba 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
cparata 0:69566eea0fba 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
cparata 0:69566eea0fba 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
cparata 0:69566eea0fba 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
cparata 0:69566eea0fba 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
cparata 0:69566eea0fba 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
cparata 0:69566eea0fba 35 *
cparata 0:69566eea0fba 36 ******************************************************************************
mitch092 18:9b9ec0b35b45 37 */
cparata 0:69566eea0fba 38
cparata 0:69566eea0fba 39 /* Includes */
cparata 0:69566eea0fba 40 #include "mbed.h"
davide.aliprandi@st.com 13:fc873da5b445 41 #include "XNucleoIKS01A2.h"
mitch092 18:9b9ec0b35b45 42 #include <iostream>
mitch092 18:9b9ec0b35b45 43 #include <cmath>
cparata 0:69566eea0fba 44
mitch092 19:29d3c7caea66 45
mitch092 19:29d3c7caea66 46
mitch092 19:29d3c7caea66 47 struct Vec3 {
mitch092 19:29d3c7caea66 48 float x, y, z;
mitch092 19:29d3c7caea66 49 };
mitch092 19:29d3c7caea66 50
cparata 0:69566eea0fba 51 /* Instantiate the expansion board */
davide.aliprandi@st.com 13:fc873da5b445 52 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
cparata 0:69566eea0fba 53
cparata 0:69566eea0fba 54 /* Retrieve the composing elements of the expansion board */
davide.aliprandi@st.com 13:fc873da5b445 55 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
cparata 0:69566eea0fba 56 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
davide.aliprandi@st.com 13:fc873da5b445 57 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
cparata 0:69566eea0fba 58
mitch092 18:9b9ec0b35b45 59
mitch092 18:9b9ec0b35b45 60
mitch092 18:9b9ec0b35b45 61
mitch092 19:29d3c7caea66 62 DigitalOut M1_step(D3);
mitch092 19:29d3c7caea66 63 DigitalOut M1_dir(D2);
mitch092 18:9b9ec0b35b45 64
mitch092 19:29d3c7caea66 65 DigitalOut M2_step(D5);
mitch092 19:29d3c7caea66 66 DigitalOut M2_dir(D4);
mitch092 18:9b9ec0b35b45 67
mitch092 19:29d3c7caea66 68 DigitalIn button1(D6);
mitch092 19:29d3c7caea66 69 DigitalIn button2(D7);
cparata 0:69566eea0fba 70
mitch092 19:29d3c7caea66 71 void delay(float time) //delay function debounce buttons
mitch092 18:9b9ec0b35b45 72 {
mitch092 19:29d3c7caea66 73 volatile int i;
mitch092 19:29d3c7caea66 74 for(i=0; i<1000000*time; i++);
mitch092 19:29d3c7caea66 75 }
cparata 0:69566eea0fba 76
mitch092 18:9b9ec0b35b45 77
mitch092 18:9b9ec0b35b45 78 Vec3 get_accel()
mitch092 18:9b9ec0b35b45 79 {
mitch092 18:9b9ec0b35b45 80 Vec3 vec3;
mitch092 18:9b9ec0b35b45 81 int32_t axes[3];
mitch092 18:9b9ec0b35b45 82 acc_gyro->get_x_axes(axes);
cparata 0:69566eea0fba 83
mitch092 18:9b9ec0b35b45 84 vec3.x = axes[0]/1000.0f + 0.013f;
mitch092 18:9b9ec0b35b45 85 vec3.y = axes[1]/1000.0f + 0.004f;
mitch092 18:9b9ec0b35b45 86 vec3.z = axes[2]/1000.0f - 0.032f;
mitch092 18:9b9ec0b35b45 87
mitch092 18:9b9ec0b35b45 88 return vec3;
mitch092 18:9b9ec0b35b45 89 }
cparata 0:69566eea0fba 90
mitch092 18:9b9ec0b35b45 91 void print_vec3(const char* str, const Vec3& vec3)
mitch092 18:9b9ec0b35b45 92 {
mitch092 18:9b9ec0b35b45 93 std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n";
cparata 0:69566eea0fba 94 }
cparata 0:69566eea0fba 95
mitch092 18:9b9ec0b35b45 96
mitch092 19:29d3c7caea66 97
mitch092 19:29d3c7caea66 98
mitch092 19:29d3c7caea66 99
mitch092 19:29d3c7caea66 100
mitch092 18:9b9ec0b35b45 101 int main()
mitch092 18:9b9ec0b35b45 102 {
mitch092 18:9b9ec0b35b45 103 /* Enable all sensors */
mitch092 18:9b9ec0b35b45 104 magnetometer->enable();
mitch092 18:9b9ec0b35b45 105 accelerometer->enable();
mitch092 18:9b9ec0b35b45 106 acc_gyro->enable_x();
mitch092 18:9b9ec0b35b45 107 acc_gyro->enable_g();
mitch092 18:9b9ec0b35b45 108
mitch092 18:9b9ec0b35b45 109 //Timer t;
mitch092 18:9b9ec0b35b45 110 //float dt = 0.02f;
mitch092 18:9b9ec0b35b45 111
mitch092 19:29d3c7caea66 112 //std::cout << "\r\n--- Starting new run ---\r\n";
mitch092 18:9b9ec0b35b45 113
mitch092 18:9b9ec0b35b45 114 //Vec3 orientation = {};
mitch092 18:9b9ec0b35b45 115
mitch092 18:9b9ec0b35b45 116 //float sens = 0.488f;
mitch092 19:29d3c7caea66 117
mitch092 18:9b9ec0b35b45 118 Vec3 vec3, tilt;
mitch092 19:29d3c7caea66 119
mitch092 18:9b9ec0b35b45 120 // 180/pi -> converts radians to degrees.
mitch092 18:9b9ec0b35b45 121 float rad_to_deg = 57.2957795131f;
mitch092 18:9b9ec0b35b45 122
mitch092 19:29d3c7caea66 123 M1_dir = 1;
mitch092 19:29d3c7caea66 124 M2_dir = 0;
mitch092 19:29d3c7caea66 125
mitch092 18:9b9ec0b35b45 126
mitch092 18:9b9ec0b35b45 127 for(;;) {
mitch092 19:29d3c7caea66 128 vec3 = get_accel();
mitch092 19:29d3c7caea66 129
mitch092 19:29d3c7caea66 130 // We only need x or y; I left the others here for documentation.
mitch092 19:29d3c7caea66 131 //tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg;
mitch092 19:29d3c7caea66 132 tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg;
mitch092 19:29d3c7caea66 133 //tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg;
mitch092 19:29d3c7caea66 134
mitch092 19:29d3c7caea66 135 //control stepper motor 1
mitch092 19:29d3c7caea66 136
mitch092 19:29d3c7caea66 137 float degree = std::abs(tilt.y);
mitch092 19:29d3c7caea66 138
mitch092 19:29d3c7caea66 139 //print_vec3("acceleration: ", tilt);
mitch092 19:29d3c7caea66 140 //std::cout << "\r\n" << std::flush;
mitch092 18:9b9ec0b35b45 141
mitch092 18:9b9ec0b35b45 142
mitch092 19:29d3c7caea66 143 int numOfSteps = degree / 0.45;
mitch092 18:9b9ec0b35b45 144
mitch092 19:29d3c7caea66 145 if(tilt.y > 0) {
mitch092 19:29d3c7caea66 146 M1_dir = 0;
mitch092 19:29d3c7caea66 147 M2_dir = 1;
mitch092 19:29d3c7caea66 148 } else {
mitch092 19:29d3c7caea66 149 M1_dir = 1;
mitch092 19:29d3c7caea66 150 M2_dir = 0;
mitch092 19:29d3c7caea66 151 }
mitch092 18:9b9ec0b35b45 152
mitch092 19:29d3c7caea66 153 for(int i = 0; i < numOfSteps; i++) {
mitch092 19:29d3c7caea66 154 M2_step = 1;
mitch092 19:29d3c7caea66 155 M1_step = 1;
mitch092 19:29d3c7caea66 156 delay(0.005);
mitch092 19:29d3c7caea66 157 M1_step = 0;
mitch092 19:29d3c7caea66 158 M2_step = 0;
mitch092 19:29d3c7caea66 159 delay(0.005);
mitch092 19:29d3c7caea66 160 }
mitch092 19:29d3c7caea66 161 }
mitch092 19:29d3c7caea66 162
mitch092 19:29d3c7caea66 163 }
mitch092 19:29d3c7caea66 164
mitch092 18:9b9ec0b35b45 165
mitch092 18:9b9ec0b35b45 166
cparata 0:69566eea0fba 167
mitch092 18:9b9ec0b35b45 168
mitch092 19:29d3c7caea66 169
mitch092 19:29d3c7caea66 170
mitch092 19:29d3c7caea66 171
mitch092 19:29d3c7caea66 172
mitch092 19:29d3c7caea66 173
mitch092 19:29d3c7caea66 174
mitch092 19:29d3c7caea66 175
mitch092 19:29d3c7caea66 176
mitch092 19:29d3c7caea66 177
mitch092 19:29d3c7caea66 178
mitch092 19:29d3c7caea66 179
mitch092 19:29d3c7caea66 180
mitch092 18:9b9ec0b35b45 181
mitch092 19:29d3c7caea66 182
mitch092 19:29d3c7caea66 183
mitch092 19:29d3c7caea66 184
mitch092 19:29d3c7caea66 185
mitch092 19:29d3c7caea66 186
mitch092 19:29d3c7caea66 187
mitch092 19:29d3c7caea66 188
mitch092 19:29d3c7caea66 189
mitch092 19:29d3c7caea66 190
mitch092 19:29d3c7caea66 191
mitch092 19:29d3c7caea66 192
mitch092 19:29d3c7caea66 193
mitch092 18:9b9ec0b35b45 194
mitch092 19:29d3c7caea66 195
mitch092 19:29d3c7caea66 196
mitch092 19:29d3c7caea66 197
mitch092 19:29d3c7caea66 198
mitch092 19:29d3c7caea66 199
mitch092 19:29d3c7caea66 200
mitch092 19:29d3c7caea66 201
mitch092 19:29d3c7caea66 202
mitch092 19:29d3c7caea66 203
mitch092 19:29d3c7caea66 204
mitch092 19:29d3c7caea66 205
mitch092 19:29d3c7caea66 206
mitch092 19:29d3c7caea66 207
mitch092 19:29d3c7caea66 208
mitch092 19:29d3c7caea66 209
mitch092 19:29d3c7caea66 210
mitch092 19:29d3c7caea66 211
mitch092 19:29d3c7caea66 212
mitch092 19:29d3c7caea66 213
mitch092 19:29d3c7caea66 214
mitch092 19:29d3c7caea66 215
mitch092 19:29d3c7caea66 216
mitch092 19:29d3c7caea66 217
mitch092 19:29d3c7caea66 218
mitch092 19:29d3c7caea66 219
mitch092 18:9b9ec0b35b45 220
mitch092 18:9b9ec0b35b45 221
mitch092 18:9b9ec0b35b45 222
mitch092 18:9b9ec0b35b45 223
mitch092 18:9b9ec0b35b45 224
mitch092 18:9b9ec0b35b45 225
mitch092 18:9b9ec0b35b45 226
mitch092 18:9b9ec0b35b45 227
mitch092 18:9b9ec0b35b45 228
mitch092 19:29d3c7caea66 229
mitch092 19:29d3c7caea66 230
mitch092 19:29d3c7caea66 231
mitch092 19:29d3c7caea66 232
mitch092 19:29d3c7caea66 233
mitch092 19:29d3c7caea66 234
mitch092 19:29d3c7caea66 235
cparata 0:69566eea0fba 236
mitch092 19:29d3c7caea66 237
mitch092 19:29d3c7caea66 238
mitch092 19:29d3c7caea66 239
mitch092 19:29d3c7caea66 240
mitch092 19:29d3c7caea66 241
mitch092 19:29d3c7caea66 242
mitch092 19:29d3c7caea66 243
mitch092 19:29d3c7caea66 244
mitch092 18:9b9ec0b35b45 245
cparata 0:69566eea0fba 246
mitch092 19:29d3c7caea66 247 //static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f};
mitch092 19:29d3c7caea66 248
mitch092 19:29d3c7caea66 249 //Vec3 get_gyro()
mitch092 19:29d3c7caea66 250 //{
mitch092 19:29d3c7caea66 251 // Vec3 vec3;
mitch092 19:29d3c7caea66 252
mitch092 19:29d3c7caea66 253 // int32_t axes[3];
mitch092 19:29d3c7caea66 254 // acc_gyro->get_g_axes(axes);
mitch092 19:29d3c7caea66 255
mitch092 19:29d3c7caea66 256 // vec3.x = axes[0]/1000.0f - gyro_calibrate.x;
mitch092 19:29d3c7caea66 257 // vec3.y = axes[1]/1000.0f - gyro_calibrate.y;
mitch092 19:29d3c7caea66 258 // vec3.z = axes[2]/1000.0f - gyro_calibrate.z;
mitch092 19:29d3c7caea66 259
mitch092 19:29d3c7caea66 260 // return vec3;
mitch092 19:29d3c7caea66 261 //}
mitch092 18:9b9ec0b35b45 262
mitch092 18:9b9ec0b35b45 263
mitch092 19:29d3c7caea66 264
cparata 0:69566eea0fba 265
mitch092 18:9b9ec0b35b45 266
mitch092 18:9b9ec0b35b45 267
mitch092 18:9b9ec0b35b45 268
cparata 0:69566eea0fba 269
mitch092 19:29d3c7caea66 270 /* Simple main function */
mitch092 19:29d3c7caea66 271 //int main()
mitch092 19:29d3c7caea66 272 //{
mitch092 19:29d3c7caea66 273
mitch092 19:29d3c7caea66 274
mitch092 19:29d3c7caea66 275
mitch092 19:29d3c7caea66 276
mitch092 19:29d3c7caea66 277 //for(;;) {
mitch092 19:29d3c7caea66 278 //t.start();
mitch092 19:29d3c7caea66 279
mitch092 19:29d3c7caea66 280
mitch092 19:29d3c7caea66 281 //Vec3 gyro_reading = get_gyro();
mitch092 19:29d3c7caea66 282
mitch092 19:29d3c7caea66 283 //orientation.x += (gyro_reading.x * dt * sens);
mitch092 19:29d3c7caea66 284 //orientation.y += (gyro_reading.y * dt * sens);
mitch092 19:29d3c7caea66 285 //orientation.z += (gyro_reading.z * dt * sens);
mitch092 19:29d3c7caea66 286
mitch092 19:29d3c7caea66 287 //print_vec3("orientation: ", orientation);
mitch092 19:29d3c7caea66 288
mitch092 19:29d3c7caea66 289
mitch092 19:29d3c7caea66 290
mitch092 19:29d3c7caea66 291
mitch092 19:29d3c7caea66 292
mitch092 19:29d3c7caea66 293
mitch092 19:29d3c7caea66 294
mitch092 19:29d3c7caea66 295
mitch092 19:29d3c7caea66 296
mitch092 19:29d3c7caea66 297
mitch092 19:29d3c7caea66 298
mitch092 19:29d3c7caea66 299 //tilt.x
mitch092 19:29d3c7caea66 300
mitch092 19:29d3c7caea66 301
mitch092 18:9b9ec0b35b45 302
cparata 0:69566eea0fba 303
mitch092 19:29d3c7caea66 304 //std::cout << "\r\n" << std::flush;
mitch092 19:29d3c7caea66 305
mitch092 19:29d3c7caea66 306 //t.stop();
mitch092 19:29d3c7caea66 307 //if (dt - t.read() > 0) wait(dt - t.read());
mitch092 19:29d3c7caea66 308 //dt = t.read();
mitch092 19:29d3c7caea66 309 //}
mitch092 19:29d3c7caea66 310
mitch092 19:29d3c7caea66 311
mitch092 19:29d3c7caea66 312
mitch092 19:29d3c7caea66 313
mitch092 19:29d3c7caea66 314
mitch092 19:29d3c7caea66 315
mitch092 19:29d3c7caea66 316
mitch092 19:29d3c7caea66 317
mitch092 19:29d3c7caea66 318
mitch092 19:29d3c7caea66 319 //for(;;) {
mitch092 19:29d3c7caea66 320 //-0.42 -1.61 1.05
mitch092 19:29d3c7caea66 321 //magnetometer->get_m_axes(axes);
mitch092 19:29d3c7caea66 322 //std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
mitch092 19:29d3c7caea66 323
mitch092 19:29d3c7caea66 324 //accelerometer->get_x_axes(axes);
mitch092 19:29d3c7caea66 325 //std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
mitch092 19:29d3c7caea66 326
mitch092 19:29d3c7caea66 327
mitch092 19:29d3c7caea66 328 //acc_gyro->get_x_axes(axes);
mitch092 19:29d3c7caea66 329 //std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
mitch092 19:29d3c7caea66 330
mitch092 19:29d3c7caea66 331
mitch092 19:29d3c7caea66 332 //acc_gyro->get_g_axes(axes);
mitch092 19:29d3c7caea66 333 //std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n";
mitch092 19:29d3c7caea66 334
mitch092 19:29d3c7caea66 335
mitch092 19:29d3c7caea66 336
mitch092 19:29d3c7caea66 337
mitch092 19:29d3c7caea66 338
mitch092 19:29d3c7caea66 339 //print_vec3("gyro/dps: ", get_gyro());
mitch092 19:29d3c7caea66 340 //
mitch092 19:29d3c7caea66 341
mitch092 19:29d3c7caea66 342
mitch092 19:29d3c7caea66 343 //wait(1.5);
mitch092 19:29d3c7caea66 344 //}
mitch092 19:29d3c7caea66 345 //}