Justyna Guzik
Dependencies: HCSR04v2 Servo mbed
Fork of
STM_read
by 
Dominik Święch
Engine.cpp@4:84f836aaf390, 2016-06-28 (annotated)
- Committer:
- jguzik
- Date:
- Tue Jun 28 12:52:35 2016 +0000
- Revision:
- 4:84f836aaf390
- Parent:
- 0:5919ea7b3b90
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yruiewyrui3 | 0:5919ea7b3b90 | 1 | #include "Engine.h" |
| yruiewyrui3 | 0:5919ea7b3b90 | 2 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 3 | // przelicza procenty na pwn: |
| yruiewyrui3 | 0:5919ea7b3b90 | 4 | // - wartosc poczatkowa - 0.2377, |
| yruiewyrui3 | 0:5919ea7b3b90 | 5 | // - prog - 0.0077 |
| yruiewyrui3 | 0:5919ea7b3b90 | 6 | float Engine::getPwm(float speed){ |
| yruiewyrui3 | 0:5919ea7b3b90 | 7 | if (speed == 0) |
| yruiewyrui3 | 0:5919ea7b3b90 | 8 | return 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 9 | else if (speed > 0) |
| yruiewyrui3 | 0:5919ea7b3b90 | 10 | return 0.2377f + (fabs(speed) - 1) * 0.0077f; |
| yruiewyrui3 | 0:5919ea7b3b90 | 11 | else |
| yruiewyrui3 | 0:5919ea7b3b90 | 12 | return 0.406f + (fabs(speed) - 1) * 0.006f; |
| yruiewyrui3 | 0:5919ea7b3b90 | 13 | } |
| yruiewyrui3 | 0:5919ea7b3b90 | 14 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 15 | void Engine::move(int speed){ |
| yruiewyrui3 | 0:5919ea7b3b90 | 16 | if (speed > 100) |
| yruiewyrui3 | 0:5919ea7b3b90 | 17 | speed =100; |
| yruiewyrui3 | 0:5919ea7b3b90 | 18 | else if (speed < -100) |
| yruiewyrui3 | 0:5919ea7b3b90 | 19 | speed = -100; |
| yruiewyrui3 | 0:5919ea7b3b90 | 20 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 21 | if (speed == 0){ |
| yruiewyrui3 | 0:5919ea7b3b90 | 22 | //soft stop |
| yruiewyrui3 | 0:5919ea7b3b90 | 23 | _fwd = 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 24 | _rev = 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 25 | return; |
| yruiewyrui3 | 0:5919ea7b3b90 | 26 | } |
| yruiewyrui3 | 0:5919ea7b3b90 | 27 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 28 | if (speed < 0){ |
| yruiewyrui3 | 0:5919ea7b3b90 | 29 | _fwd = 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 30 | _rev = 1; |
| yruiewyrui3 | 0:5919ea7b3b90 | 31 | } |
| yruiewyrui3 | 0:5919ea7b3b90 | 32 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 33 | else{ |
| yruiewyrui3 | 0:5919ea7b3b90 | 34 | _fwd = 1; |
| yruiewyrui3 | 0:5919ea7b3b90 | 35 | _rev = 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 36 | } |
| yruiewyrui3 | 0:5919ea7b3b90 | 37 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 38 | _pwm = getPwm(speed); |
| yruiewyrui3 | 0:5919ea7b3b90 | 39 | } |
| yruiewyrui3 | 0:5919ea7b3b90 | 40 | |
| yruiewyrui3 | 0:5919ea7b3b90 | 41 | //hard stop |
| yruiewyrui3 | 0:5919ea7b3b90 | 42 | void Engine::stop(){ |
| yruiewyrui3 | 0:5919ea7b3b90 | 43 | _pwm = 0; |
| yruiewyrui3 | 0:5919ea7b3b90 | 44 | } |