Diff: main.cpp

Revision:

0:586c99a83752

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 13 12:53:18 2019 +0000
@@ -0,0 +1,75 @@
+/* LIBRARY */
+#include "mbed.h"
+#include "encoderKRAI.h"
+#include "Motor.h"
+#include "millis.h"
+// #define debug_motor
+// #define looping_program
+/*PIN DECLARATION */
+DigitalOut pneua(PC_5);// input pin
+DigitalOut pneub(PC_6);
+DigitalOut pneuc(PC_8);
+encoderKRAI enc (PB_2 , PB_1 ,  538, encoderKRAI::X4_ENCODING);
+Serial pc(USBTX, USBRX, 115200);
+Motor main_motor(PB_15, PB_13, PB_14); // pwm(+)=clockwise  
+/* PROCEDURE DECLARATION */
+void hitungTheta();
+void motorMovement(bool _state);
+/*VARIABLE */
+float theta = 0.0;
+bool state;
+unsigned long theta_timer_prev=0;
+unsigned long motor_timer_prev=0;
+
+/* MAIN PROGRAM */
+int main() {  
+    state=0;  
+    startMillis();
+    pneua=1;
+    pneub=1;
+    pneuc=1;
+    #ifdef looping_program
+    while(1) { 
+        pc.printf("theta : %d \n", enc.getPulses());
+        pc.printf("hitung theta : %f \n", theta);
+        pc.printf("state : %d \n", state);
+        if (millis()-theta_timer_prev>13){
+            hitungTheta();
+            theta_timer_prev=millis();
+        }
+        #ifdef debug_motor
+            main_motor.speed(0.1);
+        #endif
+
+        if (millis()-motor_timer_prev>5){
+            motorMovement(state);
+            motor_timer_prev=millis();
+        }
+        
+        if (theta > 45 && state == 0){
+            state = 1;
+        }
+        else if (theta < -45 && state == 1){
+            state = 0;
+        }
+        
+
+    }
+    #endif
+}
+
+void hitungTheta(){
+    theta = theta + (float)enc.getPulses()*360/538;
+    enc.reset();
+}
+void motorMovement(bool _state){
+    if (_state==1) {
+        main_motor.speed(-0.1);
+    }
+    else{
+        main_motor.speed(0.1);
+    }
+}
+
+
+