mbed
Fork of Drive by
drive.cpp
- Committer:
- stueckler
- Date:
- 2017-04-27
- Revision:
- 1:49bfb0a87fbc
- Parent:
- 0:5dcb55a2880c
File content as of revision 1:49bfb0a87fbc:
#include "drive.h" #include "mbed.h" PwmOut MotorL_EN(P1_19); DigitalOut MotorL_FORWARD(P2_15); DigitalOut MotorL_REVERSE(P2_14); PwmOut MotorR_EN(P2_19); DigitalOut MotorR_FORWARD(P2_20); DigitalOut MotorR_REVERSE(P2_21); DigitalOut Von(P2_13); // 12V on void MotInit() { Von.write(1); MotorL_FORWARD.write(0); MotorL_REVERSE.write(0); MotorL_EN.period_us(255); MotorL_EN.pulsewidth_us(0); MotorR_FORWARD.write(0); MotorR_REVERSE.write(0); MotorR_EN.period_us(255); MotorR_EN.pulsewidth_us(0); } void BrakeMotL() { MotorL_FORWARD.write(1); MotorL_REVERSE.write(1); MotorL_EN.pulsewidth_us(255); } void BrakeMotR() { MotorR_FORWARD.write(1); MotorR_REVERSE.write(1); MotorR_EN.pulsewidth_us(255); } void MotL(int aPow) // aPow: -255...255 { if( aPow==0 ) { BrakeMotL(); return; } if( aPow>255 ) aPow=255; if( aPow<-255 ) aPow=-255; if( aPow>0 ) { MotorL_FORWARD.write(1); MotorL_REVERSE.write(0); MotorL_EN.pulsewidth_us(aPow); } if( aPow<0 ) { MotorL_FORWARD.write(0); MotorL_REVERSE.write(1); MotorL_EN.pulsewidth_us(-aPow); } } void MotR(int aPow) { if( aPow==0 ) { BrakeMotR(); return; } if( aPow>255 ) aPow=255; if( aPow<-255 ) aPow=-255; if( aPow>0 ) { MotorR_FORWARD.write(1); MotorR_REVERSE.write(0); MotorR_EN.pulsewidth_us(aPow); } if( aPow<0 ) { MotorR_FORWARD.write(0); MotorR_REVERSE.write(1); MotorR_EN.pulsewidth_us(-aPow); } }