steps.cpp

00001 #include "steps.h"
00002 
00003 Serial pc(USBTX, USBRX);
00004 
00005 // in the future, change get_sensor_data to append the sensor data to a rolling list 
00006 int get_sensor_data(int pid)
00007 {
00008     struct sensor s;
00009     
00010     // retrieve process memory pointer
00011     char* mem = get_output(pid);
00012     int gx=0, gy=0, gz=0;
00013     if (sscanf(mem, "gx0 %i gy0 %i gz0 %i;\r\n", &gx, &gy, &gz) != 3)
00014     {
00015         // probably first time running process, retrieve data from init_sensors and store in current process output buffer
00016         
00017     }
00018     else
00019     {
00020         s.gx0 = gx;
00021         s.gy0 = gy;
00022         s.gz0 = gz;    
00023     }
00024     
00025 
00026     if (read_accelerometer(&s) == 0)
00027     {
00028         if (USBDEBUG)
00029             pc.printf("Error in get_sensor_data while reading from accel!\r\n");
00030         return 1;
00031     }
00032     if (read_gyro(&s) == 0)
00033     {
00034         if (USBDEBUG)
00035             pc.printf("Error in get_sensor_data while reading from gyro!\r\n");
00036         return 1;
00037     }
00038     if (USBDEBUG)
00039         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);
00040     return 1;
00041 }
00042 
00043 int init_sensors(int pid)
00044 {
00045     // create config struct
00046     struct config c;
00047     
00048     // set configurations
00049     c.frequency = 10000;
00050     
00051     // pass configuration struct to configuration routine
00052     int ret = config_gy80(&c);
00053     if (ret == 0)
00054     {
00055         if (USBDEBUG)
00056             pc.printf("Error configuring sensors\r\n");
00057     }
00058     
00059     // accumulations of gyro values (for zeroing)
00060     int gx = 0, gy = 0, gz = 0;
00061     struct sensor s;
00062     int numzerotrials = 10;
00063     for (int i=0; i<numzerotrials; i++)
00064     {
00065         if (read_gyro(&s) == 0)
00066         {
00067             if (USBDEBUG)
00068                 pc.printf("Error in collecting zero-level data from gyro (init_sensors)\r\n");
00069             return 1;
00070         }
00071         gx += s.gx;
00072         gy += s.gy;
00073         gz += s.gz;  
00074     }
00075     
00076     gx /= numzerotrials;
00077     gy /= numzerotrials;
00078     gz /= numzerotrials;
00079     
00080     char * output = get_output(pid);
00081     sprintf(output, "gx0 %i gy0 %i gz0 %i;\r\n", gx, gy, gz);
00082     
00083     schedule_proc("RETDATA", &get_sensor_data);
00084     return 0;
00085 }