Diff: main.cpp

Revision:

1:9284c57e84da

Parent:

0:9dfae2e18414

Child:

2:34ed8b55eba6

--- a/main.cpp	Wed Apr 10 15:18:37 2019 +0000
+++ b/main.cpp	Sat Apr 13 18:51:43 2019 +0000
@@ -8,59 +8,37 @@
   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);
+Servo servo21(PA_7);
+Servo servo22(PB_6);
+Serial pc(D1, D0);
 
-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);
+void servo2(float f, float g){
+    servo21.write(f);
+    servo22.write(g);
+    pc.printf("%f", servo21.read());
+    pc.printf("\n");
+    pc.printf("%f", servo22.read());
+    pc.printf("\n");
 }
 
 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);
+    pc.baud(9600);
+    pc.printf("test begin\n");
     
-    //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)
+    //tes
+    servo21.calibrate(0.001,150);
+    servo22.calibrate(0.001,150);
+    servo21.write(0);
+    servo22.write(1.0);
+    while(1){
+        float m=0.0;
+        float n=0.0;
+        pc.scanf("%f",&m);
+        pc.printf("test \n");
+        pc.scanf("%f",&n);
+        pc.printf("test \n");
+        float f = m;
+        float g = n;
+        servo2(f,g);
+    }
 }