File content as of revision 4:db3011440b4f:

//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"
 
using namespace std;
 
m3pi thinggy;
btbee btbee; 
 
int main() {
 
    int sensor[5]; 
 
    //w = angular frequency, which is modified after you take each time 
    double angf = 0; 
 
    //t = how frequently you change w
    //should be between 0.001 and 0.01
    double t = 0.001; 
 
    //variables in the equation to calculate angular frequency 
    double a = .01; 
    double c = .01; 
    double d = .01; 
 
    //the middle sensor value you take every t seconds 
    double sensor2;
    //you need the previous sensor for the high pass equation 
    double prevsensor; 
 
    //this u alters left speed and right speed 
    double u; 
    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; 
 
    thinggy.sensor_auto_calibrate(); 
 
    //read in sensor value 
    thinggy.calibrated_sensor(sensor); 
    sensor2 = sensor[2]; 
 
    //variable if the best point is found 
    int found = 0;
 
    //loop to change w as it picks up values 
    while(!found){
     
        if(leftspeed >= 1 and rightspeed >= 1){
            thinggy.right_motor(1);
            thinggy.left_motor(1); 
        }
        else if(leftspeed >= 1){
            thinggy.left_motor(1); 
            thinggy.right_motor(rightspeed); 
        }
        else if (rightspeed >= 1){
            thinggy.right_motor(1); 
            thinggy.left_motor(leftspeed); 
        }
        else {
            //move robot forward for a certain amount of time 
            thinggy.right_motor(leftspeed); 
            thinggy.left_motor(rightspeed); 
        }
    
        wait(50);
    
        prevsensor = sensor2; 
        thinggy.raw_sensor(sensor); 
        sensor2 = sensor[2]; 
    
        //high pass equation 
        altsens = (1 - t*angf)*prevaltsens + sensor2 - prevsensor; 
    
        //sine movement 
        angf = a*angf*cos(angf*t) + c*altsens*sin(angf*t) - d*(altsens*altsens)*sin(angf*t); 
    
        u = cos(angf*t);
    
        leftspeed = leftspeed + u; 
        rightspeed = rightspeed - u;
    
    }//while(!found)
 
 
    
}