Diff: main.cpp

Revision:

5:e45681989201

Parent:

4:325954591d0f

Child:

6:c034f08291ab

Child:

7:1c712013123b

--- a/main.cpp	Tue May 15 11:22:14 2018 +0000
+++ b/main.cpp	Tue May 15 14:02:11 2018 +0000
@@ -20,7 +20,10 @@
 MFRC522 RfChip(SPI_MOSI, SPI_MISO, SPI_SCLK, SPI_CS, MF_RESET);
 Serial pc(USBTX, USBRX);
 
-DigitalOut led(LED1);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 DigitalOut dg0(p27);
 DigitalOut dg1(p28);
 DigitalOut dg2(p29);
@@ -37,7 +40,12 @@
 
 AnalogIn a0(p20);
 
-float sensorValue = 0;
+DigitalOut IN1(p11);
+DigitalOut IN2(p12);
+DigitalOut IN3(p13);
+DigitalOut IN4(p14);
+
+float sensorValue = 0.0000;
 
 int sevenSeg;
 
@@ -228,23 +236,67 @@
     }
 }
 
-void displayNumber7Seg(float nr) {
-    nr *= 1000;
-    int nrint = (int) nr;
-    int digits[4];
-    int i = 3;
-    while (nrint > 0) {
-        int digit = nrint % 10;
-        digits[i] = digit;
-        if(i == 0) {
-            break;    
+void control() {
+    while (true) {
+        led1 = IN1;
+        led2 = IN2;
+        led3 = IN3;
+        led4 = IN4;    
+    }    
+}
+
+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);
+    }
+}
+
+void displayNumber7Seg() {
+    while (true) {
+        float nr = sensorValue;
+        nr *= 1000;
+        int nrint = (int) nr;
+        int digits[4];
+        int i = 3;
+        while (nrint >= 0) {
+            int digit = nrint % 10;
+            digits[i] = digit;
+            if(i == 0) {
+                break;    
+            }
+            --i;
+            nrint /= 10;
+        }    
+        for(int i = 0; i < 4; i++) {
+            light7Seg(digits[3-i], i);
+            Thread::wait(100/24); //60Hz
         }
-        i--;
-        nrint /= 10;
-    }    
-    for(int i = 0; i < 4; i++) {
-        light7Seg(digits[3-i], i);
-        wait_ms(100/24); //60Hz
     }
 }
 
@@ -254,9 +306,9 @@
         float sum = 0;
         for(int i = 0; i < 20; i++) {
             samples[i] = a0.read();
-            wait_ms(10);
+            Thread::wait(5);
         }
-        for (int i = 0; i < 9; i++) {
+        for (int i = 0; i < 20; i++) {
             sum = sum + samples[i];
         }
         sensorValue = sum / 20;
@@ -266,8 +318,17 @@
 int main(void) {
   pc.printf("Start");
   
-  Thread thread_display;
-  thread_display.start(samplePotentio);
+  Thread thread_sample;
+  thread_sample.start(samplePotentio);
+  
+  Thread thread_control;
+  thread_control.start(control);
+   
+  Thread thread_motor(osPriorityRealtime);
+  thread_motor.start(motorControl);
+  
+  Thread thread_display(osPriorityRealtime);
+  thread_display.start(displayNumber7Seg);
   
   // Set debug UART speed
   //pc.baud(115200);
@@ -280,10 +341,9 @@
     dg2 = 0;
     dg3 = 0;
 
-  while (true) {
-    led = 0;    
+  while (true) {  
     
-    displayNumber7Seg(sensorValue);
+    //displayNumber7Seg(sensorValue);
 
     //printf("%0.4f \n\r", a0.read());
     // Look for new cards