File content as of revision 6:ed2f00b16b95:

//This will define all variables needed for the gradient equation 
// new angular frequency EQUATION: w(new) = a*w*cost(w*t) + c*sensor2*sin(wt) - d*(sensor2^2)*sin(wt) 
//high pass EQUATION: altsens = mult*prevaltsens + sensor2 - prevsensor 
//mult = 1 - t*w 
//u is modified by the w found, u = cos(wt)
//leftspeed = left + u
//rightspeed = right - u 

#include "mbed.h"
#include "m3pi_ng.h"
#include <cmath>
#include "iostream"
#include "btbee.h"
#include "Timer.h"
#include <stdlib.h>
 
using namespace std;
 
m3pi thinggy;
btbee btbee; 
Timer mytime;

DigitalIn button(p21);
 
float PI=3.141592;
int main() {
 
    int sensor[5]; 
 
    //w = angular frequency, which affects how much the robot oscilates  
    float angf = 2*PI; 
 
    //variables in the sine equation to calculate u
    double a = .01; 
    double c = .0007; 
    double d = .00000001;
 
    //the middle sensor value you take every t seconds 
    double sensor2;
    //you need the previous sensor for the high pass equation 
    double prevsensor = 0; 
 
    //this u alters left speed and right speed 
    double u; 
    double fwdspeed = 0.1; 
    float leftspeed = .01; 
    float rightspeed = .01; 
 
    //when you pass the sensor values through high pass
    //altered sensor 
    double altsens;
    //previous altered sensor 
    double prevaltsens = 0; 
 
    //read in sensor value 
    thinggy.raw_sensor(sensor); 
    sensor2 = sensor[2]; 
 
    //how to time 
    double t_actual;
    mytime.start(); 
    
    //how to increment through 
    double cur_step = 0;
    double max_steps = 100; //3 should run for a minute
    
    //how frequently the robot takes times 
    double delta_t = 0.01;
 
    //loop to change w as it picks up values sensor[2] != 2000
    while(cur_step < max_steps){
        //robot will go too fast if it is over .7, so this caps the speed at .7
        if(leftspeed >= .7 and rightspeed >= .7){
            thinggy.right_motor(.7);
            thinggy.left_motor(.7); 
        }
        else if(leftspeed >= .7){
            thinggy.left_motor(.7); 
            thinggy.right_motor(rightspeed); 
        }
        else if (rightspeed >= .7){
            thinggy.right_motor(.7); 
            thinggy.left_motor(leftspeed); 
        }
        else {
            thinggy.right_motor(leftspeed); 
            thinggy.left_motor(rightspeed); 
        }
        
        t_actual = mytime.read(); 
     
        thinggy.raw_sensor(sensor); 
        sensor2=sensor[2];  
    
        //high pass equation maybe needs to be made positive?
        altsens = -1*abs((1 - (.05*2*PI)*delta_t)*prevaltsens + sensor2 - prevsensor); 
        
        //sine movement 
        u = a*angf*cos(angf*t_actual) + c*altsens*sin(angf*t_actual) - d*(altsens*altsens)*sin(angf*t_actual); 
        //d is a variable to show the maximum
        
        thinggy.cls(); 
        thinggy.locate(0,0); 
        thinggy.printf(" s:%f", sensor2);
        thinggy.locate(0,1); 
        thinggy.printf("u:%f",u);
        leftspeed = fwdspeed + u; 
        rightspeed = fwdspeed - u;
        
        prevaltsens=altsens;
        prevsensor = sensor2;
        
        //make it run through delta t before it collects next sample
        while(t_actual < cur_step + delta_t){
            t_actual = mytime.read();    
        }
        cur_step = cur_step + delta_t; 
        
    }//while

    thinggy.stop(); 
    wait(500); 
}