|
@@ -1,305 +0,0 @@
|
1
|
|
-/*
|
2
|
|
- * Author: Jon Trulson <jtrulson@ics.com>
|
3
|
|
- * Copyright (c) 2015 Intel Corporation.
|
4
|
|
- *
|
5
|
|
- * Permission is hereby granted, free of charge, to any person obtaining
|
6
|
|
- * a copy of this software and associated documentation files (the
|
7
|
|
- * "Software"), to deal in the Software without restriction, including
|
8
|
|
- * without limitation the rights to use, copy, modify, merge, publish,
|
9
|
|
- * distribute, sublicense, and/or sell copies of the Software, and to
|
10
|
|
- * permit persons to whom the Software is furnished to do so, subject to
|
11
|
|
- * the following conditions:
|
12
|
|
- *
|
13
|
|
- * The above copyright notice and this permission notice shall be
|
14
|
|
- * included in all copies or substantial portions of the Software.
|
15
|
|
- *
|
16
|
|
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
17
|
|
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
18
|
|
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
19
|
|
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
|
20
|
|
- * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
21
|
|
- * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
|
22
|
|
- * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
23
|
|
- */
|
24
|
|
-
|
25
|
|
-#include <unistd.h>
|
26
|
|
-#include <math.h>
|
27
|
|
-#include <iostream>
|
28
|
|
-#include <string>
|
29
|
|
-
|
30
|
|
-#include "adafruitms1438.h"
|
31
|
|
-
|
32
|
|
-using namespace upm;
|
33
|
|
-using namespace std;
|
34
|
|
-
|
35
|
|
-
|
36
|
|
-AdafruitMS1438::AdafruitMS1438(int bus, uint8_t address) :
|
37
|
|
- m_pca9685(new PCA9685(bus, address))
|
38
|
|
-{
|
39
|
|
- setupPinMaps();
|
40
|
|
-
|
41
|
|
- // set a default period of 50Hz
|
42
|
|
- setPWMPeriod(50);
|
43
|
|
-
|
44
|
|
- // disable all PWM's (4 of them). They are shared with each other
|
45
|
|
- // (stepper/DC), so just disable the DC motors here
|
46
|
|
- disableMotor(MOTOR_M1);
|
47
|
|
- disableMotor(MOTOR_M2);
|
48
|
|
- disableMotor(MOTOR_M3);
|
49
|
|
- disableMotor(MOTOR_M4);
|
50
|
|
-
|
51
|
|
- // Set all 'on time' registers to 0
|
52
|
|
- m_pca9685->ledOnTime(PCA9685_ALL_LED, 0);
|
53
|
|
-
|
54
|
|
- // set the default stepper config at 200 steps per rev
|
55
|
|
- stepConfig(STEPMOTOR_M12, 200);
|
56
|
|
- stepConfig(STEPMOTOR_M34, 200);
|
57
|
|
-}
|
58
|
|
-
|
59
|
|
-AdafruitMS1438::~AdafruitMS1438()
|
60
|
|
-{
|
61
|
|
- delete m_pca9685;
|
62
|
|
-}
|
63
|
|
-
|
64
|
|
-void AdafruitMS1438::initClock(STEPMOTORS_T motor)
|
65
|
|
-{
|
66
|
|
- gettimeofday(&m_stepConfig[motor].startTime, NULL);
|
67
|
|
-}
|
68
|
|
-
|
69
|
|
-uint32_t AdafruitMS1438::getMillis(STEPMOTORS_T motor)
|
70
|
|
-{
|
71
|
|
- struct timeval elapsed, now;
|
72
|
|
- uint32_t elapse;
|
73
|
|
-
|
74
|
|
- // get current time
|
75
|
|
- gettimeofday(&now, NULL);
|
76
|
|
-
|
77
|
|
- struct timeval startTime = m_stepConfig[motor].startTime;
|
78
|
|
-
|
79
|
|
- // compute the delta since m_startTime
|
80
|
|
- if( (elapsed.tv_usec = now.tv_usec - startTime.tv_usec) < 0 )
|
81
|
|
- {
|
82
|
|
- elapsed.tv_usec += 1000000;
|
83
|
|
- elapsed.tv_sec = now.tv_sec - startTime.tv_sec - 1;
|
84
|
|
- }
|
85
|
|
- else
|
86
|
|
- {
|
87
|
|
- elapsed.tv_sec = now.tv_sec - startTime.tv_sec;
|
88
|
|
- }
|
89
|
|
-
|
90
|
|
- elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
|
91
|
|
-
|
92
|
|
- // never return 0
|
93
|
|
- if (elapse == 0)
|
94
|
|
- elapse = 1;
|
95
|
|
-
|
96
|
|
- return elapse;
|
97
|
|
-}
|
98
|
|
-
|
99
|
|
-// setup the pin mappings of the pca9685 outputs to the proper motor controls
|
100
|
|
-void AdafruitMS1438::setupPinMaps()
|
101
|
|
-{
|
102
|
|
- // first the dc motors
|
103
|
|
- m_dcMotors[0] = (DC_PINMAP_T){ 8, 10, 9 };
|
104
|
|
- m_dcMotors[1] = (DC_PINMAP_T){ 13, 11, 12 };
|
105
|
|
- m_dcMotors[2] = (DC_PINMAP_T){ 2, 4, 3 };
|
106
|
|
- m_dcMotors[3] = (DC_PINMAP_T){ 7, 5, 6 };
|
107
|
|
-
|
108
|
|
- // now the 2 steppers
|
109
|
|
- m_stepMotors[0] = (STEPPER_PINMAP_T){ 8, 10, 9,
|
110
|
|
- 13, 11, 12 };
|
111
|
|
- m_stepMotors[1] = (STEPPER_PINMAP_T){ 2, 4, 3,
|
112
|
|
- 7, 5, 6 };
|
113
|
|
-}
|
114
|
|
-
|
115
|
|
-void AdafruitMS1438::setPWMPeriod(float hz)
|
116
|
|
-{
|
117
|
|
- // must be in sleep mode to set the prescale register
|
118
|
|
- m_pca9685->setModeSleep(true);
|
119
|
|
- m_pca9685->setPrescaleFromHz(hz);
|
120
|
|
- m_pca9685->setModeSleep(false);
|
121
|
|
-}
|
122
|
|
-
|
123
|
|
-void AdafruitMS1438::enableMotor(DCMOTORS_T motor)
|
124
|
|
-{
|
125
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].pwm, false);
|
126
|
|
-}
|
127
|
|
-
|
128
|
|
-void AdafruitMS1438::disableMotor(DCMOTORS_T motor)
|
129
|
|
-{
|
130
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].pwm, true);
|
131
|
|
-}
|
132
|
|
-
|
133
|
|
-void AdafruitMS1438::enableStepper(STEPMOTORS_T motor)
|
134
|
|
-{
|
135
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, false);
|
136
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, false);
|
137
|
|
-}
|
138
|
|
-
|
139
|
|
-void AdafruitMS1438::disableStepper(STEPMOTORS_T motor)
|
140
|
|
-{
|
141
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, true);
|
142
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, true);
|
143
|
|
-}
|
144
|
|
-
|
145
|
|
-void AdafruitMS1438::setMotorSpeed(DCMOTORS_T motor, int speed)
|
146
|
|
-{
|
147
|
|
- if (speed < 0)
|
148
|
|
- speed = 0;
|
149
|
|
-
|
150
|
|
- if (speed > 100)
|
151
|
|
- speed = 100;
|
152
|
|
-
|
153
|
|
- float percent = float(speed) / 100.0;
|
154
|
|
-
|
155
|
|
- m_pca9685->ledOffTime(m_dcMotors[motor].pwm, int(4095.0 * percent));
|
156
|
|
-}
|
157
|
|
-
|
158
|
|
-void AdafruitMS1438::setStepperSpeed(STEPMOTORS_T motor, int speed)
|
159
|
|
-{
|
160
|
|
- m_stepConfig[motor].stepDelay = 60 * 1000 /
|
161
|
|
- m_stepConfig[motor].stepsPerRev / speed;
|
162
|
|
-}
|
163
|
|
-
|
164
|
|
-void AdafruitMS1438::setMotorDirection(DCMOTORS_T motor, DIRECTION_T dir)
|
165
|
|
-{
|
166
|
|
- if (dir & 0x01)
|
167
|
|
- {
|
168
|
|
- m_pca9685->ledFullOn(m_dcMotors[motor].in1, true);
|
169
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].in1, false);
|
170
|
|
- }
|
171
|
|
- else
|
172
|
|
- {
|
173
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].in1, true);
|
174
|
|
- m_pca9685->ledFullOn(m_dcMotors[motor].in1, false);
|
175
|
|
- }
|
176
|
|
-
|
177
|
|
- if (dir & 0x02)
|
178
|
|
- {
|
179
|
|
- m_pca9685->ledFullOn(m_dcMotors[motor].in2, true);
|
180
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].in2, false);
|
181
|
|
- }
|
182
|
|
- else
|
183
|
|
- {
|
184
|
|
- m_pca9685->ledFullOff(m_dcMotors[motor].in2, true);
|
185
|
|
- m_pca9685->ledFullOn(m_dcMotors[motor].in2, false);
|
186
|
|
- }
|
187
|
|
-}
|
188
|
|
-
|
189
|
|
-void AdafruitMS1438::setStepperDirection(STEPMOTORS_T motor, DIRECTION_T dir)
|
190
|
|
-{
|
191
|
|
- switch (dir)
|
192
|
|
- {
|
193
|
|
- case DIR_CW:
|
194
|
|
- m_stepConfig[motor].stepDirection = 1;
|
195
|
|
- break;
|
196
|
|
- case DIR_CCW:
|
197
|
|
- m_stepConfig[motor].stepDirection = -1;
|
198
|
|
- break;
|
199
|
|
- default: // default to 1 if DIR_NONE specified
|
200
|
|
- m_stepConfig[motor].stepDirection = 1;
|
201
|
|
- break;
|
202
|
|
- }
|
203
|
|
-}
|
204
|
|
-
|
205
|
|
-void AdafruitMS1438::stepConfig(STEPMOTORS_T motor, unsigned int stepsPerRev)
|
206
|
|
-{
|
207
|
|
- m_stepConfig[motor].stepsPerRev = stepsPerRev;
|
208
|
|
- m_stepConfig[motor].currentStep = 0;
|
209
|
|
- m_stepConfig[motor].stepDelay = 0;
|
210
|
|
- m_stepConfig[motor].stepDirection = 1; // forward
|
211
|
|
-
|
212
|
|
- // now, setup the control pins - we want both FULL ON and FULL OFF.
|
213
|
|
- // Since FULL OFF has precedence, we can then control the steps by
|
214
|
|
- // just turning on/off the FULL OFF bit for the relevant outputs
|
215
|
|
-
|
216
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, true);
|
217
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].pwmA, true);
|
218
|
|
-
|
219
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, true);
|
220
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].pwmB, true);
|
221
|
|
-
|
222
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
|
223
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].in1A, true);
|
224
|
|
-
|
225
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
|
226
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].in2A, true);
|
227
|
|
-
|
228
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
|
229
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].in1B, true);
|
230
|
|
-
|
231
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
|
232
|
|
- m_pca9685->ledFullOn(m_stepMotors[motor].in2B, true);
|
233
|
|
-}
|
234
|
|
-
|
235
|
|
-void AdafruitMS1438::stepperStep(STEPMOTORS_T motor)
|
236
|
|
-{
|
237
|
|
- int step = m_stepConfig[motor].currentStep % 4;
|
238
|
|
-
|
239
|
|
- // Step I0 I1 I2 I3
|
240
|
|
- // 1 1 0 1 0
|
241
|
|
- // 2 0 1 1 0
|
242
|
|
- // 3 0 1 0 1
|
243
|
|
- // 4 1 0 0 1
|
244
|
|
-
|
245
|
|
- // we invert the logic since we are essentially toggling an OFF bit,
|
246
|
|
- // not an ON bit.
|
247
|
|
- switch (step)
|
248
|
|
- {
|
249
|
|
- case 0: // 1010
|
250
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false);
|
251
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
|
252
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false);
|
253
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
|
254
|
|
- break;
|
255
|
|
- case 1: // 0110
|
256
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
|
257
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false);
|
258
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false);
|
259
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
|
260
|
|
- break;
|
261
|
|
- case 2: //0101
|
262
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
|
263
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false);
|
264
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
|
265
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false);
|
266
|
|
- break;
|
267
|
|
- case 3: //1001
|
268
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false);
|
269
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
|
270
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
|
271
|
|
- m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false);
|
272
|
|
- break;
|
273
|
|
- }
|
274
|
|
-}
|
275
|
|
-
|
276
|
|
-void AdafruitMS1438::stepperSteps(STEPMOTORS_T motor, unsigned int steps)
|
277
|
|
-{
|
278
|
|
- while (steps > 0)
|
279
|
|
- {
|
280
|
|
- if (getMillis(motor) >= m_stepConfig[motor].stepDelay)
|
281
|
|
- {
|
282
|
|
- // reset the clock
|
283
|
|
- initClock(motor);
|
284
|
|
-
|
285
|
|
- m_stepConfig[motor].currentStep += m_stepConfig[motor].stepDirection;
|
286
|
|
-
|
287
|
|
- if (m_stepConfig[motor].stepDirection == 1)
|
288
|
|
- {
|
289
|
|
- if (m_stepConfig[motor].currentStep >=
|
290
|
|
- m_stepConfig[motor].stepsPerRev)
|
291
|
|
- m_stepConfig[motor].currentStep = 0;
|
292
|
|
- }
|
293
|
|
- else
|
294
|
|
- {
|
295
|
|
- if (m_stepConfig[motor].currentStep <= 0)
|
296
|
|
- m_stepConfig[motor].currentStep =
|
297
|
|
- m_stepConfig[motor].stepsPerRev;
|
298
|
|
- }
|
299
|
|
-
|
300
|
|
- steps--;
|
301
|
|
- stepperStep(motor);
|
302
|
|
- }
|
303
|
|
- }
|
304
|
|
-}
|
305
|
|
-
|