Diff: main.cpp

Revision:

5:c91ef337eff4

Parent:

4:db3011440b4f

Child:

6:ed2f00b16b95

--- a/main.cpp	Tue Jun 10 08:07:42 2014 +0000
+++ b/main.cpp	Tue Jun 10 12:55:20 2014 +0000
@@ -11,6 +11,7 @@
 #include "cmath"
 #include "iostream"
 #include "btbee.h"
+#include <ctime>
  
 using namespace std;
  
@@ -22,21 +23,17 @@
     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; 
+    double angf = 0.01; 
  
     //variables in the equation to calculate angular frequency 
-    double a = .01; 
-    double c = .01; 
-    double d = .01; 
+    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; 
+    double prevsensor = 0; 
  
     //this u alters left speed and right speed 
     double u; 
@@ -55,46 +52,65 @@
     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;
+    //int found = 0;
  
     //loop to change w as it picks up values 
-    while(!found){
+    while(1){
+        //time(&start_t);
      
-        if(leftspeed >= 1 and rightspeed >= 1){
-            thinggy.right_motor(1);
-            thinggy.left_motor(1); 
+        //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 >= 1){
-            thinggy.left_motor(1); 
+        else if(leftspeed >= .7){
+            thinggy.left_motor(.7); 
             thinggy.right_motor(rightspeed); 
         }
-        else if (rightspeed >= 1){
-            thinggy.right_motor(1); 
+        else if (rightspeed >= .7){
+            thinggy.right_motor(.7); 
             thinggy.left_motor(leftspeed); 
         }
         else {
-            //move robot forward for a certain amount of time 
             thinggy.right_motor(leftspeed); 
             thinggy.left_motor(rightspeed); 
         }
-    
-        wait(50);
+        
+        wait(t_actual); 
     
         prevsensor = sensor2; 
-        thinggy.raw_sensor(sensor); 
+        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*angf)*prevaltsens + sensor2 - prevsensor; 
-    
+        altsens = (1 - t_set*angf)*prevaltsens + sensor2 - prevsensor; 
+        
         //sine movement 
-        angf = a*angf*cos(angf*t) + c*altsens*sin(angf*t) - d*(altsens*altsens)*sin(angf*t); 
+        u = a*angf*cos(angf*t_actual) + c*altsens*sin(angf*t_actual) - d*(altsens*altsens)*sin(angf*t_actual); 
     
-        u = cos(angf*t);
-    
-        leftspeed = leftspeed + u; 
-        rightspeed = rightspeed - u;
+        leftspeed = leftspeed - u; 
+        rightspeed = rightspeed + u;
     
     }//while(!found)