Diff: Classes/RobotMVC/RobotModel.cpp

Revision:

20:4173e6b7adde

Parent:

19:c44dc6716201

Child:

21:ee2b617cc0e6

--- a/Classes/RobotMVC/RobotModel.cpp	Wed May 10 19:14:25 2017 +0000
+++ b/Classes/RobotMVC/RobotModel.cpp	Thu May 11 15:40:01 2017 +0000
@@ -16,7 +16,7 @@
 
 //Sonar
 DigitalOut trig(A1);
-DigitalIn echo(A2);
+AnalogIn echo(A2);
 
 //Serial
 //Line Detector
@@ -244,7 +244,7 @@
         
             //Calculate Line Location
             float p = 0;
-            float Gp = 0.6f;
+            float Gp = 0.5f;
             int width = 0;
             for(int i=0; i<8; i++)
             {
@@ -259,7 +259,8 @@
                 StopMotors();
                 return -1;
             }else if(width == 0){
-                DriveStraight();
+                LeftMotor.speed(Gp*.5172);
+                RightMotor.speed(Gp*.5);
                 return 0;
             }else{
                 speedLeft = Gp-p;
@@ -270,17 +271,23 @@
             return (int)(p*100);
             //OA
         case 3:
-            sonar_t.reset();
-            //ping sensor
-            trig.write(1);
-            wait_us(10);
-            trig.write(0);
-            while(echo){}
-            sonar_t.start();
-            while(!echo){}
-            sonar_t.stop();
-            float time_us = sonar_t.read_us();
-            return (int)(time_us * 10);
+            //sonar_t.reset();
+//            //ping sensor
+//            trig.write(1);
+//            wait_us(10);
+//            trig.write(0);
+//            while(echo){}
+//            sonar_t.start();
+//            while(!echo){}
+//            sonar_t.stop();
+//            float time_us = sonar_t.read_us();
+            float volts = echo.read();
+            if(volts >= .7){
+                TurnRight();
+            }else{
+                DriveStraight();
+            }
+            return (int)(volts * 100);
             //LA
         case 4:
             //Scan sensors