File content as of revision 5:c91ef337eff4:

//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 <ctime>
 
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.01; 
 
    //variables in the equation to calculate angular frequency 
    double a = .001; 
    double c = .001; 
    double d = .001; 
 
    //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; 
    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]; 
 
    //how to time 
    //time_t start_t; 
    //time_t end_t; 
    double t_actual = .01;
    //double t_change = .01; 
    double t_set = 0.01; 
    
    //variable if the best point is found 
    //int found = 0;
 
    //loop to change w as it picks up values 
    while(1){
        //time(&start_t);
     
        //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); 
        }
        
        wait(t_actual); 
    
        prevsensor = sensor2; 
        thinggy.calibrated_sensor(sensor); 
        sensor2 = sensor[2]; 
        
        //time(&end_t); 
        //t_change = difftime(start_t, end_t);
        //t_actual = t_change; 
        
        //makes it wait amount of time leftover
        //if(t_change < t_set){
            //wait until time == t
          //  while(t_change < t_set){
            //    time(&end_t);
              //  t_change = difftime(start_t, end_t); 
            //}   
        //} 
    
        //high pass equation 
        altsens = (1 - t_set*angf)*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); 
    
        leftspeed = leftspeed - u; 
        rightspeed = rightspeed + u;
    
    }//while(!found)
 
 
    
}