Diff: utilities.cpp

Revision:

56:456d454d9ced

Parent:

55:8561b35130fc

Child:

57:0299098b2d0e

--- a/utilities.cpp	Fri Mar 27 22:44:29 2015 +0000
+++ b/utilities.cpp	Sun Mar 29 16:03:18 2015 +0000
@@ -17,6 +17,13 @@
     return diff_angle;
 }
 
+double angle_normal(double angle, double min, double max)
+{
+    while (angle < min) angle += (max-min);
+    while (angle > max) angle -= (max-min);
+    return angle;
+}
+
 double calculateDifferenceBetweenAngles(double firstAngle, double secondAngle)
 {
     double difference = secondAngle - firstAngle;
@@ -36,16 +43,9 @@
     return val / 57.2957795131;
 }
 
-/*double map(double variable, double min, double max, double newmin, double newmax)
-{
-    variable = variable / ( max - min );
-    variable *= ( newmax - newmin );
-    variable += newmin;
-    return variable;
-}*/
 double map(double x, double in_min, double in_max, double out_min, double out_max)
 {
-  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
 double constrains(double variable, double min, double max)
@@ -76,4 +76,48 @@
     double y = m * x + b;
     double theta = (ToDeg((1/sin((y-pos.GetY())/(x-pos.GetX()))))-90)*-1;
     return heading_err(theta,track_north);
+}
+
+double arr_avg(double *arr, int amount)
+{
+    return arr_sum(amount,arr)/amount;
+}
+
+double arr_degangleavg(double *a, int numb)
+{
+    double x = 0;
+    double y = 0;
+    for ( int i = 0; i < numb; i++ ) {
+        x += cos(ToRad(a[i]));
+        y += sin(ToRad(a[i]));
+    }
+    return ToDeg(atan2(y, x));
+}
+
+double arr_sum(int a, double *arr)
+{
+    double cummul = 0;
+    for ( int i = 0; i < a; i++ ) {
+        cummul += arr[i];
+    }
+    return cummul;
+}
+
+double arr_radangleavg(double *a, int numb)
+{
+    double x = 0;
+    double y = 0;
+    for ( int i = 0; i < numb; i++ ) {
+        x += cos(a[i]);
+        y += sin(a[i]);
+    }
+    return atan2(y, x);
+}
+
+void arr_pushback(double new_num, double *arr, int elements)
+{
+    for( int i = 0; i < elements; i++ ) {
+        arr[i+1] = arr[i];
+    }
+    arr[0] = new_num;
 }
\ No newline at end of file