mbed
Fork of Drive by
Diff: drive.cpp
- Revision:
- 0:5dcb55a2880c
- Child:
- 1:49bfb0a87fbc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/drive.cpp Tue Aug 18 09:00:51 2015 +0000 @@ -0,0 +1,170 @@ +#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); +}