Diff: main.cpp

Revision:

30:5efd59a0226f

Parent:

28:528c2fbd6a54

Child:

31:95396425b44b

--- a/main.cpp	Thu Jan 24 10:35:36 2019 +0000
+++ b/main.cpp	Thu Jan 24 11:27:26 2019 +0000
@@ -2,6 +2,7 @@
 #include "TPA81new.h"
 #include "SRF05.h"
 #include "Uvtron.h"
+#include "Stepper.h"
 
 #define LCD_D4_DIG PA_15
 #define LCD_D5_DIG PA_14
@@ -50,6 +51,11 @@
 AnalogIn irb3(PA_4);
 AnalogIn irb4(PB_0);
 
+#define WAIT_TIME 0.02
+
+// stepper(PinName _en, PinName ms1, PinName ms2, PinName ms3, PinName _stepPin, PinName dir);
+stepper s(PC_6, PC_8, PA_12, PC_7, PA_11, PB_12);
+
 float call_depan(uint16_t x){
     float side_distance = a_depan/pow(x,b_depan)+1;
     return side_distance;
@@ -92,43 +98,48 @@
         
         pc.printf("Line = %.2f\n", 3300*line.read());    
         
-        uint16_t raw_a1 = ira1.read_u16();
-        uint16_t raw_a2 = ira2.read_u16();
-        uint16_t raw_a3 = ira3.read_u16();
-        uint16_t raw_a4 = ira4.read_u16();
-        uint16_t raw_a5 = ira5.read_u16();
-        uint16_t raw_a6 = ira6.read_u16();
-        uint16_t raw_a7 = ira7.read_u16();
-        uint16_t raw_a8 = ira8.read_u16();
-        
-        uint16_t raw_b1 = irb1.read_u16();
-        uint16_t raw_b2 = irb2.read_u16();
-        uint16_t raw_b3 = irb3.read_u16();
-        uint16_t raw_b4 = irb4.read_u16();
-        
-        float dist_a1 = call_depan(raw_a1);
-        float dist_a2 = call_depan(raw_a2);
-        float dist_a3 = call_depan(raw_a3);
-        float dist_a4 = call_depan(raw_a4);
-        float dist_a5 = call_depan(raw_a5);
-        float dist_a6 = call_depan(raw_a6);
-        float dist_a7 = call_depan(raw_a7);
-        float dist_a8 = call_depan(raw_a8);
-        
-        float dist_b1 = call_depan(raw_b1);
-        float dist_b2 = call_depan(raw_b2);
-        float dist_b3 = call_depan(raw_b3);
-        float dist_b4 = call_depan(raw_b4);
-        
-        pc.printf("ir atas\n");
-        pc.printf("    %.2f %.2f\n", dist_a2, dist_a1);
-        pc.printf("%.2f          %.2f\n", dist_a3, dist_a7);
-        pc.printf("%.2f          %.2f\n", dist_a4, dist_a8);
-        pc.printf("    %.2f %.2f\n", dist_a6, dist_a5);
-        
-        pc.printf("ir bawah\n");
-        pc.printf("      %.2f\n", dist_b1);
-        pc.printf("%.2f            %.2f\n", dist_b2, dist_b3);
-        pc.printf("      %.2f\n", dist_b4);
+        for (int i=0; i<200; i++)
+        {
+            s.step(1, 1, 1/WAIT_TIME);
+        }
+        //uint16_t raw_a1 = ira1.read_u16();
+//        uint16_t raw_a2 = ira2.read_u16();
+//        uint16_t raw_a3 = ira3.read_u16();
+//        uint16_t raw_a4 = ira4.read_u16();
+//        uint16_t raw_a5 = ira5.read_u16();
+//        uint16_t raw_a6 = ira6.read_u16();
+//        uint16_t raw_a7 = ira7.read_u16();
+//        uint16_t raw_a8 = ira8.read_u16();
+//        
+//        uint16_t raw_b1 = irb1.read_u16();
+//        uint16_t raw_b2 = irb2.read_u16();
+//        uint16_t raw_b3 = irb3.read_u16();
+//        uint16_t raw_b4 = irb4.read_u16();
+//        
+//        float dist_a1 = call_depan(raw_a1);
+//        float dist_a2 = call_depan(raw_a2);
+//        float dist_a3 = call_depan(raw_a3);
+//        float dist_a4 = call_depan(raw_a4);
+//        float dist_a5 = call_depan(raw_a5);
+//        float dist_a6 = call_depan(raw_a6);
+//        float dist_a7 = call_depan(raw_a7);
+//        float dist_a8 = call_depan(raw_a8);
+//        
+//        float dist_b1 = call_depan(raw_b1);
+//        float dist_b2 = call_depan(raw_b2);
+//        float dist_b3 = call_depan(raw_b3);
+//        float dist_b4 = call_depan(raw_b4);
+//        
+//        pc.printf("ir atas\n");
+//        pc.printf("    %.2f %.2f\n", dist_a2, dist_a1);
+//        pc.printf("%.2f          %.2f\n", dist_a3, dist_a7);
+//        pc.printf("%.2f          %.2f\n", dist_a4, dist_a8);
+//        pc.printf("    %.2f %.2f\n", dist_a6, dist_a5);
+//        
+//        pc.printf("ir bawah\n");
+//        pc.printf("      %.2f\n", dist_b1);
+//        pc.printf("%.2f            %.2f\n", dist_b2, dist_b3);
+//        pc.printf("      %.2f\n", dist_b4);
+//        wait(1);
     }
 }