mbed
Fork of Drive by
drive.cpp@1:49bfb0a87fbc, 2017-04-27 (annotated)
- Committer:
- stueckler
- Date:
- Thu Apr 27 08:40:17 2017 +0000
- Revision:
- 1:49bfb0a87fbc
- Parent:
- 0:5dcb55a2880c
Bertl16 fahren
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stueckler | 0:5dcb55a2880c | 1 | #include "drive.h" |
stueckler | 0:5dcb55a2880c | 2 | #include "mbed.h" |
stueckler | 0:5dcb55a2880c | 3 | |
stueckler | 1:49bfb0a87fbc | 4 | PwmOut MotorL_EN(P1_19); |
stueckler | 1:49bfb0a87fbc | 5 | DigitalOut MotorL_FORWARD(P2_15); |
stueckler | 1:49bfb0a87fbc | 6 | DigitalOut MotorL_REVERSE(P2_14); |
stueckler | 0:5dcb55a2880c | 7 | |
stueckler | 1:49bfb0a87fbc | 8 | PwmOut MotorR_EN(P2_19); |
stueckler | 1:49bfb0a87fbc | 9 | DigitalOut MotorR_FORWARD(P2_20); |
stueckler | 1:49bfb0a87fbc | 10 | DigitalOut MotorR_REVERSE(P2_21); |
stueckler | 0:5dcb55a2880c | 11 | |
stueckler | 1:49bfb0a87fbc | 12 | DigitalOut Von(P2_13); // 12V on |
stueckler | 0:5dcb55a2880c | 13 | |
stueckler | 0:5dcb55a2880c | 14 | void MotInit() |
stueckler | 0:5dcb55a2880c | 15 | { |
stueckler | 1:49bfb0a87fbc | 16 | Von.write(1); |
stueckler | 0:5dcb55a2880c | 17 | MotorL_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 18 | MotorL_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 19 | MotorL_EN.period_us(255); |
stueckler | 0:5dcb55a2880c | 20 | MotorL_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 21 | |
stueckler | 0:5dcb55a2880c | 22 | MotorR_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 23 | MotorR_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 24 | MotorR_EN.period_us(255); |
stueckler | 0:5dcb55a2880c | 25 | MotorR_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 26 | } |
stueckler | 0:5dcb55a2880c | 27 | |
stueckler | 0:5dcb55a2880c | 28 | void BrakeMotL() |
stueckler | 0:5dcb55a2880c | 29 | { |
stueckler | 1:49bfb0a87fbc | 30 | MotorL_FORWARD.write(1); |
stueckler | 1:49bfb0a87fbc | 31 | MotorL_REVERSE.write(1); |
stueckler | 1:49bfb0a87fbc | 32 | MotorL_EN.pulsewidth_us(255); |
stueckler | 0:5dcb55a2880c | 33 | } |
stueckler | 0:5dcb55a2880c | 34 | |
stueckler | 0:5dcb55a2880c | 35 | void BrakeMotR() |
stueckler | 0:5dcb55a2880c | 36 | { |
stueckler | 1:49bfb0a87fbc | 37 | MotorR_FORWARD.write(1); |
stueckler | 1:49bfb0a87fbc | 38 | MotorR_REVERSE.write(1); |
stueckler | 1:49bfb0a87fbc | 39 | MotorR_EN.pulsewidth_us(255); |
stueckler | 0:5dcb55a2880c | 40 | } |
stueckler | 0:5dcb55a2880c | 41 | |
stueckler | 0:5dcb55a2880c | 42 | void MotL(int aPow) // aPow: -255...255 |
stueckler | 0:5dcb55a2880c | 43 | { |
stueckler | 0:5dcb55a2880c | 44 | if( aPow==0 ) |
stueckler | 0:5dcb55a2880c | 45 | { |
stueckler | 0:5dcb55a2880c | 46 | BrakeMotL(); |
stueckler | 0:5dcb55a2880c | 47 | return; |
stueckler | 0:5dcb55a2880c | 48 | } |
stueckler | 0:5dcb55a2880c | 49 | |
stueckler | 0:5dcb55a2880c | 50 | if( aPow>255 ) |
stueckler | 0:5dcb55a2880c | 51 | aPow=255; |
stueckler | 0:5dcb55a2880c | 52 | if( aPow<-255 ) |
stueckler | 0:5dcb55a2880c | 53 | aPow=-255; |
stueckler | 0:5dcb55a2880c | 54 | |
stueckler | 0:5dcb55a2880c | 55 | if( aPow>0 ) |
stueckler | 0:5dcb55a2880c | 56 | { |
stueckler | 0:5dcb55a2880c | 57 | MotorL_FORWARD.write(1); |
stueckler | 0:5dcb55a2880c | 58 | MotorL_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 59 | MotorL_EN.pulsewidth_us(aPow); |
stueckler | 0:5dcb55a2880c | 60 | } |
stueckler | 0:5dcb55a2880c | 61 | if( aPow<0 ) |
stueckler | 0:5dcb55a2880c | 62 | { |
stueckler | 0:5dcb55a2880c | 63 | MotorL_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 64 | MotorL_REVERSE.write(1); |
stueckler | 0:5dcb55a2880c | 65 | MotorL_EN.pulsewidth_us(-aPow); |
stueckler | 0:5dcb55a2880c | 66 | } |
stueckler | 0:5dcb55a2880c | 67 | } |
stueckler | 0:5dcb55a2880c | 68 | |
stueckler | 0:5dcb55a2880c | 69 | void MotR(int aPow) |
stueckler | 0:5dcb55a2880c | 70 | { |
stueckler | 0:5dcb55a2880c | 71 | if( aPow==0 ) |
stueckler | 0:5dcb55a2880c | 72 | { |
stueckler | 0:5dcb55a2880c | 73 | BrakeMotR(); |
stueckler | 0:5dcb55a2880c | 74 | return; |
stueckler | 0:5dcb55a2880c | 75 | } |
stueckler | 0:5dcb55a2880c | 76 | |
stueckler | 0:5dcb55a2880c | 77 | if( aPow>255 ) |
stueckler | 0:5dcb55a2880c | 78 | aPow=255; |
stueckler | 0:5dcb55a2880c | 79 | if( aPow<-255 ) |
stueckler | 0:5dcb55a2880c | 80 | aPow=-255; |
stueckler | 0:5dcb55a2880c | 81 | |
stueckler | 0:5dcb55a2880c | 82 | if( aPow>0 ) |
stueckler | 0:5dcb55a2880c | 83 | { |
stueckler | 0:5dcb55a2880c | 84 | MotorR_FORWARD.write(1); |
stueckler | 0:5dcb55a2880c | 85 | MotorR_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 86 | MotorR_EN.pulsewidth_us(aPow); |
stueckler | 0:5dcb55a2880c | 87 | } |
stueckler | 0:5dcb55a2880c | 88 | if( aPow<0 ) |
stueckler | 0:5dcb55a2880c | 89 | { |
stueckler | 0:5dcb55a2880c | 90 | MotorR_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 91 | MotorR_REVERSE.write(1); |
stueckler | 0:5dcb55a2880c | 92 | MotorR_EN.pulsewidth_us(-aPow); |
stueckler | 0:5dcb55a2880c | 93 | } |
stueckler | 0:5dcb55a2880c | 94 | } |