Revision 2:452dd766d212, committed 2013-11-01

Comitter:

teamgoat

Date:

Fri Nov 01 00:48:06 2013 +0000

Parent:

1:1abb115c2005

Child:

3:f9e18a9cd9af

Commit message:

broken :(

Changed in this revision

--- a/control.cpp	Wed Oct 30 22:51:33 2013 +0000
+++ b/control.cpp	Fri Nov 01 00:48:06 2013 +0000
@@ -7,23 +7,20 @@
 
 process procs[MAXPROC] = {0};
 
-void schedule();
-void init_procs();
-
-
 int main()
 {
-    /*init_procs();
+    init();
     while (true)
     {
-        get_sensor_data();
         schedule();
-    }*/
-    get_sensor_data();
+    }
 }
 
-void init_procs()
+void init()
 {
+    // initialize i2c sensors
+    init_sensors();
+    // set initial processes
     procs[0].status = READY;
     procs[0].start = &get_sensor_data;
     return;

--- a/control.h	Wed Oct 30 22:51:33 2013 +0000
+++ b/control.h	Fri Nov 01 00:48:06 2013 +0000
@@ -1,6 +1,9 @@
 #ifndef CONTROL_H
 #define CONTROL_H
 
+void schedule();
+void init();
+
 typedef struct
 {
     unsigned int status;

--- a/sensor.cpp	Wed Oct 30 22:51:33 2013 +0000
+++ b/sensor.cpp	Fri Nov 01 00:48:06 2013 +0000
@@ -1,15 +1,21 @@
 #include "sensor.h"
 
-I2C i2c(PTE1, PTE0);
+
+extern Serial pc;
+I2C i2c(p9, p10);
 
 char set_i2c_pointer(char addr, char reg)
 {
     if (i2c.write(addr) == 0)
     {
+        if (DEBUG)
+            pc.printf("Could not write device address (set_i2c_pointer)\r\n");
         return 0;
     }
     if (i2c.write(reg) == 0)
     {
+        if (DEBUG)
+            pc.printf("Could not write reg address (set_i2c_pointer)\r\n");
         return 0;
     }
     return 1;
@@ -19,9 +25,17 @@
 {
     i2c.start();
     if (set_i2c_pointer(addr, reg) == 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not set i2c pointer (read)\r\n");
         return 0;
+    }
     if (i2c.read(addr|1, buf, n, true) != 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not execute read sequence (read)\r\n");
         return 0;
+    }
     i2c.stop();
     return n;
 }
@@ -30,12 +44,18 @@
 {
     i2c.start();
     if (set_i2c_pointer(addr, reg) == 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not set i2c pointer (write)\r\n");
         return 0;
+    }
     for (int i=0; i<n; i++)
     {
         if (i2c.write(buf[i]) == 0)
         {   
             i2c.stop();
+            if (DEBUG)
+                pc.printf("Only sent %i/%i bytes (write)\r\n", i, n);
             return i;
         }
     }
@@ -44,39 +64,167 @@
     
 }
 
-int read_accelerometer(struct accel* s)
+int read_accelerometer(struct sensor* s)
 {
     int ret = read(accel_w, ACCEL_X, s->raw_data, 6);
     if (ret == 0)
     {
+        pc.printf("Error, could not read (read_accelerometer)\r\n");
+        return 0;
+    }
+    int16_t axlsb = (int16_t) s->raw_data[0];
+    int16_t axmsb = (int16_t) s->raw_data[1];
+    int16_t aylsb = (int16_t) s->raw_data[2];
+    int16_t aymsb = (int16_t) s->raw_data[3];
+    int16_t azlsb = (int16_t) s->raw_data[4];
+    int16_t azmsb = (int16_t) s->raw_data[5];
+
+    s->ax = ((axmsb << 8) + axlsb);
+    s->ay = ((aymsb << 8) + aylsb);
+    s->az = ((azmsb << 8) + azlsb);
+    return 1;
+}
+
+// disable accelerometer to save power
+int accelerometer_standby()
+{
+    char power_ctl;
+    int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
         return 0;
     }
-    int axlsb = (int) s->raw_data[0];
-    int axmsb = (int) s->raw_data[1];
-    int aylsb = (int) s->raw_data[2];
-    int aymsb = (int) s->raw_data[3];
-    int azlsb = (int) s->raw_data[4];
-    int azmsb = (int) s->raw_data[5];
+    power_ctl &= 0xF7 ;
+    ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    return 1;
+}
 
-    s->ax = (axmsb << 8) + axlsb;
-    s->ay = (aymsb << 8) + aylsb;
-    s->az = (azmsb << 8) + azlsb;
+// enable accelerometer for measurements
+int accelerometer_measure()
+{
+    char power_ctl;
+    int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }
+    power_ctl |= 0x8 ;
+    ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }    
     return 1;
 }
 
-void read_gyro(struct gyro* s){}
+int gyro_turnon()
+{
+    char power_ctl;
+    int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }
+    power_ctl |= 0x8 ;
+    ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }    
+    return 1;
+}
+
+int gyro_turnoff()
+{
+    char power_ctl;
+    int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    power_ctl &= 0xF7 ;
+    ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    return 1;
+}
+
+int read_gyro(struct sensor* s)
+{
+    int ret = read(gyro_w, GYRO_X, s->raw_data, 6);
+    if (ret == 0)
+    {
+        pc.printf("Error, could not read (read_gyro)\r\n");
+        return 0;
+    }
+    int16_t gxlsb = (int16_t) s->raw_data[0];
+    int16_t gxmsb = (int16_t) s->raw_data[1];
+    int16_t gylsb = (int16_t) s->raw_data[2];
+    int16_t gymsb = (int16_t) s->raw_data[3];
+    int16_t gzlsb = (int16_t) s->raw_data[4];
+    int16_t gzmsb = (int16_t) s->raw_data[5];
+
+    s->gx = ((gxmsb << 8) + gxlsb);
+    s->gy = ((gymsb << 8) + gylsb);
+    s->gz = ((gzmsb << 8) + gzlsb);
+    return 1;
+}
 int read_compass(void){return 0;}
 int read_barometer(void){return 0;}
 
-void config_accelerometer(void){}
-void config_gyro(void){}
+int config_accelerometer(void)
+{
+    // take accelerometer out of standby mode
+    int ret = accelerometer_measure();
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error starting up accelerometer\r\n");
+        return 1;
+    }
+    return ret;
+}
+int config_gyro(void)
+{
+    // turn on the gyro via i2c
+    int ret = gyro_turnon();
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error starting up gyro\r\n");
+        return 1;
+    }
+    return ret;
+}
 void config_compass(void){}
 void config_barometer(void){}
 
-void config_gy80(struct config *c)
+int config_gy80(struct config *c)
 {
     i2c.frequency(c->frequency);
-    config_accelerometer();
-    config_gyro();
-    config_compass();
+    return config_accelerometer();
+    /*config_gyro();
+    config_compass()*/;
 }
\ No newline at end of file

--- a/sensor.h	Wed Oct 30 22:51:33 2013 +0000
+++ b/sensor.h	Fri Nov 01 00:48:06 2013 +0000
@@ -1,35 +1,34 @@
 #ifndef SENSOR_H
 #define SENSOR_H
 
+#define DEBUG 1
+
 #include "mbed.h"
 
 
 
-#define accel_w 0x3A
-#define accel_r 0x3B
-#define ACCEL_X 0x32 // x: 0x32,0x33 y: 0x34,0x35 z: 0x36,0x37 Little Endian!!! (x = 0x33<<8|0x22), etc...
-#define ACCEL_Z
+#define accel_w 0xA6
+#define ACCEL_X 0x32 // x: 0x32,0x33 y: 0x34,0x35 z: 0x36,0x37 Little Endian!!! (x = 0x33<<8|0x22). 2's complement 16bit
+#define ACCEL_POWER_CTL 0x2D
 #define gyro_w 0xD0
-#define gyro_r 0xD1
+#define GYRO_X 0x28 // x: 0x28,0x29 y: 0x2A,0x2B z: 0x2C,0x2D Little Endian! (x = 0x28<<8|0x22). 2's complement 16bit
+#define GYRO_CTRL_REG1 0x20
 #define compass_w 0x3C
 #define compass_r 0x3D
 #define barometer_w 0xEE
 #define barometer_r 0xEF
 
-struct accel
+struct sensor
 {
-    int ax, ay, az;
+    int16_t ax, ay, az;
+    int16_t gx, gy, gz;
     char raw_data[6];
 };
 
-struct gyro
-{
-    int gy, gx, gz;
-};
-
 struct config
 {
     int frequency;
+    int accel_resolution;
 };
 
 
@@ -39,17 +38,22 @@
 
 int write(char addr, char reg, char *buf, int n);
 
-int read_accelerometer(struct accel* s);
+int read_accelerometer(struct sensor* s);
+int accelerometer_standby();
+int accelerometer_measure();
 
-void read_gyro(struct gyro* s);
+int read_gyro(struct sensor* s);
+int gyro_turnon();
+int gyro_turnoff();
+
 int read_compass(void);
 int read_barometer(void);
 
-void config_accelerometer(void);
-void config_gyro(void);
+int config_accelerometer(void);
+int config_gyro(void);
 void config_compass(void);
 void config_barometer(void);
 
-void config_gy80(struct config *c);
+int config_gy80(struct config *c);
 
 #endif //SENSOR_H
\ No newline at end of file

--- a/steps.cpp	Wed Oct 30 22:51:33 2013 +0000
+++ b/steps.cpp	Fri Nov 01 00:48:06 2013 +0000
@@ -5,12 +5,29 @@
 // in the future, change get_sensor_data to append the sensor data to a rolling list 
 void get_sensor_data()
 {
-    struct accel s;
+    struct sensor s;
     if (read_accelerometer(&s) == 0)
     {
-        pc.printf("Error!");
+        pc.printf("Error in get_sensor_data while reading from accel!\r\n");
         return;
     }
-    pc.printf("Ax: %i Ay: %i Az: %i\n", s.ax, s.ay, s.az);
+    
+    pc.printf("Ax: %i Ay: %i Az: %i;\r\n", s.ax, s.ay, s.az);
     return;
+}
+
+void init_sensors()
+{
+    // create config struct
+    struct config c;
+    
+    // set configurations
+    c.frequency = 10000;
+    
+    // pass configuration struct to configuration routine
+    int ret = config_gy80(&c);
+    if (ret == 0)
+    {
+        pc.printf("Error configuring sensors\r\n");
+    }
 }
\ No newline at end of file

--- a/steps.h	Wed Oct 30 22:51:33 2013 +0000
+++ b/steps.h	Fri Nov 01 00:48:06 2013 +0000
@@ -3,7 +3,7 @@
 #include "sensor.h"
 
 
-
+void init_sensors();
 void get_sensor_data();
 
 #endif //STEPS_H
\ No newline at end of file