Justyna Guzik / Mbed 2 deprecated STM_read24_06

Dependencies:   HCSR04v2 Servo mbed

Fork of STM_read by Dominik Święch

Engine.cpp

Committer:
jguzik
Date:
2016-06-28
Revision:
4:84f836aaf390
Parent:
0:5919ea7b3b90

File content as of revision 4:84f836aaf390:

#include "Engine.h"

// przelicza procenty na pwn:
// - wartosc poczatkowa - 0.2377,
// - prog - 0.0077
float Engine::getPwm(float speed){
    if (speed == 0)
        return 0;
    else if (speed > 0)
        return 0.2377f + (fabs(speed) - 1) * 0.0077f;
    else
        return 0.406f + (fabs(speed) - 1) * 0.006f;
    }

void Engine::move(int speed){
    if (speed > 100)
        speed =100;
    else if (speed < -100)
        speed = -100;
    
    if (speed == 0){
        //soft stop
        _fwd = 0;
        _rev = 0;
        return;
        }
            
    if (speed < 0){
        _fwd = 0;
        _rev = 1;
        }
        
    else{
        _fwd = 1;
        _rev = 0;
        }
    
    _pwm = getPwm(speed);
}

//hard stop
void Engine::stop(){
    _pwm = 0;
}