Diff: main.cpp

Revision:

2:acc52907bcce

Parent:

1:6402638c6f6d

Child:

3:ac07a11318f9

--- a/main.cpp	Thu May 22 14:43:25 2014 +0000
+++ b/main.cpp	Fri Jun 06 09:46:32 2014 +0000
@@ -1,3 +1,9 @@
+//This will define all variables needed for the gradient equation 
+// EQUATION: w(new) = a*w*cost(w*t) + c*sensor2*sin(wt) - d*(sensor2^2)*sin(wt) 
+//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"
@@ -8,153 +14,50 @@
  
 m3pi thinggy;
 btbee btbee; 
-
-DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
-DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
-DigitalIn m3pi_pb(p21);
  
 int main() {
-    
-    //..................................................................................................\\
-    //......BlueTooth Communication.....................................................................\\
-    //..................................................................................................\\
-    
-    // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
-   thinggy.locate(0,1);
-   btbee.reset();
-   for (int i = 0; i <4; i++) {
-       mbed_led[i] = 0;
-   }
-   for (int i = 0; i <8; i++) {
-       m3pi_led[i]=0;
-   }
-   m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
-
-   // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
-
-   thinggy.locate(0,0);
-   wait(0.3);
-   
-   thinggy.locate(0,0);
-   thinggy.printf("%s","btTest");
-   thinggy.locate(0,1);
-   thinggy.printf("%s","PBonLNK");
-
-   // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
-   while(m3pi_pb) {
-       m3pi_led[0]=!m3pi_led[0];
-       wait(0.1);
-   }
-
-   int iline=1;
-   char arr_read[30]; // this should be long enough to store any reply coming in over bt.
-   int  chars_read;   // number of chars read in a bt reply
-   
-   while (true) {
-       // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
-       if ( btbee.writeable() ) {
-           if (iline==6) {
-               btbee.printf("Your Turn!\n");
-               iline++;
-           }//if
-           else {
-               if (iline <6){
-               btbee.printf("Line %0.3d \n",iline);
-               m3pi_led[0]=0;
-               thinggy.locate(0,0);
-               thinggy.printf("Sent %0.3d",iline);
-               iline++;
-               }
-           }//else
-       }//if_write
-
-       // check for answers after each write /not write
-       while ( btbee.readable() ) {
-           m3pi_led[7]=1;
-           btbee.read_all(arr_read, 30, &chars_read );
-           m3pi_led[6]=1;
-           thinggy.locate(0,1);
-           thinggy.print(arr_read,chars_read);
-           m3pi_led[5]=1;
-       }//while_readable
-       wait(0.1);
-   }//while_true
-    
-    //..................................................................................................\\
-    //......End BlueTooth Communication.................................................................\\
-    //..................................................................................................\\
-    
-/*    
-    
-    
-    
-    float speed = 0.25;
-    float correction;
-    float k = -0.3;
-    int sensor[5];
-    int returned;   
-    thinggy.locate(0,1);
-    thinggy.printf("Villan");
-    thinggy.locate(0,0);
-    thinggy.printf("Pimpin");
-    
-    
-    
-    wait(1.0);
+ 
+ int sensor[5]; 
+ 
+ //w = angular frequency, which is modified after you take each time 
+ int w = 0.5; 
+ 
+ //t = how frequently you change w
+ //should be between 0.001 and 0.01
+ int t = 0.001; 
+ 
+ //variables in the equation to calculate angular frequency 
+ int a = 1; 
+ int c = 1; 
+ int d = 1; 
+ 
+ //the middle sensor value you take every t seconds 
+ int sensor2;
+ 
+ //NOT DONE need variables for an equation that filters out noise
  
-    thinggy.sensor_auto_calibrate();
+ //this u alters left speed and right speed 
+ int u; 
+ int leftspeed = .01; 
+ int right speed = .01; 
+ 
+ //read in sensor value 
+ thinggy.raw_sensor(sensor); 
+ sensor2 = sensor[2]; 
  
-    thinggy.calibrated_sensor(sensor);
-    returned = (sensor[1] + sensor[2] + sensor[3])/3;
+ //variable if the best point is found 
+ int found = 0;
+ 
+ //loop to change w as it picks up values 
+ while(!found){
+     
+     
+     
+}//while(!found)
+ 
+ 
  
     
-    
-    while(1) {
-        // change "if" to while
-        while(returned < 220){
-                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
-                    thinggy.left_motor(.2);
-                    thinggy.right_motor(-.2);
-                    thinggy.calibrated_sensor(sensor);
-                }
-                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
-                    thinggy.left_motor(-.2);
-                    thinggy.right_motor(.2);
-                    thinggy.calibrated_sensor(sensor);
-                }
-                thinggy.calibrated_sensor(sensor);
-                returned = (sensor[1] + sensor[2] + sensor[3])/3;
-        }
-        
-        // Curves and straightaways    
-        while(returned > 220){
- 
-        float position = thinggy.line_position();
-        correction = k*(position);
-       
-         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
-        
-        //speed limiting for right motor
-        if(speed + correction > 1){
-            thinggy.right_motor(0.6);
-            thinggy.left_motor(speed-correction);
-        } 
-        else if(speed - correction > 1){
-            thinggy.right_motor(speed+correction);
-            thinggy.left_motor(0.6);
-        }
-        else{
-            thinggy.left_motor(speed-correction);
-            thinggy.right_motor(speed+correction);
-        }
-         thinggy.calibrated_sensor(sensor);
-         returned = (sensor[1] + sensor[2] + sensor[3])/3;   
-        }
-    thinggy.calibrated_sensor(sensor);
-    returned = (sensor[1] + sensor[2] + sensor[3])/3;
-    }
-*/
 }
  
- 
  
\ No newline at end of file