Hiroshi M
Gyro Sensor Library
Diff: GyroSensor.cpp
- Revision:
- 0:da10b9c9db7a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GyroSensor.cpp Mon Jul 16 03:42:01 2012 +0000
@@ -0,0 +1,218 @@
+/**
+ *****************************************************************************
+ * 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();
+}