Diff: main.cpp

Revision:

0:9dfae2e18414

Child:

1:9284c57e84da

diff -r 000000000000 -r 9dfae2e18414 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 10 15:18:37 2019 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Motor.h"
+
+/* Nama servo
+    4
+    3
+  21 22
+    1 */
+    
+Servo servo1(PA_1);
+Servo servo21(PA_2);
+Servo servo22(PA_3);
+Servo servo3(PA_4);
+Servo servo4(PA_5);
+Motor motorKanan(PB_3, PA_10, PC_4);
+Motor motorKiri(PB_7, PC_3, PC_0);
+
+void kalibrasi(float deg){
+    servo1.calibrate(0.0005,deg);
+    servo21.calibrate(0.0005,deg);
+    servo22.calibrate(0.0005,deg);
+    servo3.calibrate(0.0005,deg);
+    servo4.calibrate(0.0005,deg);
+}
+
+void idle(float s1,float s21,float s22, float s3, float s4){
+    servo1.position(s1);
+    wait_ms(1);
+    servo21.position(s21);
+    servo22.position(s22);
+    wait_ms(1);
+    servo3.position(s3);
+    wait_ms(1);
+    servo4.position(s4);
+    wait_ms(1);
+}
+
+int main() {
+    /***Gerak Jalan***
+    //Maju
+    motorKanan.speed(0.3);
+    motorKiri.speed(0.3);
+    
+    //Mundur
+    motorKanan.speed(0.3);
+    motorKiri.speed(0.3);
+    
+    //PivotKiri
+    motorKanan.brake();
+    motorKiri.speed(0.3);
+    
+    //PivotKanan
+    motorKiri.brake();
+    motorKanan.speed(0.3);
+    */
+    
+    /***Gerak Tangan***/
+    //kalibrasi
+    kalibrasi(300);
+    
+    //mode1(idle)
+    idle(0.0,90.0,90.0,90.0,0.0);
+    
+    //mode2(gerakan keluar)
+}