Gala Ramadhan / Mbed 2 deprecated Baca_Program

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?

UserRevisionLine numberNew 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