Diff: main.cpp

Revision:

1:d5adf9f6ca32

Parent:

0:ac3026899cce

--- a/main.cpp	Sat Oct 29 13:48:08 2016 +0000
+++ b/main.cpp	Sat Oct 29 16:06:40 2016 +0000
@@ -1,22 +1,67 @@
 // Continuously sweep the servo through it's full range
 #include "mbed.h"
 #include "Servo.h"
+#include "Motor.h"
 
-Servo myservo(PC_8);
+Servo servoFlap(PB_15);
+Servo servoDorong(PC_8);
+Motor myMotor (PA_8, PC_12, PB_7);
+Motor myMotor2 (PA_11, PC_15, PA_15);
+
+DigitalIn turunFlap(PC_4,PullUp);
+DigitalIn naikFlap(PB_13,PullUp);
+DigitalIn dorong(PB_14,PullUp);
 
-int main() {
+float sudutMin = 90;
+float sudutMax = 60;
+float sudut = 20;
+
+int main() 
+{
     wait_ms(100);
-    myservo.position(-90);
+    servoDorong.position(3);
+    wait_ms(2000);
+    servoFlap.position(20);
     wait_ms(2000);
-    while(1) {
-        for(int i=-90; i<=90; i++) {
-            myservo.position(i);
-            wait(0.01);
+    while(1)
+    {
+        int flagTurun = 1;
+        int flagNaik = 1;
+        int flagDorong = 1;
+        
+        myMotor.speed(1);
+        myMotor2.speed(-1);
+        
+        if (!naikFlap && flagNaik)
+        {
+            flagNaik = 0;
+            sudut++;
+            if (sudut > sudutMin)
+            {
+                sudut = sudutMin;    
+            }
+            servoFlap.position(sudut);
+            wait_ms(100);    
         }
-        for(int i=90; i>-90; i--) {
-            myservo.position(i);
-            wait(0.01);
+        if (!turunFlap && flagTurun)
+        {
+            flagNaik = 0;
+            sudut--;
+            if (sudut < sudutMax)
+            {
+                sudut = sudutMax;    
+            }
+            servoFlap.position(sudut);
+            wait_ms(100);    
         }
+        if (!dorong && flagDorong)
+        {
+            flagDorong = 0;
+            servoDorong.position(30);
+            wait_ms(2000);
+            servoDorong.position(3);
+            wait_ms(2000);    
+        }    
     }
    
 }