BULME_AHEL20 / Drive16

mbed

Fork of Drive by BULME_BHEL18

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?

UserRevisionLine numberNew 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 }