Diff: main.cpp

Revision:

0:b888fb67c9cf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 22 21:28:33 2013 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+
+#define PI              3.14159265358979f
+#define FILTER_FREQ     800
+#define FILTER_SPLIT    0.95
+#define ANGLE_SAMPLES   8
+
+//Pin Defines
+DigitalOut  led(LED1);
+Serial      pc(USBTX,USBRX);
+PwmOut      m0(p21);
+PwmOut      m1(p22);
+PwmOut      m2(p23);
+PwmOut      m3(p24);
+ADXL345_I2C accelerometer(p9, p10);
+ITG3200     gyroscope(p9, p10);
+
+Ticker filter_tick;
+Ticker debug_tick;
+
+//Prototypes
+void accelSetup(void);
+void accelSample(void);
+void accelCal(int samples);
+void accelAngle(void);
+void gyroSetup(void);
+void gyroSample(void);
+void gyroCal(int samples);
+void angleIni(void);
+
+//Vars
+float accel[3];
+float accel_offset[3];
+float gyro[3];
+float gyro_offset[3];
+float angle[2];
+float angle_buf[2];
+float angle_sam[2][ANGLE_SAMPLES];
+float last_sample[2];
+
+int buf_cnt = 0;
+
+void filter(void){
+    gyroSample();
+    accelSample();
+    accelAngle();
+    for(int h=0;h<2;h++){
+        float tmpf = (float)FILTER_SPLIT*(angle[h]+(gyro[h]/(float)FILTER_FREQ))+(1-(float)FILTER_SPLIT)*accel[h];
+        angle_buf[h] += tmpf - angle_sam[h][buf_cnt];
+        angle_sam[h][buf_cnt] = tmpf;
+        angle[h] = angle_buf[h]/ANGLE_SAMPLES;
+    }
+    if(buf_cnt++ >= ANGLE_SAMPLES-1) buf_cnt = 0;
+    led = !led;
+}
+
+void debug(void){
+    pc.printf("x:%6.3f y:%6.3f\n\r",angle[0],angle[1]);
+}
+
+int main() {
+    accelSetup();
+    gyroSetup();
+    
+    gyroCal(100);
+    
+    pc.printf("Calibrated\n\r");
+    while(1) if(pc.readable()) break;
+    
+    angleIni();
+    
+    pc.printf("x:%5.3f y:%5.3f\n\r",angle[0],angle[1]);
+    wait(0.5);
+    pc.printf("%d - %d\n\r",(int)(1000000/FILTER_FREQ),ANGLE_SAMPLES);
+    wait(0.5);
+    
+    debug_tick.attach(&debug, 0.1);
+    filter_tick.attach_us(&filter, (int)(1000000/FILTER_FREQ));
+    
+    while(1) {
+ //       pc.printf("x:%5.3f y:%5.3f \n\r",angle[0],angle[1]);
+ //       wait_us(1000);
+    }
+}
+
+void accelSetup(void) {
+    accelerometer.setPowerControl(0x00);        //Go into standby mode to configure the device.
+    accelerometer.setDataFormatControl(0x0B);   //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataRate(ADXL345_200HZ);   //200Hz data rate.
+    accelerometer.setPowerControl(0x08);        //Measurement mode.
+    wait_ms(22);                                //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+}
+
+void accelSample(void){
+    int readings[3];
+    accelerometer.getOutput(readings);
+    for(int h=0;h<3;h++) accel[h] = ((float)(int16_t)readings[h]/256)-accel_offset[h]; 
+}
+
+void accelCal(int samples){
+    float  temp[3]  = {0,0,0};
+    for(int h=0;h<=samples;h++){
+        accelSample();
+        for(int j=0;j<=2;j++) temp[j] += accel[j];
+    }
+    for(int h=0;h<=2;h++) accel_offset[h] = temp[h]/samples;
+    accel_offset[2] -= 1;
+}
+
+void accelAngle(void){
+    accel[0] = atan2(accel[2],accel[0])*(180/(float)PI)-90;
+    accel[1] = atan2(accel[2],accel[1])*(180/(float)PI)-90;
+}
+
+void gyroSetup(void){
+    gyroscope.setLpBandwidth(LPFBW_42HZ);       //Set highest bandwidth.
+    wait_ms(100);
+}
+
+void gyroSample(void){
+    gyro[0] = (gyroscope.getGyroX()-gyro_offset[0])/14.375;
+    gyro[1] = (gyroscope.getGyroY()-gyro_offset[1])/14.375;
+    gyro[2] = (gyroscope.getGyroZ()-gyro_offset[2])/14.375;
+}
+
+void gyroCal(int samples){
+    int temp[3] = {0,0,0};
+    for(int h=0;h<=samples;h++){
+        temp[0] += gyroscope.getGyroX();
+        temp[1] += gyroscope.getGyroY();
+        temp[2] += gyroscope.getGyroZ();
+    } 
+    for(int h=0;h<=2;h++) gyro_offset[h] = temp[h]/samples;
+}
+
+void angleIni(void){
+    for(int h=0;h<2;h++) angle_buf[h] = 0;
+    for(int h=0;h<ANGLE_SAMPLES;h++){
+        accelSample();
+        accelAngle();
+        for(int j=0;j<2;j++){
+            angle_buf[j] += accel[j];
+            angle_sam[j][h] = accel[j];
+        }
+    }
+    for(int h=0;h<2;h++) angle[h] = angle_buf[h]/ANGLE_SAMPLES;
+}
\ No newline at end of file