Diff: main.cpp

Revision:

3:db2dc06c0686

Parent:

2:9cf609b74de9

diff -r 9cf609b74de9 -r db2dc06c0686 main.cpp
--- a/main.cpp	Mon May 29 17:01:57 2017 +0000
+++ b/main.cpp	Tue May 30 14:11:07 2017 +0000
@@ -17,7 +17,7 @@
 /*   SETTINGS (WAJIB!) :                                                    */
 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding                   */
 /*   2. Deklarasi penggunaan library pada bagian deklarasi encoder          */
-/*   3. Searah jarum jam positif 517 pulse 1 putaran enc Atas, 524 kiri     */
+/*   3. !arah jarum jam positif 517 pulse 1 putaran enc Atas, 524 kiri      */
 /*                                                                          */
 /****************************************************************************/
 
@@ -51,8 +51,8 @@
 Serial pc(USBTX,USBRX);
 
 /* Deklarasi Encoder Launcher */
-encoderKRAI encKiri( PB_5, PB_4, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encAtas( PA_15, PA_14, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encBawah( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encDepan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
 
 /* Deklarasi Motor Base */
 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 
@@ -62,15 +62,25 @@
 
 void setCenter(){
 /* Fungsi untuk menentukan center dari robot */
-    encKiri.reset();
-    encAtas.reset();
+    encBawah.reset();
+    encDepan.reset();
 }
 
 int main(void){
+    double p_samp=0,error=0,kp=0.082;
     pc.baud(115200);
     setCenter();
-    while(1){
-        pc.printf("kiri %d, atas %d \n",(int)encKiri.getPulses(),(int)encAtas.getPulses());
-        //wait_ms(500); 
+    
+    while(encDepan.getPulses()<5740){
+        pc.printf("error %d, p_samp %lf \n",(int)encDepan.getPulses()+(int)encBawah.getPulses(),p_samp);
+        error = encDepan.getPulses()+encBawah.getPulses();
+        p_samp = kp * error;
+        motorDpn.speed(0.7-p_samp);
+        motorBlk.speed(-0.7);
+        wait_ms(12.5); 
+        if(p_samp>1.5)p_samp = 1.5;
+        if(p_samp<-0.3)p_samp = -0.3;
     }
+    motorDpn.speed(0);
+    motorBlk.speed(0);
 }
\ No newline at end of file