Revision 6:c76078abc03e, committed 2014-04-29

Comitter:

rokash000

Date:

Tue Apr 29 22:41:58 2014 +0000

Parent:

5:50887f863c01

Commit message:

WORKING CHASSIS!

Changed in this revision

--- a/main.cpp	Tue Apr 29 22:23:19 2014 +0000
+++ b/main.cpp	Tue Apr 29 22:41:58 2014 +0000
@@ -48,30 +48,103 @@
         //pc.printf("Front Sensor is: %f \n", frontsensor);
 //        pc.printf("Left Sensor is: %f \n", leftsensor);
 //        pc.printf("Right Sensor is: %f \n", rightsensor);
-              
-        if((frontsensor > 52)&&(frontsensor < 100)){
-              mA.speed(-.6);
-                mB.speed(-.8);   
-                wait(1);
-              led_front = 1;
-              }
-              else {led_front= 0;}
-
-              if((backsensor > 52)&&(backsensor < 100)){
-                  
-            led_back = 1;}
-            else {led_back = 0;}
-
-          if((leftsensor > 52)&&(leftsensor < 100)){
-            led_left = 1;}
-            else {led_left = 0;}
-
-          if((rightsensor > 52)&&(rightsensor < 100)){
-            led_right = 1;}
-          else {led_right = 0;}
-                mA.speed(-.2);
-                mB.speed(-.2); 
-
-
+//        
+        if((backsensor > 52)&&(backsensor < 100)){
+            sensor = 3;}
+        else if((frontsensor > 52)&&(frontsensor < 100)){
+            sensor = 2;}
+        else if((leftsensor > 52)&&(leftsensor < 100)){
+            sensor = 1;}
+         else if((rightsensor > 52)&&(rightsensor < 100)){
+            sensor = 0;}
+        
+        
+        switch(sensor) 
+        {
+            case 0: //Right
+                mA.speed(.4);
+                mB.speed(.6);
+                wait(0.5);
+                led_right = 1;
+                break;
+            case 1: // Left
+                mA.speed(.6);
+                mB.speed(.4);
+                wait(0.5);  
+                led_left = 1;
+                break;
+            case 2: // Front
+                mA.speed(-.6);
+                mB.speed(-.8);
+                 wait(0.9);
+                 mA.stop(0.5);
+                 mB.stop(0.5);
+                mA.speed(-.6);
+                mB.speed(-.8);
+                wait(0.9);
+                mA.speed(.2);
+                mB.speed(.2);
+                wait(0.9); 
+                led_front = 1;
+                break;    
+            case 3: // Back
+                mA.speed(.4);
+                mB.speed(.4);
+                wait(0.5);
+                 mA.stop(0.5);
+                 mB.stop(0.5);
+                mA.speed(.6);
+                mB.speed(.6);  
+                led_back = 1;
+                break;
+            default :
+                mA.speed(.6);
+                mB.speed(.6);
+                wait(0.5);
+                led_left = 0;
+                led_back = 0;
+                led_front = 0;
+                led_right = 0;
+                 break; 
         }
+                
+        sensor = 4; //DEFAULT CONDITION
+       // Back Sensor
+       // if((backsensor > 52)&&(backsensor < 100)){
+//            mA.speed(.8);
+//            mB.speed(.8);  
+//            led_back = 1;
+//                             
+//        }
+//        // front Sensor
+//        else if((frontsensor > 52)&&(frontsensor < 100)){
+//            mA.speed(-1);
+//            mB.speed(-1);  
+//            led_front = 1;
+//                             
+//        }else
+//        // right Sensor
+//        if((rightsensor > 52)&&(rightsensor < 100)){
+//            mA.speed(.4);
+//            mB.speed(.6);  
+//            led_right = 1;
+//                             
+//        }else // Left Sensor
+//        if((leftsensor > 52)&&(leftsensor < 100)){
+//            mA.speed(.6);
+//            mB.speed(.4);  
+//            led_left = 1;
+//                             
+//        }else{
+//             mA.speed(1);
+//           mB.speed(1);
+//            led_left = 0; 
+//            led_back = 0;
+//            led_front = 0;
+//            led_right = 0;
+//        }     
+        
+        
+        wait(0.1);
+    }
 }
\ No newline at end of file