main.cpp

00001 /* LIBRARY */
00002 #include "mbed.h"
00003 #include "encoderKRAI.h"
00004 #include "Motor.h"
00005 #include "millis.h"
00006 // #define debug_motor
00007 // #define looping_program
00008 /*PIN DECLARATION */
00009 DigitalOut pneua(PC_5);// input pin
00010 DigitalOut pneub(PC_6);
00011 DigitalOut pneuc(PC_8);
00012 encoderKRAI enc (PB_2 , PB_1 ,  538, encoderKRAI::X4_ENCODING);
00013 Serial pc(USBTX, USBRX, 115200);
00014 Motor main_motor(PB_15, PB_13, PB_14); // pwm(+)=clockwise  
00015 /* PROCEDURE DECLARATION */
00016 void hitungTheta();
00017 void motorMovement(bool _state);
00018 /*VARIABLE */
00019 float theta = 0.0;
00020 bool state;
00021 unsigned long theta_timer_prev=0;
00022 unsigned long motor_timer_prev=0;
00023 
00024 /* MAIN PROGRAM */
00025 int main() {  
00026     state=0;  
00027     startMillis();
00028     pneua=1;
00029     pneub=1;
00030     pneuc=1;
00031     #ifdef looping_program
00032     while(1) { 
00033         pc.printf("theta : %d \n", enc.getPulses());
00034         pc.printf("hitung theta : %f \n", theta);
00035         pc.printf("state : %d \n", state);
00036         if (millis()-theta_timer_prev>13){
00037             hitungTheta();
00038             theta_timer_prev=millis();
00039         }
00040         #ifdef debug_motor
00041             main_motor.speed(0.1);
00042         #endif
00043 
00044         if (millis()-motor_timer_prev>5){
00045             motorMovement(state);
00046             motor_timer_prev=millis();
00047         }
00048         
00049         if (theta > 45 && state == 0){
00050             state = 1;
00051         }
00052         else if (theta < -45 && state == 1){
00053             state = 0;
00054         }
00055         
00056 
00057     }
00058     #endif
00059 }
00060 
00061 void hitungTheta(){
00062     theta = theta + (float)enc.getPulses()*360/538;
00063     enc.reset();
00064 }
00065 void motorMovement(bool _state){
00066     if (_state==1) {
00067         main_motor.speed(-0.1);
00068     }
00069     else{
00070         main_motor.speed(0.1);
00071     }
00072 }
00073 
00074 
00075