File content as of revision 1:53d1b31be014:

#include "mbed.h"
#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, samp;

// 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)
{
    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); 
}


int main() 
{
    int z = 1;
    int sensors[5];
    
    //Sets the speed and angular velocity of the robot
    float speed = 0.1;
    double freq = 0.5;//.125
    double angVel = 2*pi*freq;
    
    float u;
    
    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
    
    huey.raw_sensor(sensors);// gets sensor values
    
    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)
    {
        samp.reset();//resets the sampling time for each iteration
        huey.raw_sensor(sensors);
        ucur = sensors[2];
        
        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;
        // 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();
}