Diff: main.cpp

Revision:

1:53d1b31be014

Parent:

0:ce0a2659915b

diff -r ce0a2659915b -r 53d1b31be014 main.cpp
--- a/main.cpp	Fri Jun 06 08:44:31 2014 +0000
+++ b/main.cpp	Thu Jun 12 12:10:06 2014 +0000
@@ -2,92 +2,76 @@
 #include "m3pi_ng.h"
 #include "math.h"
 
+// estimate for the value of PI
 const double pi = 3.14159;
 
+// initializes robot and timer object
 m3pi huey;
-Timer timer;
+Timer timer, samp;
 
-double average(int* arr)
+// Puts the sensor value through a high pass filter, cutting off low frequencies, and returning the manipulated gradient value
+double highPass(double yp, int uc, int up, double cutoff, double sampTime)
 {
-    int sum = 0;
-    for(int i = 0; i < sizeof(arr); i++)
-    {
-        sum = sum + arr[i];
-    } 
-    return sum / sizeof(arr);
+    return -abs((1-(sampTime*cutoff))*yp+uc-up);
 }
 
-/*
+// returns a speed based on the calculated gradient, wavelength of oscilation, and the run time
+float angleFormula(double angVel,double &yc, double yp, int uc, int up, float time, double a, double c, double d, double cutoff, double sampTime)
+{
+    yc = highPass(yp,uc,up,cutoff,sampTime);
+    return a*angVel*cos(angVel*time)+c*yc*sin(angVel*time)-d*yc*yc*sin(angVel*time); 
+}
 
-    End goal is to have heuy oscilate side to side and constatly determine which side is greater so that using the
-    right and left motors huey can favor that direction
-    Currently oscilates but does not do anything other than printing the sensor value, does not use the value for anything
-    
-    Failed attempt at taking averages
-    
-    Attempting to only use two consecutive sensor values as our judges;
-
-*/
 
 int main() 
 {
     int z = 1;
     int sensors[5];
     
+    //Sets the speed and angular velocity of the robot
     float speed = 0.1;
-    int freq = 5;
+    double freq = 0.5;//.125
+    double angVel = 2*pi*freq;
+    
     float u;
     
-    int s,f;
-    double time;
-    timer.start();
-    s = timer.read_ms();
+    double cutoff = 0.05; // creates the cutoff frequency for the high pass
+    double sampTime = 0.1; // sampling time for the high pass
+    
+    double ycur, yprev = 0; // the current and previous gradient values, initializes previous to 0 for first calculation
+    int ucur, uprev = 0; // the current and previous raw sensor values, initializes previous to 0 for first calculation
     
-    //int* right;
-    //int* left;
-    //int r = 0,l = 0;
-    //double avgDif;
+    huey.raw_sensor(sensors);// gets sensor values
     
-    int diff;
-     
-    huey.raw_sensor(sensors);
-    int prev = sensors[2];
-    int curr;
+    double a = .05/angVel,c = 0.00075,d = pow(.0001,2); // initializes 3 constants for the angle formula funciton
+    ucur = sensors[2];//sets current sensor value to the raw sensor value
+    ycur = highPass(yprev, ucur, uprev, cutoff, sampTime);//calculates initial gradient
+    uprev = ucur;// sets previous sensor value to the current sensor value for next calculation
+    timer.start();//starts run timer
+    samp.start();// starts sampling timer
     while(z == 1)
     {
-        f = timer.read_ms();
-        time = f - s;
-        u = speed*sin(time/(2*pi*freq));
+        samp.reset();//resets the sampling time for each iteration
         huey.raw_sensor(sensors);
-        curr = sensors[2];
-        diff = curr - prev;
-        prev = curr;
-        //u = u + diff/2000;
-        huey.cls();
-        huey.printf("%i", sensors[2]);
+        ucur = sensors[2];
         
-        /*
-        Attempting to determine which side of the sensors are greater so that the robot can move in that direction
-        if(u > 0)
-        {
-            right[r] = sensors[2];
-            r++;
-        }
-        else if(u < 0)
-        {
-            left[l] = sensors[2];
-            l++;   
-        }
-        avgDif = average(right) - average(left);
-        u = u + avgDif/2000;
-        */
+        huey.cls();
+        u = angleFormula(angVel,ycur,yprev,ucur,uprev,timer.read(),a,c,d,cutoff,sampTime);//recieves speed from formula
+        huey.printf("%f", u);
         
+        //adds u to the speed of the left and right motors to control the direction the robot moves
         huey.right_motor(speed - u);
         huey.left_motor(speed + u);
 
-        if(sensors[2] == 2000)
-            break;   
-        //wait(0.01);
+        //if(sensors[2] == 2000)
+            //break;
+        // sets previous  values to the current values for next calculation
+        yprev = ycur;
+        uprev = ucur;
+        //waits until the system has fully reached its sampling time
+        while((samp.read_ms()) < (sampTime * 1000) )
+        {     
+        }
     } 
     huey.stop();
 }