Diff: main.cpp

Revision:

6:c034f08291ab

Parent:

5:e45681989201

Child:

8:4638e3f14c41

--- a/main.cpp	Tue May 15 14:02:11 2018 +0000
+++ b/main.cpp	Thu May 17 11:00:46 2018 +0000
@@ -47,6 +47,8 @@
 
 float sensorValue = 0.0000;
 
+int speedcontrol = 0.00001;
+
 int sevenSeg;
 
 void light7Seg(int nr, int output) {
@@ -247,33 +249,35 @@
 
 void motorControl() {
     while(true) {
-        IN1 = 1;
-        IN2 = 0;
-        IN3 = 1;
-        IN4 = 0;
-        
-        Thread::wait(100 - sensorValue * 100);
-        
-        IN1 = 1;
-        IN2 = 0;
-        IN3 = 0;
-        IN4 = 1;
-        
-        Thread::wait(100 - sensorValue * 100); 
-        
-        IN1 = 0;
-        IN2 = 1;
-        IN3 = 0;
-        IN4 = 1;
-        
-        Thread::wait(100 - sensorValue * 100);
-        
-        IN1 = 0;
-        IN2 = 1;
-        IN3 = 1;
-        IN4 = 0;
-        
-        Thread::wait(100 - sensorValue * 100);
+        if (sensorValue != 0) {
+            IN1 = 1;
+            IN2 = 0;
+            IN3 = 1;
+            IN4 = 0;
+            
+            Thread::wait(speedcontrol - speedcontrol/sensorValue);
+            
+            IN1 = 1;
+            IN2 = 0;
+            IN3 = 0;
+            IN4 = 1;
+            
+            Thread::wait(speedcontrol - speedcontrol/sensorValue); 
+            
+            IN1 = 0;
+            IN2 = 1;
+            IN3 = 0;
+            IN4 = 1;
+            
+            Thread::wait(speedcontrol - speedcontrol/sensorValue);
+            
+            IN1 = 0;
+            IN2 = 1;
+            IN3 = 1;
+            IN4 = 0;
+            
+            Thread::wait(speedcontrol - speedcontrol/sensorValue);
+        }
     }
 }
 
@@ -342,8 +346,6 @@
     dg3 = 0;
 
   while (true) {  
-    
-    //displayNumber7Seg(sensorValue);
 
     //printf("%0.4f \n\r", a0.read());
     // Look for new cards