mbed
Fork of Drive by
drive.cpp
- Committer:
- stueckler
- Date:
- 2015-08-18
- Revision:
- 0:5dcb55a2880c
- Child:
- 1:49bfb0a87fbc
File content as of revision 0:5dcb55a2880c:
#include "drive.h" #include "mbed.h" PwmOut MotorL_EN(P1_15); DigitalOut MotorL_FORWARD(P1_1); DigitalOut MotorL_REVERSE(P1_0); InterruptIn TachoL(P1_12); PwmOut MotorR_EN(P0_21); DigitalOut MotorR_FORWARD(P1_3); DigitalOut MotorR_REVERSE(P1_4); InterruptIn TachoR(P1_13); int counterL, counterR; int turnL, turnR; int map(int x, int in_min, int in_max, int out_min, int out_max) { long t1 = (x - in_min) * (out_max - out_min); long t2 = (in_max - in_min) + out_min; long t3 = t1/t2; int t4 = t3 + 0.5; return t4; } int getL() { return counterL; } int getR() { return counterR; } void setL(int val) { counterL = val; } void setR(int val) { counterR = val; } void handleL() { if (counterL==0) return; counterL--; if (counterL==0) MotorL_EN.pulsewidth_us(0); } void handleR() { if (counterR==0) return; counterR--; if (counterR==0) MotorR_EN.pulsewidth_us(0); } void MotConfig(int tl, int tr) { turnL = tl; turnR = tr; } void MotInit() { MotorL_FORWARD.write(0); MotorL_REVERSE.write(0); MotorL_EN.period_us(255); MotorL_EN.pulsewidth_us(0); counterL=0; turnL=24; TachoL.rise(&handleL); MotorR_FORWARD.write(0); MotorR_REVERSE.write(0); MotorR_EN.period_us(255); MotorR_EN.pulsewidth_us(0); counterR=0; turnR=24; TachoR.rise(&handleR); } void BrakeMotL() { MotorL_FORWARD.write(0); MotorL_REVERSE.write(0); MotorL_EN.pulsewidth_us(0); } void BrakeMotR() { MotorR_FORWARD.write(0); MotorR_REVERSE.write(0); MotorR_EN.pulsewidth_us(0); } 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); } } void MotDegL(int aPow, int deg) // aPow: -255...255 deg: 0...360 { counterL=map(deg,0,360,0,turnL); MotL(aPow); } void MotDegR(int aPow, int deg) { counterR=map(deg,0,360,0,turnR); MotR(aPow); }