User | Revision | Line number | New contents of line |
littlexc |
0:774d7a3e63ae
|
1
|
/*motor driver libary modified from the following libary,
|
littlexc |
0:774d7a3e63ae
|
2
|
*
|
littlexc |
0:774d7a3e63ae
|
3
|
* mbed simple H-bridge motor controller
|
littlexc |
0:774d7a3e63ae
|
4
|
* Copyright (c) 2007-2010, sford
|
littlexc |
0:774d7a3e63ae
|
5
|
*
|
littlexc |
0:774d7a3e63ae
|
6
|
* by Christopher Hasler.
|
littlexc |
0:774d7a3e63ae
|
7
|
*
|
littlexc |
0:774d7a3e63ae
|
8
|
* from sford's libary,
|
littlexc |
0:774d7a3e63ae
|
9
|
*
|
littlexc |
0:774d7a3e63ae
|
10
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
littlexc |
0:774d7a3e63ae
|
11
|
* of this software and associated documentation files (the "Software"), to deal
|
littlexc |
0:774d7a3e63ae
|
12
|
* in the Software without restriction, including without limitation the rights
|
littlexc |
0:774d7a3e63ae
|
13
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
littlexc |
0:774d7a3e63ae
|
14
|
* copies of the Software, and to permit persons to whom the Software is
|
littlexc |
0:774d7a3e63ae
|
15
|
* furnished to do so, subject to the following conditions:
|
littlexc |
0:774d7a3e63ae
|
16
|
*
|
littlexc |
0:774d7a3e63ae
|
17
|
* The above copyright notice and this permission notice shall be included in
|
littlexc |
0:774d7a3e63ae
|
18
|
* all copies or substantial portions of the Software.
|
littlexc |
0:774d7a3e63ae
|
19
|
*
|
littlexc |
0:774d7a3e63ae
|
20
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
littlexc |
0:774d7a3e63ae
|
21
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
littlexc |
0:774d7a3e63ae
|
22
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
littlexc |
0:774d7a3e63ae
|
23
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
littlexc |
0:774d7a3e63ae
|
24
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
littlexc |
0:774d7a3e63ae
|
25
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
littlexc |
0:774d7a3e63ae
|
26
|
* THE SOFTWARE.
|
littlexc |
0:774d7a3e63ae
|
27
|
*/
|
littlexc |
0:774d7a3e63ae
|
28
|
|
littlexc |
0:774d7a3e63ae
|
29
|
#include "motordrivercs.h"
|
littlexc |
0:774d7a3e63ae
|
30
|
|
littlexc |
0:774d7a3e63ae
|
31
|
#include "mbed.h"
|
littlexc |
0:774d7a3e63ae
|
32
|
|
littlexc |
0:774d7a3e63ae
|
33
|
Motor::Motor(PinName pwm, PinName fwd, PinName rev, PinName ain, float ohms, float limit, float cutby, int brakeable):
|
littlexc |
0:774d7a3e63ae
|
34
|
_pwm(pwm), _fwd(fwd), _rev(rev), _ain(ain) {
|
littlexc |
0:774d7a3e63ae
|
35
|
|
littlexc |
0:774d7a3e63ae
|
36
|
// Set initial condition of PWM
|
littlexc |
0:774d7a3e63ae
|
37
|
_pwm.period(0.001);
|
littlexc |
0:774d7a3e63ae
|
38
|
_pwm = 0;
|
littlexc |
0:774d7a3e63ae
|
39
|
|
littlexc |
0:774d7a3e63ae
|
40
|
// Initial condition of output enables
|
littlexc |
0:774d7a3e63ae
|
41
|
_fwd = 0;
|
littlexc |
0:774d7a3e63ae
|
42
|
_rev = 0;
|
littlexc |
0:774d7a3e63ae
|
43
|
|
littlexc |
0:774d7a3e63ae
|
44
|
//set if the motor dirver is capable of braking. (addition)
|
littlexc |
0:774d7a3e63ae
|
45
|
Brakeable= brakeable;
|
littlexc |
0:774d7a3e63ae
|
46
|
sign = 0;//i.e nothing.
|
littlexc |
0:774d7a3e63ae
|
47
|
currentlimit = limit;
|
littlexc |
0:774d7a3e63ae
|
48
|
modifyspeed = cutby;
|
littlexc |
0:774d7a3e63ae
|
49
|
}
|
littlexc |
0:774d7a3e63ae
|
50
|
|
littlexc |
0:774d7a3e63ae
|
51
|
float Motor::speedcs(float speed) {
|
littlexc |
0:774d7a3e63ae
|
52
|
float temp = 0;
|
littlexc |
0:774d7a3e63ae
|
53
|
if (sign == 0) {
|
littlexc |
0:774d7a3e63ae
|
54
|
_fwd = (speed > 0.0);
|
littlexc |
0:774d7a3e63ae
|
55
|
_rev = (speed < 0.0);
|
littlexc |
0:774d7a3e63ae
|
56
|
_pwm = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
57
|
temp = current();
|
littlexc |
0:774d7a3e63ae
|
58
|
if (temp > currentlimit) {
|
littlexc |
0:774d7a3e63ae
|
59
|
temp = abs(speed) / modifyspeed;
|
littlexc |
0:774d7a3e63ae
|
60
|
_pwm = temp;
|
littlexc |
0:774d7a3e63ae
|
61
|
} else
|
littlexc |
0:774d7a3e63ae
|
62
|
temp = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
63
|
} else if (sign == 1) {
|
littlexc |
0:774d7a3e63ae
|
64
|
if (speed < 0) {
|
littlexc |
0:774d7a3e63ae
|
65
|
_fwd = (speed > 0.0);
|
littlexc |
0:774d7a3e63ae
|
66
|
_rev = (speed < 0.0);
|
littlexc |
0:774d7a3e63ae
|
67
|
_pwm = 0;
|
littlexc |
0:774d7a3e63ae
|
68
|
temp = 0;
|
littlexc |
0:774d7a3e63ae
|
69
|
} else {
|
littlexc |
0:774d7a3e63ae
|
70
|
_fwd = (speed > 0.0);
|
littlexc |
0:774d7a3e63ae
|
71
|
_rev = (speed < 0.0);
|
littlexc |
0:774d7a3e63ae
|
72
|
_pwm = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
73
|
temp = current();
|
littlexc |
0:774d7a3e63ae
|
74
|
if (temp > currentlimit) {
|
littlexc |
0:774d7a3e63ae
|
75
|
temp = abs(speed) / modifyspeed;
|
littlexc |
0:774d7a3e63ae
|
76
|
_pwm = temp;
|
littlexc |
0:774d7a3e63ae
|
77
|
} else
|
littlexc |
0:774d7a3e63ae
|
78
|
temp = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
79
|
}
|
littlexc |
0:774d7a3e63ae
|
80
|
} else if (sign == -1) {
|
littlexc |
0:774d7a3e63ae
|
81
|
if (speed > 0) {
|
littlexc |
0:774d7a3e63ae
|
82
|
_fwd = (speed > 0.0);
|
littlexc |
0:774d7a3e63ae
|
83
|
_rev = (speed < 0.0);
|
littlexc |
0:774d7a3e63ae
|
84
|
_pwm = 0;
|
littlexc |
0:774d7a3e63ae
|
85
|
temp = 0;
|
littlexc |
0:774d7a3e63ae
|
86
|
} else {
|
littlexc |
0:774d7a3e63ae
|
87
|
_fwd = (speed > 0.0);
|
littlexc |
0:774d7a3e63ae
|
88
|
_rev = (speed < 0.0);
|
littlexc |
0:774d7a3e63ae
|
89
|
_pwm = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
90
|
temp = current();
|
littlexc |
0:774d7a3e63ae
|
91
|
if (temp > currentlimit) {
|
littlexc |
0:774d7a3e63ae
|
92
|
temp = abs(speed) / modifyspeed;
|
littlexc |
0:774d7a3e63ae
|
93
|
_pwm = temp;
|
littlexc |
0:774d7a3e63ae
|
94
|
} else
|
littlexc |
0:774d7a3e63ae
|
95
|
temp = abs(speed);
|
littlexc |
0:774d7a3e63ae
|
96
|
}
|
littlexc |
0:774d7a3e63ae
|
97
|
}
|
littlexc |
0:774d7a3e63ae
|
98
|
if (speed > 0)
|
littlexc |
0:774d7a3e63ae
|
99
|
sign = 1;
|
littlexc |
0:774d7a3e63ae
|
100
|
else if (speed < 0) {
|
littlexc |
0:774d7a3e63ae
|
101
|
sign = -1;
|
littlexc |
0:774d7a3e63ae
|
102
|
} else if (speed == 0) {
|
littlexc |
0:774d7a3e63ae
|
103
|
sign = 0;
|
littlexc |
0:774d7a3e63ae
|
104
|
}
|
littlexc |
0:774d7a3e63ae
|
105
|
return temp;
|
littlexc |
0:774d7a3e63ae
|
106
|
}
|
littlexc |
0:774d7a3e63ae
|
107
|
// (additions)
|
littlexc |
0:774d7a3e63ae
|
108
|
void Motor::coastcs(void) {
|
littlexc |
0:774d7a3e63ae
|
109
|
_fwd = 0;
|
littlexc |
0:774d7a3e63ae
|
110
|
_rev = 0;
|
littlexc |
0:774d7a3e63ae
|
111
|
_pwm = 0;
|
littlexc |
0:774d7a3e63ae
|
112
|
sign = 0;
|
littlexc |
0:774d7a3e63ae
|
113
|
}
|
littlexc |
0:774d7a3e63ae
|
114
|
|
littlexc |
0:774d7a3e63ae
|
115
|
float Motor::stopcs(float duty) {
|
littlexc |
0:774d7a3e63ae
|
116
|
float temp = 0;
|
littlexc |
0:774d7a3e63ae
|
117
|
if (Brakeable == 1) {
|
littlexc |
0:774d7a3e63ae
|
118
|
_fwd = 1;
|
littlexc |
0:774d7a3e63ae
|
119
|
_rev = 1;
|
littlexc |
0:774d7a3e63ae
|
120
|
_pwm = duty;
|
littlexc |
0:774d7a3e63ae
|
121
|
sign = 0;
|
littlexc |
0:774d7a3e63ae
|
122
|
temp = current();
|
littlexc |
0:774d7a3e63ae
|
123
|
if (temp > currentlimit)
|
littlexc |
0:774d7a3e63ae
|
124
|
temp = duty / modifyspeed;
|
littlexc |
0:774d7a3e63ae
|
125
|
_pwm = temp;
|
littlexc |
0:774d7a3e63ae
|
126
|
} else
|
littlexc |
0:774d7a3e63ae
|
127
|
return current();
|
littlexc |
0:774d7a3e63ae
|
128
|
return temp;
|
littlexc |
0:774d7a3e63ae
|
129
|
}
|
littlexc |
0:774d7a3e63ae
|
130
|
|
littlexc |
0:774d7a3e63ae
|
131
|
float Motor::current(void) {
|
littlexc |
0:774d7a3e63ae
|
132
|
float temp;
|
littlexc |
0:774d7a3e63ae
|
133
|
temp = _ain.read();
|
littlexc |
0:774d7a3e63ae
|
134
|
temp = temp * 0.303;// = 1/3.3, gives volts
|
littlexc |
0:774d7a3e63ae
|
135
|
temp = temp / ohms;//V=IR V/R = I
|
littlexc |
0:774d7a3e63ae
|
136
|
return temp;
|
littlexc |
0:774d7a3e63ae
|
137
|
}
|
littlexc |
0:774d7a3e63ae
|
138
|
|
littlexc |
0:774d7a3e63ae
|
139
|
/*
|
littlexc |
0:774d7a3e63ae
|
140
|
test code, this demonstrates working motor drivers.
|
littlexc |
0:774d7a3e63ae
|
141
|
|
littlexc |
0:774d7a3e63ae
|
142
|
Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
|
littlexc |
0:774d7a3e63ae
|
143
|
Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
|
littlexc |
0:774d7a3e63ae
|
144
|
int main() {
|
littlexc |
0:774d7a3e63ae
|
145
|
for (float s=-1.0; s < 1.0 ; s += 0.01) {
|
littlexc |
0:774d7a3e63ae
|
146
|
A.speed(s);
|
littlexc |
0:774d7a3e63ae
|
147
|
B.speed(s);
|
littlexc |
0:774d7a3e63ae
|
148
|
wait(0.02);
|
littlexc |
0:774d7a3e63ae
|
149
|
}
|
littlexc |
0:774d7a3e63ae
|
150
|
A.stop();
|
littlexc |
0:774d7a3e63ae
|
151
|
B.stop();
|
littlexc |
0:774d7a3e63ae
|
152
|
wait(1);
|
littlexc |
0:774d7a3e63ae
|
153
|
A.coast();
|
littlexc |
0:774d7a3e63ae
|
154
|
B.coast();
|
littlexc |
0:774d7a3e63ae
|
155
|
}
|
littlexc |
0:774d7a3e63ae
|
156
|
*/ |