BULME_BHEL18 / Drive

mbed

drive.cpp@0:5dcb55a2880c, 2015-08-18 (annotated)

Committer:
stueckler
Date:
Tue Aug 18 09:00:51 2015 +0000
Revision:
0:5dcb55a2880c
mbed

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