TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

SensorsLibrary/FXAS21002.cpp@12:273752f540be, 2016-04-30 (annotated)

Committer:
drelliak
Date:
Sat Apr 30 21:23:13 2016 +0000
Revision:
12:273752f540be
Parent:
11:7f569811a5f1
Child:
14:e8cd237c8639
TrekkingController with the gyroscope class and motor class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
drelliak 0:88faaa1afb83 1 /* Copyright (c) 2015 NXP Semiconductors. MIT License
drelliak 0:88faaa1afb83 2 *
drelliak 0:88faaa1afb83 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
drelliak 0:88faaa1afb83 4 * and associated documentation files (the "Software"), to deal in the Software without
drelliak 0:88faaa1afb83 5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
drelliak 0:88faaa1afb83 6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
drelliak 0:88faaa1afb83 7 * Software is furnished to do so, subject to the following conditions:
drelliak 0:88faaa1afb83 8 *
drelliak 0:88faaa1afb83 9 * The above copyright notice and this permission notice shall be included in all copies or
drelliak 0:88faaa1afb83 10 * substantial portions of the Software.
drelliak 0:88faaa1afb83 11 *
drelliak 0:88faaa1afb83 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
drelliak 0:88faaa1afb83 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
drelliak 0:88faaa1afb83 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
drelliak 0:88faaa1afb83 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
drelliak 0:88faaa1afb83 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
drelliak 0:88faaa1afb83 17 */
drelliak 0:88faaa1afb83 18
drelliak 0:88faaa1afb83 19 #include "FXAS21002.h"
Alexandre Seidy 9:bd0fb9d17803 20 #include "mbed.h"
drelliak 0:88faaa1afb83 21 FXAS21002::FXAS21002(PinName sda, PinName scl) : gyroi2c(sda,scl)
Alexandre Seidy 9:bd0fb9d17803 22 {
drelliak 0:88faaa1afb83 23
Alexandre Seidy 9:bd0fb9d17803 24 }
Alexandre Seidy 9:bd0fb9d17803 25
drelliak 12:273752f540be 26 void FXAS21002::set_gyro(gyro_mode mode) // Protected method
drelliak 12:273752f540be 27 {
Alexandre Seidy 9:bd0fb9d17803 28 char d[2];
Alexandre Seidy 9:bd0fb9d17803 29 d[0] = FXAS21002_CTRL_REG1; //Puts device in standby mode
Alexandre Seidy 9:bd0fb9d17803 30 d[1] = 0x08;
Alexandre Seidy 9:bd0fb9d17803 31 gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
Alexandre Seidy 9:bd0fb9d17803 32
Alexandre Seidy 9:bd0fb9d17803 33 d[0] = FXAS21002_CTRL_REG0; //Sets FSR and Sensitivity
Alexandre Seidy 9:bd0fb9d17803 34 d[1] = mode;
Alexandre Seidy 9:bd0fb9d17803 35 gyroi2c.write(FXAS21002_I2C_ADDRESS, d, 2);
Alexandre Seidy 9:bd0fb9d17803 36
Alexandre Seidy 9:bd0fb9d17803 37 d[0] = FXAS21002_CTRL_REG1; //Puts device in active mode
Alexandre Seidy 9:bd0fb9d17803 38 d[1] = 0x0A;
Alexandre Seidy 9:bd0fb9d17803 39 gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
Alexandre Seidy 9:bd0fb9d17803 40 }
Alexandre Seidy 9:bd0fb9d17803 41
drelliak 12:273752f540be 42
drelliak 12:273752f540be 43 void FXAS21002::stop_measure(void)
drelliak 12:273752f540be 44 {
drelliak 12:273752f540be 45 interrupt.detach();
drelliak 12:273752f540be 46 }
drelliak 12:273752f540be 47
drelliak 12:273752f540be 48 float FXAS21002::get_angle(void)
drelliak 12:273752f540be 49 {
drelliak 12:273752f540be 50 return angle;
drelliak 12:273752f540be 51 }
drelliak 12:273752f540be 52
drelliak 12:273752f540be 53 void FXAS21002::integrate_gyro_angle(void)
drelliak 12:273752f540be 54 {
drelliak 12:273752f540be 55 float gyro_data[3];
drelliak 12:273752f540be 56 acquire_gyro_data_dps(gyro_data);
drelliak 12:273752f540be 57 angle = angle + (gyro_data[2]-GYRO_OFFSET)*(period/1000000);
drelliak 12:273752f540be 58 if(angle > 180)
drelliak 12:273752f540be 59 angle = angle - 360;
drelliak 12:273752f540be 60 if(angle < -180)
drelliak 12:273752f540be 61 angle = angle + 360;
drelliak 12:273752f540be 62 }
drelliak 12:273752f540be 63
drelliak 12:273752f540be 64 void FXAS21002::start_measure(float period_us)
drelliak 12:273752f540be 65 {
drelliak 12:273752f540be 66 period = period_us;
drelliak 12:273752f540be 67 interrupt.attach_us(this,&FXAS21002::integrate_gyro_angle,period);
drelliak 12:273752f540be 68 angle = 0;
drelliak 12:273752f540be 69 }
drelliak 12:273752f540be 70
Alexandre Seidy 9:bd0fb9d17803 71 void FXAS21002::gyro_config(void)
Alexandre Seidy 9:bd0fb9d17803 72 {
Alexandre Seidy 11:7f569811a5f1 73 set_gyro(MODE_2); //Default implementation
Alexandre Seidy 9:bd0fb9d17803 74 sensitivity = 0.03125;
Alexandre Seidy 9:bd0fb9d17803 75 }
drelliak 0:88faaa1afb83 76
drelliak 12:273752f540be 77 void FXAS21002::gyro_config(gyro_mode mode)
drelliak 12:273752f540be 78 {
Alexandre Seidy 9:bd0fb9d17803 79 set_gyro(mode);
Alexandre Seidy 9:bd0fb9d17803 80 switch(mode)
Alexandre Seidy 9:bd0fb9d17803 81 {
Alexandre Seidy 9:bd0fb9d17803 82 case MODE_1: sensitivity = 0.0625; break;
Alexandre Seidy 9:bd0fb9d17803 83 case MODE_2: sensitivity = 0.03125; break;
Alexandre Seidy 9:bd0fb9d17803 84 case MODE_3: sensitivity = 0.015625; break;
Alexandre Seidy 11:7f569811a5f1 85 case MODE_4: sensitivity = 0.0078125; break;
Alexandre Seidy 9:bd0fb9d17803 86 }
Alexandre Seidy 9:bd0fb9d17803 87 }
Alexandre Seidy 9:bd0fb9d17803 88
Alexandre Seidy 9:bd0fb9d17803 89 void FXAS21002::acquire_gyro_data_dps(float * g_data)
Alexandre Seidy 9:bd0fb9d17803 90 {
drelliak 0:88faaa1afb83 91
drelliak 0:88faaa1afb83 92 char data_bytes[7];
Alexandre Seidy 9:bd0fb9d17803 93 gyroi2c.write(FXAS21002_I2C_ADDRESS,FXAS21002_STATUS,1,true); // Read the 6 data bytes - LSB and MSB for X, Y and Z Axes.
Alexandre Seidy 9:bd0fb9d17803 94 gyroi2c.read(FXAS21002_I2C_ADDRESS,data_bytes,7);
Alexandre Seidy 9:bd0fb9d17803 95
Alexandre Seidy 9:bd0fb9d17803 96 g_data[0] = (float)((int16_t)((data_bytes[1]*256) + (data_bytes[2]))) * sensitivity;
Alexandre Seidy 9:bd0fb9d17803 97 g_data[1] = (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * sensitivity;
Alexandre Seidy 9:bd0fb9d17803 98 g_data[2] = (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * sensitivity;
drelliak 0:88faaa1afb83 99
Alexandre Seidy 9:bd0fb9d17803 100 }
drelliak 0:88faaa1afb83 101
drelliak 0:88faaa1afb83 102
drelliak 0:88faaa1afb83 103
drelliak 0:88faaa1afb83 104