rev
Dependencies: mbed encoderKRAI Motor_new millis
main.cpp@0:586c99a83752, 2019-08-13 (annotated)
- Committer:
- ilhamwp0701
- Date:
- Tue Aug 13 12:53:18 2019 +0000
- Revision:
- 0:586c99a83752
rev
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ilhamwp0701 | 0:586c99a83752 | 1 | /* LIBRARY */ |
ilhamwp0701 | 0:586c99a83752 | 2 | #include "mbed.h" |
ilhamwp0701 | 0:586c99a83752 | 3 | #include "encoderKRAI.h" |
ilhamwp0701 | 0:586c99a83752 | 4 | #include "Motor.h" |
ilhamwp0701 | 0:586c99a83752 | 5 | #include "millis.h" |
ilhamwp0701 | 0:586c99a83752 | 6 | // #define debug_motor |
ilhamwp0701 | 0:586c99a83752 | 7 | // #define looping_program |
ilhamwp0701 | 0:586c99a83752 | 8 | /*PIN DECLARATION */ |
ilhamwp0701 | 0:586c99a83752 | 9 | DigitalOut pneua(PC_5);// input pin |
ilhamwp0701 | 0:586c99a83752 | 10 | DigitalOut pneub(PC_6); |
ilhamwp0701 | 0:586c99a83752 | 11 | DigitalOut pneuc(PC_8); |
ilhamwp0701 | 0:586c99a83752 | 12 | encoderKRAI enc (PB_2 , PB_1 , 538, encoderKRAI::X4_ENCODING); |
ilhamwp0701 | 0:586c99a83752 | 13 | Serial pc(USBTX, USBRX, 115200); |
ilhamwp0701 | 0:586c99a83752 | 14 | Motor main_motor(PB_15, PB_13, PB_14); // pwm(+)=clockwise |
ilhamwp0701 | 0:586c99a83752 | 15 | /* PROCEDURE DECLARATION */ |
ilhamwp0701 | 0:586c99a83752 | 16 | void hitungTheta(); |
ilhamwp0701 | 0:586c99a83752 | 17 | void motorMovement(bool _state); |
ilhamwp0701 | 0:586c99a83752 | 18 | /*VARIABLE */ |
ilhamwp0701 | 0:586c99a83752 | 19 | float theta = 0.0; |
ilhamwp0701 | 0:586c99a83752 | 20 | bool state; |
ilhamwp0701 | 0:586c99a83752 | 21 | unsigned long theta_timer_prev=0; |
ilhamwp0701 | 0:586c99a83752 | 22 | unsigned long motor_timer_prev=0; |
ilhamwp0701 | 0:586c99a83752 | 23 | |
ilhamwp0701 | 0:586c99a83752 | 24 | /* MAIN PROGRAM */ |
ilhamwp0701 | 0:586c99a83752 | 25 | int main() { |
ilhamwp0701 | 0:586c99a83752 | 26 | state=0; |
ilhamwp0701 | 0:586c99a83752 | 27 | startMillis(); |
ilhamwp0701 | 0:586c99a83752 | 28 | pneua=1; |
ilhamwp0701 | 0:586c99a83752 | 29 | pneub=1; |
ilhamwp0701 | 0:586c99a83752 | 30 | pneuc=1; |
ilhamwp0701 | 0:586c99a83752 | 31 | #ifdef looping_program |
ilhamwp0701 | 0:586c99a83752 | 32 | while(1) { |
ilhamwp0701 | 0:586c99a83752 | 33 | pc.printf("theta : %d \n", enc.getPulses()); |
ilhamwp0701 | 0:586c99a83752 | 34 | pc.printf("hitung theta : %f \n", theta); |
ilhamwp0701 | 0:586c99a83752 | 35 | pc.printf("state : %d \n", state); |
ilhamwp0701 | 0:586c99a83752 | 36 | if (millis()-theta_timer_prev>13){ |
ilhamwp0701 | 0:586c99a83752 | 37 | hitungTheta(); |
ilhamwp0701 | 0:586c99a83752 | 38 | theta_timer_prev=millis(); |
ilhamwp0701 | 0:586c99a83752 | 39 | } |
ilhamwp0701 | 0:586c99a83752 | 40 | #ifdef debug_motor |
ilhamwp0701 | 0:586c99a83752 | 41 | main_motor.speed(0.1); |
ilhamwp0701 | 0:586c99a83752 | 42 | #endif |
ilhamwp0701 | 0:586c99a83752 | 43 | |
ilhamwp0701 | 0:586c99a83752 | 44 | if (millis()-motor_timer_prev>5){ |
ilhamwp0701 | 0:586c99a83752 | 45 | motorMovement(state); |
ilhamwp0701 | 0:586c99a83752 | 46 | motor_timer_prev=millis(); |
ilhamwp0701 | 0:586c99a83752 | 47 | } |
ilhamwp0701 | 0:586c99a83752 | 48 | |
ilhamwp0701 | 0:586c99a83752 | 49 | if (theta > 45 && state == 0){ |
ilhamwp0701 | 0:586c99a83752 | 50 | state = 1; |
ilhamwp0701 | 0:586c99a83752 | 51 | } |
ilhamwp0701 | 0:586c99a83752 | 52 | else if (theta < -45 && state == 1){ |
ilhamwp0701 | 0:586c99a83752 | 53 | state = 0; |
ilhamwp0701 | 0:586c99a83752 | 54 | } |
ilhamwp0701 | 0:586c99a83752 | 55 | |
ilhamwp0701 | 0:586c99a83752 | 56 | |
ilhamwp0701 | 0:586c99a83752 | 57 | } |
ilhamwp0701 | 0:586c99a83752 | 58 | #endif |
ilhamwp0701 | 0:586c99a83752 | 59 | } |
ilhamwp0701 | 0:586c99a83752 | 60 | |
ilhamwp0701 | 0:586c99a83752 | 61 | void hitungTheta(){ |
ilhamwp0701 | 0:586c99a83752 | 62 | theta = theta + (float)enc.getPulses()*360/538; |
ilhamwp0701 | 0:586c99a83752 | 63 | enc.reset(); |
ilhamwp0701 | 0:586c99a83752 | 64 | } |
ilhamwp0701 | 0:586c99a83752 | 65 | void motorMovement(bool _state){ |
ilhamwp0701 | 0:586c99a83752 | 66 | if (_state==1) { |
ilhamwp0701 | 0:586c99a83752 | 67 | main_motor.speed(-0.1); |
ilhamwp0701 | 0:586c99a83752 | 68 | } |
ilhamwp0701 | 0:586c99a83752 | 69 | else{ |
ilhamwp0701 | 0:586c99a83752 | 70 | main_motor.speed(0.1); |
ilhamwp0701 | 0:586c99a83752 | 71 | } |
ilhamwp0701 | 0:586c99a83752 | 72 | } |
ilhamwp0701 | 0:586c99a83752 | 73 | |
ilhamwp0701 | 0:586c99a83752 | 74 | |
ilhamwp0701 | 0:586c99a83752 | 75 |