File content as of revision 0:ce0a2659915b:

#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();
}