mbed
Fork of Drive by
drive.cpp@0:5dcb55a2880c, 2015-08-18 (annotated)
- Committer:
- stueckler
- Date:
- Tue Aug 18 09:00:51 2015 +0000
- Revision:
- 0:5dcb55a2880c
- Child:
- 1:49bfb0a87fbc
mbed
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 | 0:5dcb55a2880c | 4 | PwmOut MotorL_EN(P1_15); |
stueckler | 0:5dcb55a2880c | 5 | DigitalOut MotorL_FORWARD(P1_1); |
stueckler | 0:5dcb55a2880c | 6 | DigitalOut MotorL_REVERSE(P1_0); |
stueckler | 0:5dcb55a2880c | 7 | InterruptIn TachoL(P1_12); |
stueckler | 0:5dcb55a2880c | 8 | |
stueckler | 0:5dcb55a2880c | 9 | PwmOut MotorR_EN(P0_21); |
stueckler | 0:5dcb55a2880c | 10 | DigitalOut MotorR_FORWARD(P1_3); |
stueckler | 0:5dcb55a2880c | 11 | DigitalOut MotorR_REVERSE(P1_4); |
stueckler | 0:5dcb55a2880c | 12 | InterruptIn TachoR(P1_13); |
stueckler | 0:5dcb55a2880c | 13 | |
stueckler | 0:5dcb55a2880c | 14 | int counterL, counterR; |
stueckler | 0:5dcb55a2880c | 15 | int turnL, turnR; |
stueckler | 0:5dcb55a2880c | 16 | |
stueckler | 0:5dcb55a2880c | 17 | int map(int x, int in_min, int in_max, int out_min, int out_max) |
stueckler | 0:5dcb55a2880c | 18 | { |
stueckler | 0:5dcb55a2880c | 19 | long t1 = (x - in_min) * (out_max - out_min); |
stueckler | 0:5dcb55a2880c | 20 | long t2 = (in_max - in_min) + out_min; |
stueckler | 0:5dcb55a2880c | 21 | long t3 = t1/t2; |
stueckler | 0:5dcb55a2880c | 22 | int t4 = t3 + 0.5; |
stueckler | 0:5dcb55a2880c | 23 | return t4; |
stueckler | 0:5dcb55a2880c | 24 | } |
stueckler | 0:5dcb55a2880c | 25 | |
stueckler | 0:5dcb55a2880c | 26 | int getL() |
stueckler | 0:5dcb55a2880c | 27 | { |
stueckler | 0:5dcb55a2880c | 28 | return counterL; |
stueckler | 0:5dcb55a2880c | 29 | } |
stueckler | 0:5dcb55a2880c | 30 | |
stueckler | 0:5dcb55a2880c | 31 | int getR() |
stueckler | 0:5dcb55a2880c | 32 | { |
stueckler | 0:5dcb55a2880c | 33 | return counterR; |
stueckler | 0:5dcb55a2880c | 34 | } |
stueckler | 0:5dcb55a2880c | 35 | |
stueckler | 0:5dcb55a2880c | 36 | void setL(int val) |
stueckler | 0:5dcb55a2880c | 37 | { |
stueckler | 0:5dcb55a2880c | 38 | counterL = val; |
stueckler | 0:5dcb55a2880c | 39 | } |
stueckler | 0:5dcb55a2880c | 40 | |
stueckler | 0:5dcb55a2880c | 41 | void setR(int val) |
stueckler | 0:5dcb55a2880c | 42 | { |
stueckler | 0:5dcb55a2880c | 43 | counterR = val; |
stueckler | 0:5dcb55a2880c | 44 | } |
stueckler | 0:5dcb55a2880c | 45 | |
stueckler | 0:5dcb55a2880c | 46 | |
stueckler | 0:5dcb55a2880c | 47 | void handleL() |
stueckler | 0:5dcb55a2880c | 48 | { |
stueckler | 0:5dcb55a2880c | 49 | if (counterL==0) |
stueckler | 0:5dcb55a2880c | 50 | return; |
stueckler | 0:5dcb55a2880c | 51 | |
stueckler | 0:5dcb55a2880c | 52 | counterL--; |
stueckler | 0:5dcb55a2880c | 53 | if (counterL==0) |
stueckler | 0:5dcb55a2880c | 54 | MotorL_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 55 | } |
stueckler | 0:5dcb55a2880c | 56 | |
stueckler | 0:5dcb55a2880c | 57 | void handleR() |
stueckler | 0:5dcb55a2880c | 58 | { |
stueckler | 0:5dcb55a2880c | 59 | if (counterR==0) |
stueckler | 0:5dcb55a2880c | 60 | return; |
stueckler | 0:5dcb55a2880c | 61 | |
stueckler | 0:5dcb55a2880c | 62 | counterR--; |
stueckler | 0:5dcb55a2880c | 63 | if (counterR==0) |
stueckler | 0:5dcb55a2880c | 64 | MotorR_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 65 | } |
stueckler | 0:5dcb55a2880c | 66 | |
stueckler | 0:5dcb55a2880c | 67 | void MotConfig(int tl, int tr) |
stueckler | 0:5dcb55a2880c | 68 | { |
stueckler | 0:5dcb55a2880c | 69 | turnL = tl; |
stueckler | 0:5dcb55a2880c | 70 | turnR = tr; |
stueckler | 0:5dcb55a2880c | 71 | } |
stueckler | 0:5dcb55a2880c | 72 | |
stueckler | 0:5dcb55a2880c | 73 | void MotInit() |
stueckler | 0:5dcb55a2880c | 74 | { |
stueckler | 0:5dcb55a2880c | 75 | MotorL_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 76 | MotorL_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 77 | MotorL_EN.period_us(255); |
stueckler | 0:5dcb55a2880c | 78 | MotorL_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 79 | counterL=0; |
stueckler | 0:5dcb55a2880c | 80 | turnL=24; |
stueckler | 0:5dcb55a2880c | 81 | TachoL.rise(&handleL); |
stueckler | 0:5dcb55a2880c | 82 | |
stueckler | 0:5dcb55a2880c | 83 | MotorR_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 84 | MotorR_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 85 | MotorR_EN.period_us(255); |
stueckler | 0:5dcb55a2880c | 86 | MotorR_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 87 | counterR=0; |
stueckler | 0:5dcb55a2880c | 88 | turnR=24; |
stueckler | 0:5dcb55a2880c | 89 | TachoR.rise(&handleR); |
stueckler | 0:5dcb55a2880c | 90 | } |
stueckler | 0:5dcb55a2880c | 91 | |
stueckler | 0:5dcb55a2880c | 92 | void BrakeMotL() |
stueckler | 0:5dcb55a2880c | 93 | { |
stueckler | 0:5dcb55a2880c | 94 | MotorL_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 95 | MotorL_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 96 | MotorL_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 97 | } |
stueckler | 0:5dcb55a2880c | 98 | |
stueckler | 0:5dcb55a2880c | 99 | void BrakeMotR() |
stueckler | 0:5dcb55a2880c | 100 | { |
stueckler | 0:5dcb55a2880c | 101 | MotorR_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 102 | MotorR_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 103 | MotorR_EN.pulsewidth_us(0); |
stueckler | 0:5dcb55a2880c | 104 | } |
stueckler | 0:5dcb55a2880c | 105 | |
stueckler | 0:5dcb55a2880c | 106 | void MotL(int aPow) // aPow: -255...255 |
stueckler | 0:5dcb55a2880c | 107 | { |
stueckler | 0:5dcb55a2880c | 108 | if( aPow==0 ) |
stueckler | 0:5dcb55a2880c | 109 | { |
stueckler | 0:5dcb55a2880c | 110 | BrakeMotL(); |
stueckler | 0:5dcb55a2880c | 111 | return; |
stueckler | 0:5dcb55a2880c | 112 | } |
stueckler | 0:5dcb55a2880c | 113 | |
stueckler | 0:5dcb55a2880c | 114 | if( aPow>255 ) |
stueckler | 0:5dcb55a2880c | 115 | aPow=255; |
stueckler | 0:5dcb55a2880c | 116 | if( aPow<-255 ) |
stueckler | 0:5dcb55a2880c | 117 | aPow=-255; |
stueckler | 0:5dcb55a2880c | 118 | |
stueckler | 0:5dcb55a2880c | 119 | if( aPow>0 ) |
stueckler | 0:5dcb55a2880c | 120 | { |
stueckler | 0:5dcb55a2880c | 121 | MotorL_FORWARD.write(1); |
stueckler | 0:5dcb55a2880c | 122 | MotorL_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 123 | MotorL_EN.pulsewidth_us(aPow); |
stueckler | 0:5dcb55a2880c | 124 | } |
stueckler | 0:5dcb55a2880c | 125 | if( aPow<0 ) |
stueckler | 0:5dcb55a2880c | 126 | { |
stueckler | 0:5dcb55a2880c | 127 | MotorL_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 128 | MotorL_REVERSE.write(1); |
stueckler | 0:5dcb55a2880c | 129 | MotorL_EN.pulsewidth_us(-aPow); |
stueckler | 0:5dcb55a2880c | 130 | } |
stueckler | 0:5dcb55a2880c | 131 | } |
stueckler | 0:5dcb55a2880c | 132 | |
stueckler | 0:5dcb55a2880c | 133 | void MotR(int aPow) |
stueckler | 0:5dcb55a2880c | 134 | { |
stueckler | 0:5dcb55a2880c | 135 | if( aPow==0 ) |
stueckler | 0:5dcb55a2880c | 136 | { |
stueckler | 0:5dcb55a2880c | 137 | BrakeMotR(); |
stueckler | 0:5dcb55a2880c | 138 | return; |
stueckler | 0:5dcb55a2880c | 139 | } |
stueckler | 0:5dcb55a2880c | 140 | |
stueckler | 0:5dcb55a2880c | 141 | if( aPow>255 ) |
stueckler | 0:5dcb55a2880c | 142 | aPow=255; |
stueckler | 0:5dcb55a2880c | 143 | if( aPow<-255 ) |
stueckler | 0:5dcb55a2880c | 144 | aPow=-255; |
stueckler | 0:5dcb55a2880c | 145 | |
stueckler | 0:5dcb55a2880c | 146 | if( aPow>0 ) |
stueckler | 0:5dcb55a2880c | 147 | { |
stueckler | 0:5dcb55a2880c | 148 | MotorR_FORWARD.write(1); |
stueckler | 0:5dcb55a2880c | 149 | MotorR_REVERSE.write(0); |
stueckler | 0:5dcb55a2880c | 150 | MotorR_EN.pulsewidth_us(aPow); |
stueckler | 0:5dcb55a2880c | 151 | } |
stueckler | 0:5dcb55a2880c | 152 | if( aPow<0 ) |
stueckler | 0:5dcb55a2880c | 153 | { |
stueckler | 0:5dcb55a2880c | 154 | MotorR_FORWARD.write(0); |
stueckler | 0:5dcb55a2880c | 155 | MotorR_REVERSE.write(1); |
stueckler | 0:5dcb55a2880c | 156 | MotorR_EN.pulsewidth_us(-aPow); |
stueckler | 0:5dcb55a2880c | 157 | } |
stueckler | 0:5dcb55a2880c | 158 | } |
stueckler | 0:5dcb55a2880c | 159 | |
stueckler | 0:5dcb55a2880c | 160 | void MotDegL(int aPow, int deg) // aPow: -255...255 deg: 0...360 |
stueckler | 0:5dcb55a2880c | 161 | { |
stueckler | 0:5dcb55a2880c | 162 | counterL=map(deg,0,360,0,turnL); |
stueckler | 0:5dcb55a2880c | 163 | MotL(aPow); |
stueckler | 0:5dcb55a2880c | 164 | } |
stueckler | 0:5dcb55a2880c | 165 | |
stueckler | 0:5dcb55a2880c | 166 | void MotDegR(int aPow, int deg) |
stueckler | 0:5dcb55a2880c | 167 | { |
stueckler | 0:5dcb55a2880c | 168 | counterR=map(deg,0,360,0,turnR); |
stueckler | 0:5dcb55a2880c | 169 | MotR(aPow); |
stueckler | 0:5dcb55a2880c | 170 | } |