GyroSensor.cpp

00001 /**
00002  *****************************************************************************
00003  * File Name    : GyroSensor.cpp
00004  *
00005  * Title        : Gyro sensor Class Source File
00006  * Revision     : 0.1
00007  * Notes        :
00008  * Target Board : mbed NXP LPC1768
00009  * Tool Chain   : ????
00010  *
00011  * Revision History:
00012  * When         Who         Description of change
00013  * -----------  ----------- -----------------------
00014  * 2012/07/10   Hiroshi M   init
00015  *****************************************************************************
00016  *
00017  * Copyright (C) 2012 Hiroshi M, MIT License
00018  *
00019  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00020  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00021  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00022  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00023  * furnished to do so, subject to the following conditions:
00024  *
00025  * The above copyright notice and this permission notice shall be included in all copies or
00026  * substantial portions of the Software.
00027  *
00028  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00029  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00030  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00031  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00032  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00033  *
00034  **/
00035 
00036 /* Includes ----------------------------------------------------------------- */
00037 #include "GyroSensor.h"
00038 #include "mbed.h"
00039 #include "util.h"
00040 
00041 /* Private typedef ---------------------------------------------------------- */
00042 /* Private define ----------------------------------------------------------- */
00043 /* Private macro ------------------------------------------------------------ */
00044 /* Private variables -------------------------------------------------------- */
00045 static int data[16];
00046 
00047 /* member fanctions --------------------------------------------------------- */
00048 
00049 // Constractor
00050 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)
00051 :gyro_adc_in(_pin_no)
00052 {
00053     callback = _p_callback;
00054     getDistanceFeedback = _p_feedback;
00055     getDisatnceGain = _p_disatnce_gain;
00056 }
00057 
00058 // Destrutctor
00059 GyroSensor::~GyroSensor()
00060 {
00061 }
00062 
00063 GyroState GyroSensor::getState(void)
00064 {
00065     return state;
00066 }
00067 
00068 int GyroSensor::getAngle(void)
00069 {
00070     return gya;            //
00071 }
00072 
00073 int GyroSensor::getVelocity(void)
00074 {
00075     return gyv;            //
00076 }
00077 
00078 int GyroSensor::getOffset(void)
00079 {
00080     return offset;         //
00081 }
00082 
00083 void GyroSensor::setReady(void)
00084 {
00085     state = READY;
00086 }
00087 
00088 void GyroSensor::setIdle(void)
00089 {
00090     state = IDLE;
00091 }
00092 
00093 void GyroSensor::ad_conv(void) 
00094 {
00095     static int i = 0;
00096     static float sum = 0.0;
00097     float gyro_volt;
00098 
00099     if (i++==32) {
00100         gyro_volt = (3.3 * sum) / 32.0;
00101         adcav = (int)(1024 * gyro_volt*1.5);
00102         sum = 0.0;
00103         i = 0;
00104         gyro_control();
00105     } else {
00106         sum += gyro_adc_in.read();// Gyro read
00107     }
00108 }
00109 
00110 
00111 
00112 void GyroSensor::getParameter(void)
00113 {
00114     if(getDisatnceGain!=NULL) {
00115         d_gain = getDisatnceGain();
00116     }
00117     else {
00118         d_gain = 1;
00119     }
00120     
00121     if (getDistanceFeedback!=NULL) {
00122         d_fb = getDistanceFeedback();
00123     }
00124     else {
00125         d_fb = 0; 
00126     }
00127 }
00128 
00129 void GyroSensor::gyro_control(void) {
00130     int i;
00131     static int x = 0;
00132 
00133     switch (state) {
00134         case CLEAR:
00135             gyaa = 0;
00136             gyv = 0;
00137             gya = 0;
00138             x = 0;
00139             offset_count = 0;
00140             
00141             d_fb = 0;
00142             d_gain = 0;
00143            
00144             offset_sum_old = offset_sum_old_ini;
00145             offset_sum = offset_sum_ini;
00146             state = INI;
00147             break;
00148 
00149         case INI:
00150             if (offset_count == AV_COUNT) {
00151                 for (i = 0; i < 15; i++) {
00152                     data[i] = data[i + 1];
00153                 }
00154                 offset = (int)(offset_sum / AV_COUNT);
00155                 data[15] = offset;
00156                 offset_sum_old += offset_sum;
00157                 offset_ini_count++;
00158                 offset_sum = 0;
00159                 offset_count = 0;
00160             }
00161             if (offset_ini_count == INI_COUNT) {
00162                 offset = (int)(offset_sum_old / ((int)AV_COUNT * INI_COUNT));
00163                 offset_sum_old = offset_sum_old / INI_COUNT;
00164                 offset_sum_ini = offset_sum;
00165                 offset_sum_old_ini = offset_sum_old;
00166                 state = IDLE;
00167                 break;
00168             }
00169             offset_sum += adcav;
00170             offset_count++;
00171             break;
00172 
00173         case IDLE:
00174             break;
00175 
00176         case READY:
00177             getParameter(); // get d_fb & d_gain.
00178             if (offset_count == AV_COUNT) {
00179                 if (((offset_sum - offset_sum_old) < (int)AV_COUNT * 4) && ((offset_sum - offset_sum_old) > (int)AV_COUNT * -4)) {
00180                     offset = (int)((offset_sum + d_fb / d_gain) / AV_COUNT);
00181                     offset_sum_old = offset_sum;
00182                     x= 0;
00183                     for (i = 0; i < 15; i++) {
00184                         data[i] = data[i + 1];
00185                     }
00186                     data[15] = offset;
00187                 } else {
00188                     offset = lsm(data, x++) + d_fb / (d_gain * AV_COUNT);
00189                     offset_sum_old = offset * AV_COUNT;
00190                 }
00191                 offset_sum = 0;
00192                 offset_count = 0;
00193             }
00194             offset_sum += adcav;
00195             offset_count++;
00196             gyv = (adcav - offset) / 8;
00197             gyaa += gyv;
00198             gya = gyaa / 32;
00199             if (callback!=NULL) {
00200                 state = callback(gyv, gyv, offset);
00201             }
00202             break;
00203 
00204         default:
00205             state = CLEAR;
00206             break;
00207     }
00208 }
00209 
00210 void GyroSensor::Enable(void)
00211 {
00212     Pulse.attach_us(this,&GyroSensor::ad_conv,312);
00213 }
00214 
00215 void GyroSensor::Disable(void)
00216 {
00217     Pulse.detach();
00218 }