Diff: steps.cpp

Revision:

4:44a5b1e8fd27

Parent:

3:f9e18a9cd9af

Child:

5:d85bac38cdff

--- a/steps.cpp	Fri Nov 01 01:23:04 2013 +0000
+++ b/steps.cpp	Sat Nov 02 08:47:14 2013 +0000
@@ -3,24 +3,44 @@
 Serial pc(USBTX, USBRX);
 
 // in the future, change get_sensor_data to append the sensor data to a rolling list 
-void get_sensor_data()
+int get_sensor_data(int pid)
 {
     struct sensor s;
+    
+    // retrieve process memory pointer
+    char* mem = get_output(pid);
+    int gx=0, gy=0, gz=0;
+    if (sscanf(mem, "gx0 %i gy0 %i gz0 %i;\r\n", &gx, &gy, &gz) != 3)
+    {
+        // probably first time running process, retrieve data from init_sensors and store in current process output buffer
+        
+    }
+    else
+    {
+        s.gx0 = gx;
+        s.gy0 = gy;
+        s.gz0 = gz;    
+    }
+    
+
     if (read_accelerometer(&s) == 0)
     {
-        pc.printf("Error in get_sensor_data while reading from accel!\r\n");
-        return;
+        if (USBDEBUG)
+            pc.printf("Error in get_sensor_data while reading from accel!\r\n");
+        return 1;
     }
     if (read_gyro(&s) == 0)
     {
-        pc.printf("Error in get_sensor_data while reading from gyro!\r\n");
-        return;
+        if (USBDEBUG)
+            pc.printf("Error in get_sensor_data while reading from gyro!\r\n");
+        return 1;
     }
-    pc.printf("Ax: %i Ay: %i Az: %i Gx: %i Gy: %i Gz: %i\r\n", s.ax, s.ay, s.az, s.gx, s.gy, s.gz);
-    return;
+    if (USBDEBUG)
+        pc.printf("Ax: %i Ay: %i Az: %i Gx: %i Gy: %i Gz: %i\r\n", s.ax, s.ay, s.az, s.gx, s.gy, s.gz);
+    return 1;
 }
 
-void init_sensors()
+int init_sensors(int pid)
 {
     // create config struct
     struct config c;
@@ -32,6 +52,34 @@
     int ret = config_gy80(&c);
     if (ret == 0)
     {
-        pc.printf("Error configuring sensors\r\n");
+        if (USBDEBUG)
+            pc.printf("Error configuring sensors\r\n");
     }
-}
\ No newline at end of file
+    
+    // accumulations of gyro values (for zeroing)
+    int gx = 0, gy = 0, gz = 0;
+    struct sensor s;
+    int numzerotrials = 10;
+    for (int i=0; i<numzerotrials; i++)
+    {
+        if (read_gyro(&s) == 0)
+        {
+            if (USBDEBUG)
+                pc.printf("Error in collecting zero-level data from gyro (init_sensors)\r\n");
+            return 1;
+        }
+        gx += s.gx;
+        gy += s.gy;
+        gz += s.gz;  
+    }
+    
+    gx /= numzerotrials;
+    gy /= numzerotrials;
+    gz /= numzerotrials;
+    
+    char * output = get_output(pid);
+    sprintf(output, "gx0 %i gy0 %i gz0 %i;\r\n", gx, gy, gz);
+    
+    schedule_proc("RETDATA", &get_sensor_data);
+    return 0;
+}