Patrik Kolar / Motors_Out

control of 2 propulsive DC motors and servo, in RF controlled state and autonomous state

Dependents:   aNXPCupCar

Motors_Out.cpp

Committer:
pkolar1
Date:
2018-07-01
Revision:
2:a5effe31eec0
Parent:
1:6b22ad85a4ab
Child:
3:8a5c76437c38

File content as of revision 2:a5effe31eec0:

#include "mbed.h"
#include "Motors_Out.h"


Motors_Out::Motors_Out(PinName pwm1, PinName pwm2, PinName pwm3, PinName pwm4, PinName pwms, PinName en):
 DC_A1(pwm1),DC_A2(pwm2),DC_B1(pwm3),DC_B2(pwm4),SERVO(pwms),H_ENABLE(en)
{
    init();
}

void Motors_Out::init()
{
    smj=3;
    pol=16;
    brz=1;
}

void Motors_Out::slanjePodataka(int smj1, int pol1, int brz1)
{
    smj=smj1;
    pol=pol1;
    brz=brz1;
}

void Motors_Out::brzina_DC_motora()
{
    DC_A1.period_us(100);
    switch(smj) {
        case 1:
            H_ENABLE=1;
            DC_B2=0;
            DC_A2=DC_B2;
            DC_A1=(float)(brz-1)/14;
            DC_B1=DC_A1;
            break;
        case 2:
            H_ENABLE=1;
            DC_B1=0;
            DC_A1=DC_B1;
            DC_A2=(float)(brz-1)/14;
            DC_B2=DC_A2;
            break;
        default:
            H_ENABLE=0;
            DC_B1=0;
            DC_B2=0;
            DC_A1=DC_B1;
            DC_A2=DC_B2;
    }
}

void Motors_Out::polozaj_serva()
{
    SERVO.period(0.010);
    SERVO=(float)(pol+29)/300;
}