Minor changes to calibration functions as well as wait_for_light

module/accel/public/kipr/accel/accel.h

@@ -52,8 +52,9 @@ signed short accel_z();
 
 
 /*!
- * Initiates a calibration of the accelerometer
- * \note Not Yet Implemented
+ * \brief Calibrates a lowpass filter for the accelerometer
+ * \description Sets a low-pass filter. Put at beginning of your program or before you use accelerometer commands if you want calibrated output.
+ * \description This function will block for around 500ms taking samples of the accelerometer at standstill.
  * \return 1: success 0: failure
  * \ingroup accel
  */

module/accel/src/accel_p.cpp

@@ -33,37 +33,25 @@ short kipr::accel::accel_z()
 // Simple low-pass filter for accelerometer
 bool kipr::accel::accel_calibrate()
 {
+  int samples = 50;
 
   // Find the average noise
   int i = 0;
-  double avg = 0;
-  while (i < 50)
+  double sumX = 0, sumY = 0, sumZ = 0;
+  while (i < samples)
   {
-    avg += accel_z();
-    msleep(10);
-    i++;
-  }
-  Biasz = avg / 50.0 + 512; // Z axis should be detecting gravity
+    sumX += accel_z();
+    sumY += accel_y();
+    sumZ += accel_x();
 
-  i = 0;
-  avg = 0;
-  while (i < 50)
-  {
-    avg += accel_y();
     msleep(10);
     i++;
   }
-  Biasy = avg / 50.0;
 
-  i = 0;
-  avg = 0;
-  while (i < 50)
-  {
-    avg += accel_x();
-    msleep(10);
-    i++;
-  }
-  Biasx = avg / 50.0;
+  Biasx = sumX / samples;
+  Biasy = sumY / samples;
+  Biasz = sumZ / samples + 512; // Z axis should be detecting gravity
 
+  printf("[Accel Calibration]: Bias Z: %f | Bias Y: %f | Bias X: %f \n", Biasz, Biasy, Biasx);
   return 0;
 }

module/botball/src/botball_c.cpp

@@ -139,7 +139,16 @@ void wait_for_light(int port)
                 printf("---------------------- \n");
                 printf("Threshold Value: %d \n \n", threshold);
                 printf("Current Value: %d  <----  \n", analog(port));
-                msleep(2000);
+
+                unsigned long startTime = systime()
+                while ((systime() - startTime) < 2000) {
+                    if (analog(port) > threshold) {
+                        break;
+                    }
+
+                    msleep(10);
+                }
+
                 console_clear();
             }
             return;

module/gyro/public/kipr/gyro/gyro.h

@@ -41,6 +41,7 @@ signed short gyro_z();
 /*!
  * \description Calibrates gyroscope
  * \description Sets a low-pass filter. Put at beginning of your program or before you use gyroscope commands if you want calibrated output.
+ * \description This function will block for around 500ms taking samples of the gyroscope at standstill.
  * \ingroup gyro
  */
 int gyro_calibrate();

module/gyro/src/gyro_p.cpp

@@ -39,41 +39,23 @@ bool kipr::gyro::gyro_calibrate()
 {
   int samples = 50;
 
-  // Find the average noise
-
-  // Get the bias for the Z axis by sampling the stationary output.
+  // Find the average noise of the gyro. Get the bias for gyro axis by sampling the stationary output.
   int i = 0;
-  double avg = 0;
+  double sumX = 0, sumY = 0, sumZ = 0;
   while (i < samples)
   {
-    avg += gyro_z();
+    sumX += gyro_x();
+    sumY += gyro_y();
+    sumZ += gyro_z();
     msleep(10);
     i++;
   }
-  biasz += avg / samples;
 
-  // Get the bias for the Y axis by sampling the stationary output.
-  i = 0;
-  avg = 0;
-  while (i < samples)
-  {
-    avg += gyro_y();
-    msleep(10);
-    i++;
-  }
-  biasy += avg / samples;
+  biasx = sumX / samples;
+  biasy = sumY / samples;
+  biasz = sumZ / samples;
 
-  // Get the bias for the X axis by sampling the stationary output.
-  i = 0;
-  avg = 0;
-  while (i < samples)
-  {
-    avg += gyro_x();
-    msleep(10);
-    i++;
-  }
-  biasx += avg / samples;
-  printf("Bias Z: %d | Bias Y: %d | Bias X: %d \n", biasz, biasy, biasx);
+  printf("[Gyro Calibrate]: Bias Z: %d | Bias Y: %d | Bias X: %d \n", biasz, biasy, biasx);
   return 0;
 }