Diff: main.cpp

Revision:

99:6efb1c0a6a68

Parent:

98:d8ead5e04047

--- a/main.cpp	Thu Jun 13 11:12:37 2019 +0000
+++ b/main.cpp	Mon Jun 17 02:28:58 2019 +0000
@@ -246,12 +246,13 @@
                 break;
             
             case 0x19:
-            // 100+ button (Calibrate)
+            // 100+ button (Calibrate)                          
+                motorDriver.user_left(0.2,0.2);
                 pc.printf("calibrate!\r\n");
                 trs.calibrate();
                 pc.printf("calibrate done\r\n");
                 wait(0.2);
-                button = 0x1C;
+                //button = 0x1C;
                 break;
                 
             case 0x18:
@@ -272,14 +273,14 @@
             
             case 0x5A:
             // 6 button (clockwise turn)
-                motorDriver.user_right(0.3,0.1);
+                motorDriver.right();
                 wait(0.1);
                 button = 0x1C;
                 break;
             
             case 0x08:
             // 4 button (counter clockwise turn)
-                motorDriver.user_left(0.1,0.3);
+                motorDriver.left();
                 wait(0.1);
                 button = 0x1C;
                 break;
@@ -295,14 +296,16 @@
                 timer.reset();
                 timer.start();
                 ultra.setMode(false);           // have updates every .1 seconds and try only once.
-
+                int_err = 0;
+                err = 0;
+                prev_err = 0;
                 while(1)
                 {  
                     t=timer.read();
                     position=trs.readLine(sensorValues,0);
                     ultra.trig();
                     dist = ultra.getDistance();
-                    pc.printf("dist:%d\r\n", dist);
+                   // pc.printf("dist:%d\r\n", dist);
                     if(dist<=21){
                           motorDriver.user_left(0.2,0.2);
                           wait(0.1);
@@ -367,17 +370,17 @@
                         power_difference = -maximum;      
                         
                     if(power_difference<0)
-                        motorDriver.user_forward((maximum-10)/255,(maximum+power_difference)/255);               
+                        motorDriver.user_forward((maximum)/255,(maximum+power_difference-10)/255);               
                     else 
-                        motorDriver.user_forward((maximum-power_difference)/255,(maximum+10)/255); 
+                        motorDriver.user_forward((maximum-power_difference)/255,(maximum-10)/255); 
                     
-                    pc.printf("position value: %d\r\n", position);
+                   // pc.printf("position value: %d\r\n", position);
                     
                     //pc.printf("cnt: %d\r\n", cnt);
-                    for(int i=0;i<5;i++){
-                        pc.printf("%d\r\n",sensorValues[i]);
-                    }
-                    if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650))
+                    //for(int i=0;i<5;i++){
+//                        pc.printf("%d\r\n",sensorValues[i]);
+//                    }
+                    if((sensorValues[0] > 650 ) && (sensorValues[1]>650 ) && (sensorValues[2]>650 ) && (sensorValues[3]>650 && sensorValues[3] < 850) && (sensorValues[4]>650&& sensorValues[4] < 850)  )
                     {   // 5 IR sensor are on black
                         timer.stop();
                         t=timer.read();