Diff: main.cpp

Revision:

0:b455cd43929c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,122 @@
+/****************************************************************************/
+/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
+/*                                                                          */
+/*  - Digunakan encoder autonics                                            */
+/*  - Konfigurasi Motor dan Encoder sbb :                                   */
+/*        _________________                                                 */
+/*        |     DEPAN      |                                                */
+/*        | 1.    e     .2 |    Angka ==> Motor                             */
+/*        | `            ` |    e     ==> Encoder                           */
+/*        | e            e |                                                */
+/*        | .            . |                                                */
+/*        | 4`    e     `3 |                                                */
+/*        |________________|                                                */
+/*                                                                          */
+/*   SETTINGS (WAJIB!) :                                                    */
+/*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
+/*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
+/*                                                                          */
+/****************************************************************************/
+
+#include "mbed.h"
+#include "Motor.h"
+#include "omniPos.h"
+
+#define diameterRoda 10
+#define delta_t 0.001
+
+// Deklarasi variabel motor
+Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
+Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
+Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
+Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
+
+// Deklarasi variabel encoder
+//~ Dkiri untuk Motor 1
+//~ Dkanan untuk Motor 2        
+//~ Bkanan untuk Motor 3        
+//~ Bkiri untuk Motor 4
+omniPos omni1(Dkiri);
+omniPos omni2(Dkanan);
+omniPos omni3(Bkiri);
+omniPos omni4(Bkiri);
+
+// Inisialisasi  Pin TX-RX Joystik dan PC
+Serial pc(USBTX,USBRX);
+
+// Deklarasi Variabel Global
+/*
+ * posX dan posY berdasarkan arah robot
+ * encoder Depan & Belakang sejajar sumbu Y
+ * encoder Kanan & Kiri sejajar sumbu X 
+ */
+float jarak, posX, posY;
+float keliling = pi*diameterRoda;
+
+void detect_encoder()
+{
+    int pv;
+    // Motor1
+    
+    PID.setProcessValue(omni1.getVel(delta_t));
+    PID.setSetPoint();
+}
+
+int main()
+{
+    pc.baud(115200);
+    
+}
+
+void gerakKanan()
+{
+    if(vcurr<0.1) {
+        vcurr=0.1;
+    } else {
+        vcurr+=ax;
+    }
+    //perlambatan=0;
+            } else {
+                //perlambatan=1;
+            } 
+            
+            if (vcurr>=vmax) { 
+                vcurr=vmax; 
+            }
+            
+            if(joystick.R2==255 && joystick.L2==0) { 
+                koef=2;  
+            } else if (joystick.L2==255 && joystick.R2==0)  { 
+                koef=0.5;
+            } else { 
+                koef=1;  
+            }
+            
+            s1 =(float)(-1*koef*vcurr);
+            s2 =(float)(-1.0*koef*vcurr); 
+            s3 =(float)(1*koef*vcurr); 
+            s4 =(float)(1.0*koef*vcurr);           
+            
+            kanan=true; 
+            maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            
+            pc.printf("Kanan\n");
+            
+            motor1.speed(s1);
+            motor2.speed(s2);
+            motor3.speed(s3);
+            motor4.speed(s4);
+            break;
+        }    
+}
+
+void gerakKiri()
+{
+    float revDepan, revBelakang, revKanan, revKiri;
+    float tempX, tempY;
+    
+    revDepan    = encoderDepan.getRevolutions();
+    revBelakang = encoderBelakang.getRevolutions();
+    revKanan    = encoderKanan.getRevolutions();
+    revKiri     = encoderKiri.getRevolutions();    
+}
\ No newline at end of file