Diff: main.cpp

Revision:

0:ce0a2659915b

Child:

1:53d1b31be014

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 06 08:44:31 2014 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+#include "math.h"
+
+const double pi = 3.14159;
+
+m3pi huey;
+Timer timer;
+
+double average(int* arr)
+{
+    int sum = 0;
+    for(int i = 0; i < sizeof(arr); i++)
+    {
+        sum = sum + arr[i];
+    } 
+    return sum / sizeof(arr);
+}
+
+/*
+
+    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];
+    
+    float speed = 0.1;
+    int freq = 5;
+    float u;
+    
+    int s,f;
+    double time;
+    timer.start();
+    s = timer.read_ms();
+    
+    //int* right;
+    //int* left;
+    //int r = 0,l = 0;
+    //double avgDif;
+    
+    int diff;
+     
+    huey.raw_sensor(sensors);
+    int prev = sensors[2];
+    int curr;
+    while(z == 1)
+    {
+        f = timer.read_ms();
+        time = f - s;
+        u = speed*sin(time/(2*pi*freq));
+        huey.raw_sensor(sensors);
+        curr = sensors[2];
+        diff = curr - prev;
+        prev = curr;
+        //u = u + diff/2000;
+        huey.cls();
+        huey.printf("%i", 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.right_motor(speed - u);
+        huey.left_motor(speed + u);
+
+        if(sensors[2] == 2000)
+            break;   
+        //wait(0.01);
+    } 
+    huey.stop();
+}