Gyro Sensor Library
GyroSensor.cpp
- Committer:
- bant62
- Date:
- 2012-07-16
- Revision:
- 0:da10b9c9db7a
File content as of revision 0:da10b9c9db7a:
/** ***************************************************************************** * File Name : GyroSensor.cpp * * Title : Gyro sensor Class Source File * Revision : 0.1 * Notes : * Target Board : mbed NXP LPC1768 * Tool Chain : ???? * * Revision History: * When Who Description of change * ----------- ----------- ----------------------- * 2012/07/10 Hiroshi M init ***************************************************************************** * * Copyright (C) 2012 Hiroshi M, MIT License * * Permission is hereby granted, free of charge, to any person obtaining a copy of this software * and associated documentation files (the "Software"), to deal in the Software without restriction, * including without limitation the rights to use, copy, modify, merge, publish, distribute, * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all copies or * substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. * **/ /* Includes ----------------------------------------------------------------- */ #include "GyroSensor.h" #include "mbed.h" #include "util.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ static int data[16]; /* member fanctions --------------------------------------------------------- */ // Constractor 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) :gyro_adc_in(_pin_no) { callback = _p_callback; getDistanceFeedback = _p_feedback; getDisatnceGain = _p_disatnce_gain; } // Destrutctor GyroSensor::~GyroSensor() { } GyroState GyroSensor::getState(void) { return state; } int GyroSensor::getAngle(void) { return gya; // } int GyroSensor::getVelocity(void) { return gyv; // } int GyroSensor::getOffset(void) { return offset; // } void GyroSensor::setReady(void) { state = READY; } void GyroSensor::setIdle(void) { state = IDLE; } void GyroSensor::ad_conv(void) { static int i = 0; static float sum = 0.0; float gyro_volt; if (i++==32) { gyro_volt = (3.3 * sum) / 32.0; adcav = (int)(1024 * gyro_volt*1.5); sum = 0.0; i = 0; gyro_control(); } else { sum += gyro_adc_in.read();// Gyro read } } void GyroSensor::getParameter(void) { if(getDisatnceGain!=NULL) { d_gain = getDisatnceGain(); } else { d_gain = 1; } if (getDistanceFeedback!=NULL) { d_fb = getDistanceFeedback(); } else { d_fb = 0; } } void GyroSensor::gyro_control(void) { int i; static int x = 0; switch (state) { case CLEAR: gyaa = 0; gyv = 0; gya = 0; x = 0; offset_count = 0; d_fb = 0; d_gain = 0; offset_sum_old = offset_sum_old_ini; offset_sum = offset_sum_ini; state = INI; break; case INI: if (offset_count == AV_COUNT) { for (i = 0; i < 15; i++) { data[i] = data[i + 1]; } offset = (int)(offset_sum / AV_COUNT); data[15] = offset; offset_sum_old += offset_sum; offset_ini_count++; offset_sum = 0; offset_count = 0; } if (offset_ini_count == INI_COUNT) { offset = (int)(offset_sum_old / ((int)AV_COUNT * INI_COUNT)); offset_sum_old = offset_sum_old / INI_COUNT; offset_sum_ini = offset_sum; offset_sum_old_ini = offset_sum_old; state = IDLE; break; } offset_sum += adcav; offset_count++; break; case IDLE: break; case READY: getParameter(); // get d_fb & d_gain. if (offset_count == AV_COUNT) { if (((offset_sum - offset_sum_old) < (int)AV_COUNT * 4) && ((offset_sum - offset_sum_old) > (int)AV_COUNT * -4)) { offset = (int)((offset_sum + d_fb / d_gain) / AV_COUNT); offset_sum_old = offset_sum; x= 0; for (i = 0; i < 15; i++) { data[i] = data[i + 1]; } data[15] = offset; } else { offset = lsm(data, x++) + d_fb / (d_gain * AV_COUNT); offset_sum_old = offset * AV_COUNT; } offset_sum = 0; offset_count = 0; } offset_sum += adcav; offset_count++; gyv = (adcav - offset) / 8; gyaa += gyv; gya = gyaa / 32; if (callback!=NULL) { state = callback(gyv, gyv, offset); } break; default: state = CLEAR; break; } } void GyroSensor::Enable(void) { Pulse.attach_us(this,&GyroSensor::ad_conv,312); } void GyroSensor::Disable(void) { Pulse.detach(); }