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?
User | Revision | Line number | New 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 | } |