KRAI ITB GARUDAGO / Mbed 2 deprecated TESTROBOT

Dependencies:   mbed

main.cpp

Committer:
oktavianusirvan
Date:
2019-10-09
Revision:
0:6a83d8fc84cf
Child:
1:f318636315dc

File content as of revision 0:6a83d8fc84cf:

#include "mbed.h"
#include "pinlist.h"
#include "robotpin.h" 
#include "encoder_krai/encoderKRAI.h"
#include "motor/Motor.h"
Serial pc(USBTX, USBRX, 115200);

float   pulseA = 0,
        pulseB = 0,
        pulseC = 0,
        pulseD = 0;
encoderKRAI enc_A      (PIN_A_CHA, PIN_A_CHB, 538, encoderKRAI::X4_ENCODING);
encoderKRAI enc_B      (PIN_B_CHA, PIN_B_CHB, 538, encoderKRAI::X4_ENCODING);
encoderKRAI enc_C      (PIN_C_CHA, PIN_C_CHB, 538, encoderKRAI::X4_ENCODING);
encoderKRAI enc_D      (PIN_D_CHA, PIN_D_CHB, 538, encoderKRAI::X4_ENCODING);

Motor motor_A           (PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
Motor motor_B           (PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Motor motor_C           (PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
Motor motor_D           (PIN_PWM_D, PIN_FWD_D, PIN_REV_D);
    
int main() {
    while(1){
        motor_A.speed(0.5);
        motor_B.speed(0.5);
        motor_C.speed(0.5);
        motor_D.speed(0.5);
        
        pulseA += (float)enc_A.getPulses()*360/538;
        pulseB += (float)enc_B.getPulses()*360/538;
        pulseC += (float)enc_C.getPulses()*360/538;
        pulseD += (float)enc_D.getPulses()*360/538;
        
        enc_A.reset();
        enc_B.reset();
        enc_C.reset();
        enc_D.reset();

        pc.printf("Pulse A : %f ", pulseA);
        pc.printf("Pulse B : %f ", pulseB);
        pc.printf("Pulse C : %f ", pulseC);
        pc.printf("Pulse D : %f \n", pulseD);

    }
    return 0;    
}