Revision 0:da10b9c9db7a, committed 2012-07-16

Comitter:

bant62

Date:

Mon Jul 16 03:42:01 2012 +0000

Commit message:

init revision

Changed in this revision

--- /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();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GyroSensor.h	Mon Jul 16 03:42:01 2012 +0000
@@ -0,0 +1,108 @@
+/**
+ *****************************************************************************
+ * File Name    : GyroSensor.h
+ *
+ * Title        : Gyro Sensor Class Header 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.
+ *
+ **/
+
+#ifndef MBED_GYRO_SENSOR_H
+#define MBED_GYRO_SENSOR_H
+
+/* Includes ----------------------------------------------------------------- */
+#include "mbed.h"
+
+/* typedef ------------------------------------------------------------------ */
+typedef enum {CLEAR,INI,IDLE,READY} GyroState;
+
+typedef GyroState (*GYRO_CALLBACK_FUNC)(int gyv, int gyv, int offset);
+typedef int (*GET_DISTANCE_GAIN_FUNC)(void);
+typedef int (*GET_DISTANCE_FEEDBACK_FUNC)(void);
+
+/* define ------------------------------------------------------------------- */
+#define INI_COUNT 16
+#define AV_COUNT 32
+
+/* macro -------------------------------------------------------------------- */
+/* variables ---------------------------------------------------------------- */
+
+/* class define --------------------------------------------------------------*/
+class GyroSensor
+{
+public:
+    GyroSensor(PinName _pin_no, GYRO_CALLBACK_FUNC _p_callack, GET_DISTANCE_FEEDBACK_FUNC _p_feedback, GET_DISTANCE_GAIN_FUNC _p_disatnce_gain);
+    ~GyroSensor();
+    
+    void Enable(void);
+    void Disable(void);
+    
+    GyroState getState(void);
+    int getAngle(void);
+    int getVelocity(void);
+    int getOffset(void);
+    
+    void setReady(void);
+    void setIdle(void);
+    
+
+private:
+    int gya;            //
+    int gyv;            //
+    int offset;         //
+
+    int adcav;
+    int offset_count;
+    int offset_ini_count;
+    long int offset_sum;
+    long int offset_sum_old;
+    long int offset_sum_ini;
+    long int offset_sum_old_ini;
+    long int gyaa;
+    
+    int d_fb;
+    int d_gain;
+
+    GyroState state;
+    
+    GYRO_CALLBACK_FUNC          callback;
+    GET_DISTANCE_FEEDBACK_FUNC  getDistanceFeedback;
+    GET_DISTANCE_GAIN_FUNC      getDisatnceGain;
+   
+    void getParameter(void); 
+    
+    AnalogIn    gyro_adc_in;
+    Ticker      Pulse;
+    
+    void ad_conv(void);
+    void gyro_control(void);
+
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/util.c	Mon Jul 16 03:42:01 2012 +0000
@@ -0,0 +1,78 @@
+/**
+ *****************************************************************************
+ * File Name    : util.c
+ *
+ * Title        : Utility functions 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 "util.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function prototypes -----------------------------------------------*/
+
+/**
+ * least squares method
+ *
+ */
+int lsm(int *y, int x)
+{
+    int n;
+    static long a0, a1, a2;
+    long s01 = 0, s02 = 0, s11 = 0, s12 = 0;
+
+    if (x == 0) {
+        for (n = 0; n < N; n++) {
+            s01 += n;
+            s02 += (long)y[n];
+            s11 += n * n;
+            s12 += n * (long)y[n];
+        }
+
+        a0 = (s02 * s11 - s01 * s12) / (N * s11 - s01 * s01);
+        a1 = (N * s12 - s01 * s02);
+        a2 = (N * s11 - s01 * s01 * 2);
+    }
+    return (int)(a0 + a1 * (N + x) / a2);
+}
+
+
+int xabs(int x,int y)
+{
+    if(x >= y) {
+        return (x - y);
+    } else {
+        return (y - x);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/util.h	Mon Jul 16 03:42:01 2012 +0000
@@ -0,0 +1,60 @@
+/**
+ *****************************************************************************
+ * File Name    : util.c
+ *
+ * Title        : Utility functions Header 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.
+ *
+ **/
+
+#ifndef MBED_MY_UTIL_H
+#define MBED_MY_UTIL_H
+
+/* Includes ------------------------------------------------------------------*/
+/* typedef -------------------------------------------------------------------*/
+/* define --------------------------------------------------------------------*/
+/* macro ---------------------------------------------------------------------*/
+#define N 16
+
+/* variables -----------------------------------------------------------------*/
+/* function prototypes -------------------------------------------------------*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int lsm(int *y, int x);
+int xabs(int x,int y);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif