rev

Dependencies:   mbed encoderKRAI Motor_new millis

main.cpp

Committer:
ilhamwp0701
Date:
4 weeks ago
Revision:
0:586c99a83752

File content as of revision 0:586c99a83752:

/* 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);
    }
}