Hiroshi M / GyroSensor

Gyro Sensor Library

GyroSensor.cpp@0:da10b9c9db7a, 2012-07-16 (annotated)

Committer:
bant62
Date:
Mon Jul 16 03:42:01 2012 +0000
Revision:
0:da10b9c9db7a
init revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bant62 0:da10b9c9db7a 1 /**
bant62 0:da10b9c9db7a 2 *****************************************************************************
bant62 0:da10b9c9db7a 3 * File Name : GyroSensor.cpp
bant62 0:da10b9c9db7a 4 *
bant62 0:da10b9c9db7a 5 * Title : Gyro sensor Class Source File
bant62 0:da10b9c9db7a 6 * Revision : 0.1
bant62 0:da10b9c9db7a 7 * Notes :
bant62 0:da10b9c9db7a 8 * Target Board : mbed NXP LPC1768
bant62 0:da10b9c9db7a 9 * Tool Chain : ????
bant62 0:da10b9c9db7a 10 *
bant62 0:da10b9c9db7a 11 * Revision History:
bant62 0:da10b9c9db7a 12 * When Who Description of change
bant62 0:da10b9c9db7a 13 * ----------- ----------- -----------------------
bant62 0:da10b9c9db7a 14 * 2012/07/10 Hiroshi M init
bant62 0:da10b9c9db7a 15 *****************************************************************************
bant62 0:da10b9c9db7a 16 *
bant62 0:da10b9c9db7a 17 * Copyright (C) 2012 Hiroshi M, MIT License
bant62 0:da10b9c9db7a 18 *
bant62 0:da10b9c9db7a 19 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
bant62 0:da10b9c9db7a 20 * and associated documentation files (the "Software"), to deal in the Software without restriction,
bant62 0:da10b9c9db7a 21 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
bant62 0:da10b9c9db7a 22 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
bant62 0:da10b9c9db7a 23 * furnished to do so, subject to the following conditions:
bant62 0:da10b9c9db7a 24 *
bant62 0:da10b9c9db7a 25 * The above copyright notice and this permission notice shall be included in all copies or
bant62 0:da10b9c9db7a 26 * substantial portions of the Software.
bant62 0:da10b9c9db7a 27 *
bant62 0:da10b9c9db7a 28 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
bant62 0:da10b9c9db7a 29 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
bant62 0:da10b9c9db7a 30 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
bant62 0:da10b9c9db7a 31 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
bant62 0:da10b9c9db7a 32 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
bant62 0:da10b9c9db7a 33 *
bant62 0:da10b9c9db7a 34 **/
bant62 0:da10b9c9db7a 35
bant62 0:da10b9c9db7a 36 /* Includes ----------------------------------------------------------------- */
bant62 0:da10b9c9db7a 37 #include "GyroSensor.h"
bant62 0:da10b9c9db7a 38 #include "mbed.h"
bant62 0:da10b9c9db7a 39 #include "util.h"
bant62 0:da10b9c9db7a 40
bant62 0:da10b9c9db7a 41 /* Private typedef ---------------------------------------------------------- */
bant62 0:da10b9c9db7a 42 /* Private define ----------------------------------------------------------- */
bant62 0:da10b9c9db7a 43 /* Private macro ------------------------------------------------------------ */
bant62 0:da10b9c9db7a 44 /* Private variables -------------------------------------------------------- */
bant62 0:da10b9c9db7a 45 static int data[16];
bant62 0:da10b9c9db7a 46
bant62 0:da10b9c9db7a 47 /* member fanctions --------------------------------------------------------- */
bant62 0:da10b9c9db7a 48
bant62 0:da10b9c9db7a 49 // Constractor
bant62 0:da10b9c9db7a 50 GyroSensor::GyroSensor(PinName _pin_no=p15, GYRO_CALLBACK_FUNC _p_callback=NULL, GET_DISTANCE_FEEDBACK_FUNC _p_feedback=NULL, GET_DISTANCE_GAIN_FUNC _p_disatnce_gain=NULL)
bant62 0:da10b9c9db7a 51 :gyro_adc_in(_pin_no)
bant62 0:da10b9c9db7a 52 {
bant62 0:da10b9c9db7a 53 callback = _p_callback;
bant62 0:da10b9c9db7a 54 getDistanceFeedback = _p_feedback;
bant62 0:da10b9c9db7a 55 getDisatnceGain = _p_disatnce_gain;
bant62 0:da10b9c9db7a 56 }
bant62 0:da10b9c9db7a 57
bant62 0:da10b9c9db7a 58 // Destrutctor
bant62 0:da10b9c9db7a 59 GyroSensor::~GyroSensor()
bant62 0:da10b9c9db7a 60 {
bant62 0:da10b9c9db7a 61 }
bant62 0:da10b9c9db7a 62
bant62 0:da10b9c9db7a 63 GyroState GyroSensor::getState(void)
bant62 0:da10b9c9db7a 64 {
bant62 0:da10b9c9db7a 65 return state;
bant62 0:da10b9c9db7a 66 }
bant62 0:da10b9c9db7a 67
bant62 0:da10b9c9db7a 68 int GyroSensor::getAngle(void)
bant62 0:da10b9c9db7a 69 {
bant62 0:da10b9c9db7a 70 return gya; //
bant62 0:da10b9c9db7a 71 }
bant62 0:da10b9c9db7a 72
bant62 0:da10b9c9db7a 73 int GyroSensor::getVelocity(void)
bant62 0:da10b9c9db7a 74 {
bant62 0:da10b9c9db7a 75 return gyv; //
bant62 0:da10b9c9db7a 76 }
bant62 0:da10b9c9db7a 77
bant62 0:da10b9c9db7a 78 int GyroSensor::getOffset(void)
bant62 0:da10b9c9db7a 79 {
bant62 0:da10b9c9db7a 80 return offset; //
bant62 0:da10b9c9db7a 81 }
bant62 0:da10b9c9db7a 82
bant62 0:da10b9c9db7a 83 void GyroSensor::setReady(void)
bant62 0:da10b9c9db7a 84 {
bant62 0:da10b9c9db7a 85 state = READY;
bant62 0:da10b9c9db7a 86 }
bant62 0:da10b9c9db7a 87
bant62 0:da10b9c9db7a 88 void GyroSensor::setIdle(void)
bant62 0:da10b9c9db7a 89 {
bant62 0:da10b9c9db7a 90 state = IDLE;
bant62 0:da10b9c9db7a 91 }
bant62 0:da10b9c9db7a 92
bant62 0:da10b9c9db7a 93 void GyroSensor::ad_conv(void)
bant62 0:da10b9c9db7a 94 {
bant62 0:da10b9c9db7a 95 static int i = 0;
bant62 0:da10b9c9db7a 96 static float sum = 0.0;
bant62 0:da10b9c9db7a 97 float gyro_volt;
bant62 0:da10b9c9db7a 98
bant62 0:da10b9c9db7a 99 if (i++==32) {
bant62 0:da10b9c9db7a 100 gyro_volt = (3.3 * sum) / 32.0;
bant62 0:da10b9c9db7a 101 adcav = (int)(1024 * gyro_volt*1.5);
bant62 0:da10b9c9db7a 102 sum = 0.0;
bant62 0:da10b9c9db7a 103 i = 0;
bant62 0:da10b9c9db7a 104 gyro_control();
bant62 0:da10b9c9db7a 105 } else {
bant62 0:da10b9c9db7a 106 sum += gyro_adc_in.read();// Gyro read
bant62 0:da10b9c9db7a 107 }
bant62 0:da10b9c9db7a 108 }
bant62 0:da10b9c9db7a 109
bant62 0:da10b9c9db7a 110
bant62 0:da10b9c9db7a 111
bant62 0:da10b9c9db7a 112 void GyroSensor::getParameter(void)
bant62 0:da10b9c9db7a 113 {
bant62 0:da10b9c9db7a 114 if(getDisatnceGain!=NULL) {
bant62 0:da10b9c9db7a 115 d_gain = getDisatnceGain();
bant62 0:da10b9c9db7a 116 }
bant62 0:da10b9c9db7a 117 else {
bant62 0:da10b9c9db7a 118 d_gain = 1;
bant62 0:da10b9c9db7a 119 }
bant62 0:da10b9c9db7a 120
bant62 0:da10b9c9db7a 121 if (getDistanceFeedback!=NULL) {
bant62 0:da10b9c9db7a 122 d_fb = getDistanceFeedback();
bant62 0:da10b9c9db7a 123 }
bant62 0:da10b9c9db7a 124 else {
bant62 0:da10b9c9db7a 125 d_fb = 0;
bant62 0:da10b9c9db7a 126 }
bant62 0:da10b9c9db7a 127 }
bant62 0:da10b9c9db7a 128
bant62 0:da10b9c9db7a 129 void GyroSensor::gyro_control(void) {
bant62 0:da10b9c9db7a 130 int i;
bant62 0:da10b9c9db7a 131 static int x = 0;
bant62 0:da10b9c9db7a 132
bant62 0:da10b9c9db7a 133 switch (state) {
bant62 0:da10b9c9db7a 134 case CLEAR:
bant62 0:da10b9c9db7a 135 gyaa = 0;
bant62 0:da10b9c9db7a 136 gyv = 0;
bant62 0:da10b9c9db7a 137 gya = 0;
bant62 0:da10b9c9db7a 138 x = 0;
bant62 0:da10b9c9db7a 139 offset_count = 0;
bant62 0:da10b9c9db7a 140
bant62 0:da10b9c9db7a 141 d_fb = 0;
bant62 0:da10b9c9db7a 142 d_gain = 0;
bant62 0:da10b9c9db7a 143
bant62 0:da10b9c9db7a 144 offset_sum_old = offset_sum_old_ini;
bant62 0:da10b9c9db7a 145 offset_sum = offset_sum_ini;
bant62 0:da10b9c9db7a 146 state = INI;
bant62 0:da10b9c9db7a 147 break;
bant62 0:da10b9c9db7a 148
bant62 0:da10b9c9db7a 149 case INI:
bant62 0:da10b9c9db7a 150 if (offset_count == AV_COUNT) {
bant62 0:da10b9c9db7a 151 for (i = 0; i < 15; i++) {
bant62 0:da10b9c9db7a 152 data[i] = data[i + 1];
bant62 0:da10b9c9db7a 153 }
bant62 0:da10b9c9db7a 154 offset = (int)(offset_sum / AV_COUNT);
bant62 0:da10b9c9db7a 155 data[15] = offset;
bant62 0:da10b9c9db7a 156 offset_sum_old += offset_sum;
bant62 0:da10b9c9db7a 157 offset_ini_count++;
bant62 0:da10b9c9db7a 158 offset_sum = 0;
bant62 0:da10b9c9db7a 159 offset_count = 0;
bant62 0:da10b9c9db7a 160 }
bant62 0:da10b9c9db7a 161 if (offset_ini_count == INI_COUNT) {
bant62 0:da10b9c9db7a 162 offset = (int)(offset_sum_old / ((int)AV_COUNT * INI_COUNT));
bant62 0:da10b9c9db7a 163 offset_sum_old = offset_sum_old / INI_COUNT;
bant62 0:da10b9c9db7a 164 offset_sum_ini = offset_sum;
bant62 0:da10b9c9db7a 165 offset_sum_old_ini = offset_sum_old;
bant62 0:da10b9c9db7a 166 state = IDLE;
bant62 0:da10b9c9db7a 167 break;
bant62 0:da10b9c9db7a 168 }
bant62 0:da10b9c9db7a 169 offset_sum += adcav;
bant62 0:da10b9c9db7a 170 offset_count++;
bant62 0:da10b9c9db7a 171 break;
bant62 0:da10b9c9db7a 172
bant62 0:da10b9c9db7a 173 case IDLE:
bant62 0:da10b9c9db7a 174 break;
bant62 0:da10b9c9db7a 175
bant62 0:da10b9c9db7a 176 case READY:
bant62 0:da10b9c9db7a 177 getParameter(); // get d_fb & d_gain.
bant62 0:da10b9c9db7a 178 if (offset_count == AV_COUNT) {
bant62 0:da10b9c9db7a 179 if (((offset_sum - offset_sum_old) < (int)AV_COUNT * 4) && ((offset_sum - offset_sum_old) > (int)AV_COUNT * -4)) {
bant62 0:da10b9c9db7a 180 offset = (int)((offset_sum + d_fb / d_gain) / AV_COUNT);
bant62 0:da10b9c9db7a 181 offset_sum_old = offset_sum;
bant62 0:da10b9c9db7a 182 x= 0;
bant62 0:da10b9c9db7a 183 for (i = 0; i < 15; i++) {
bant62 0:da10b9c9db7a 184 data[i] = data[i + 1];
bant62 0:da10b9c9db7a 185 }
bant62 0:da10b9c9db7a 186 data[15] = offset;
bant62 0:da10b9c9db7a 187 } else {
bant62 0:da10b9c9db7a 188 offset = lsm(data, x++) + d_fb / (d_gain * AV_COUNT);
bant62 0:da10b9c9db7a 189 offset_sum_old = offset * AV_COUNT;
bant62 0:da10b9c9db7a 190 }
bant62 0:da10b9c9db7a 191 offset_sum = 0;
bant62 0:da10b9c9db7a 192 offset_count = 0;
bant62 0:da10b9c9db7a 193 }
bant62 0:da10b9c9db7a 194 offset_sum += adcav;
bant62 0:da10b9c9db7a 195 offset_count++;
bant62 0:da10b9c9db7a 196 gyv = (adcav - offset) / 8;
bant62 0:da10b9c9db7a 197 gyaa += gyv;
bant62 0:da10b9c9db7a 198 gya = gyaa / 32;
bant62 0:da10b9c9db7a 199 if (callback!=NULL) {
bant62 0:da10b9c9db7a 200 state = callback(gyv, gyv, offset);
bant62 0:da10b9c9db7a 201 }
bant62 0:da10b9c9db7a 202 break;
bant62 0:da10b9c9db7a 203
bant62 0:da10b9c9db7a 204 default:
bant62 0:da10b9c9db7a 205 state = CLEAR;
bant62 0:da10b9c9db7a 206 break;
bant62 0:da10b9c9db7a 207 }
bant62 0:da10b9c9db7a 208 }
bant62 0:da10b9c9db7a 209
bant62 0:da10b9c9db7a 210 void GyroSensor::Enable(void)
bant62 0:da10b9c9db7a 211 {
bant62 0:da10b9c9db7a 212 Pulse.attach_us(this,&GyroSensor::ad_conv,312);
bant62 0:da10b9c9db7a 213 }
bant62 0:da10b9c9db7a 214
bant62 0:da10b9c9db7a 215 void GyroSensor::Disable(void)
bant62 0:da10b9c9db7a 216 {
bant62 0:da10b9c9db7a 217 Pulse.detach();
bant62 0:da10b9c9db7a 218 }