rev
Dependencies: mbed encoderKRAI Motor_new millis
main.cpp
- Committer:
- ilhamwp0701
- Date:
- 2019-08-13
- 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); } }