Diff: main.cpp

Revision:

13:0c5942931cad

Parent:

12:1e3227a6fcd7

Child:

14:207770fefedf

--- a/main.cpp	Sun Dec 16 01:53:07 2018 +0000
+++ b/main.cpp	Fri Jan 04 11:58:19 2019 +0000
@@ -3,28 +3,29 @@
 #include "AX12.h"
 #include "Uvtron.h"
 #include "TPA81new.h"
+#include "SRF05.h"
 
 #define WAIT_TIME 0.02
 
 Serial pc(USBTX, USBRX);
+AnalogIn IR(PA_0);
 // 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);
 Uvtron uv(PC_12);
-
-AX12 servo1(PC_10, PC_11, PA_13, 6,  1000000); // tx, rx, tx_enable, servo ID, baud rate  
-AX12 servo2(PC_10, PC_11, PA_13, 22, 1000000);
-AX12 servo3(PC_10, PC_11, PA_13, 19, 1000000);
-
-TPA81 tpay(PB_9, PB_8, 0xDE);
+AX12 servo1(PC_10, PC_11, PA_13, 1, 1000000); // tx, rx, tx_enable, servo ID, baud rate  
+TPA81 tpay(PB_9,PB_8, 0xDE);
+SRF05 srf(PA_15, PA_14); // trigger, echo
 DigitalOut led(LED1);
 
 int main()
 {
-    s.enable();
+//    s.enable();
     while (1)
     {
-        
-        // TPA
+//      Ultrasonik
+        pc.printf("Distance = %.1f\n", srf.read());
+
+//      TPA
         pc.printf("%d", tpay.getTemp(0));
         int i;
         pc.printf("\nTPA Y \n");
@@ -33,22 +34,61 @@
             pc.printf("%d ",tpay.getTemp(i));
         }
         
-        // Stepper
-        pc.printf("Stepper\n");
+//         Stepper
+        pc.printf("\n stepper \n");
         s.step(1, 1, 1/WAIT_TIME);
         
         // Servo
-        servo1.SetGoal(240);
-        servo2.SetGoal(240);
-        servo3.SetGoal(240);
-        pc.printf("Servo : 240\n");
-        led = 1;
+        float degrees2 = 150;
+        float speed1 = 1.0;
+        servo1.MultSetGoal(
+            degrees2, speed1, 
+            degrees2, speed1,  
+            degrees2, speed1,   
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1
+        );
+        
+        pc.printf("Servo 150\n");
         wait(1);
-        servo1.SetGoal(150);
-        servo2.SetGoal(150);
-        servo3.SetGoal(150);
-        pc.printf("Servo : 150\n");
-        led = 0;
+        
+        degrees2 = 240;
+        
+        servo1.MultSetGoal(
+            degrees2, speed1, 
+            degrees2, speed1,  
+            degrees2, speed1,   
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1, 
+            degrees2, speed1
+        );
+        
+        pc.printf("Servo 240\n");
         wait(1);
         
         // Uvtron
@@ -56,5 +96,11 @@
         int read = uv.Flag;
         if (read) pc.printf("FIRE DETECTED\n");
         else pc.printf("NOT DETECTED\n");
+        
+        // IR
+        float meas = IR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+        meas = meas * 3300; // Change the value to be in the 0 to 3300 range
+        float ir_dist = (330377) * (pow(meas , (-1.349f)));
+        printf("IR : %.4f\n", ir_dist);
     }    
 }
\ No newline at end of file