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
AdafruitMS1438(int bus,uint8_t address)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
~AdafruitMS1438()59 AdafruitMS1438::~AdafruitMS1438()
60 {
61 delete m_pca9685;
62 }
63
initClock(STEPMOTORS_T motor)64 void AdafruitMS1438::initClock(STEPMOTORS_T motor)
65 {
66 gettimeofday(&m_stepConfig[motor].startTime, NULL);
67 }
68
getMillis(STEPMOTORS_T motor)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
setupPinMaps()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
setPWMPeriod(float hz)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
enableMotor(DCMOTORS_T motor)123 void AdafruitMS1438::enableMotor(DCMOTORS_T motor)
124 {
125 m_pca9685->ledFullOff(m_dcMotors[motor].pwm, false);
126 }
127
disableMotor(DCMOTORS_T motor)128 void AdafruitMS1438::disableMotor(DCMOTORS_T motor)
129 {
130 m_pca9685->ledFullOff(m_dcMotors[motor].pwm, true);
131 }
132
enableStepper(STEPMOTORS_T motor)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
disableStepper(STEPMOTORS_T motor)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
setMotorSpeed(DCMOTORS_T motor,int speed)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 // make sure that the FullOn bit is turned off, or the speed setting
156 // (PWM duty cycle) won't have any effect.
157 m_pca9685->ledFullOn(m_dcMotors[motor].pwm, false);
158
159 // set the PWM duty cycle
160 m_pca9685->ledOffTime(m_dcMotors[motor].pwm, int(4095.0 * percent));
161 }
162
setStepperSpeed(STEPMOTORS_T motor,int speed)163 void AdafruitMS1438::setStepperSpeed(STEPMOTORS_T motor, int speed)
164 {
165 m_stepConfig[motor].stepDelay = 60 * 1000 /
166 m_stepConfig[motor].stepsPerRev / speed;
167 }
168
setMotorDirection(DCMOTORS_T motor,DIRECTION_T dir)169 void AdafruitMS1438::setMotorDirection(DCMOTORS_T motor, DIRECTION_T dir)
170 {
171 if (dir & 0x01)
172 {
173 m_pca9685->ledFullOn(m_dcMotors[motor].in1, true);
174 m_pca9685->ledFullOff(m_dcMotors[motor].in1, false);
175 }
176 else
177 {
178 m_pca9685->ledFullOff(m_dcMotors[motor].in1, true);
179 m_pca9685->ledFullOn(m_dcMotors[motor].in1, false);
180 }
181
182 if (dir & 0x02)
183 {
184 m_pca9685->ledFullOn(m_dcMotors[motor].in2, true);
185 m_pca9685->ledFullOff(m_dcMotors[motor].in2, false);
186 }
187 else
188 {
189 m_pca9685->ledFullOff(m_dcMotors[motor].in2, true);
190 m_pca9685->ledFullOn(m_dcMotors[motor].in2, false);
191 }
192 }
193
setStepperDirection(STEPMOTORS_T motor,DIRECTION_T dir)194 void AdafruitMS1438::setStepperDirection(STEPMOTORS_T motor, DIRECTION_T dir)
195 {
196 switch (dir)
197 {
198 case DIR_CW:
199 m_stepConfig[motor].stepDirection = 1;
200 break;
201 case DIR_CCW:
202 m_stepConfig[motor].stepDirection = -1;
203 break;
204 default: // default to 1 if DIR_NONE specified
205 m_stepConfig[motor].stepDirection = 1;
206 break;
207 }
208 }
209
stepConfig(STEPMOTORS_T motor,unsigned int stepsPerRev)210 void AdafruitMS1438::stepConfig(STEPMOTORS_T motor, unsigned int stepsPerRev)
211 {
212 m_stepConfig[motor].stepsPerRev = stepsPerRev;
213 m_stepConfig[motor].currentStep = 0;
214 m_stepConfig[motor].stepDelay = 0;
215 m_stepConfig[motor].stepDirection = 1; // forward
216
217 // now, setup the control pins - we want both FULL ON and FULL OFF.
218 // Since FULL OFF has precedence, we can then control the steps by
219 // just turning on/off the FULL OFF bit for the relevant outputs
220
221 m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, true);
222 m_pca9685->ledFullOn(m_stepMotors[motor].pwmA, true);
223
224 m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, true);
225 m_pca9685->ledFullOn(m_stepMotors[motor].pwmB, true);
226
227 m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
228 m_pca9685->ledFullOn(m_stepMotors[motor].in1A, true);
229
230 m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
231 m_pca9685->ledFullOn(m_stepMotors[motor].in2A, true);
232
233 m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
234 m_pca9685->ledFullOn(m_stepMotors[motor].in1B, true);
235
236 m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
237 m_pca9685->ledFullOn(m_stepMotors[motor].in2B, true);
238 }
239
stepperStep(STEPMOTORS_T motor)240 void AdafruitMS1438::stepperStep(STEPMOTORS_T motor)
241 {
242 int step = m_stepConfig[motor].currentStep % 4;
243
244 // Step I0 I1 I2 I3
245 // 1 1 0 1 0
246 // 2 0 1 1 0
247 // 3 0 1 0 1
248 // 4 1 0 0 1
249
250 // we invert the logic since we are essentially toggling an OFF bit,
251 // not an ON bit.
252 switch (step)
253 {
254 case 0: // 1010
255 m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false);
256 m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
257 m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false);
258 m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
259 break;
260 case 1: // 0110
261 m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
262 m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false);
263 m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false);
264 m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true);
265 break;
266 case 2: //0101
267 m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true);
268 m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false);
269 m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
270 m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false);
271 break;
272 case 3: //1001
273 m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false);
274 m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true);
275 m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true);
276 m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false);
277 break;
278 }
279 }
280
stepperSteps(STEPMOTORS_T motor,unsigned int steps)281 void AdafruitMS1438::stepperSteps(STEPMOTORS_T motor, unsigned int steps)
282 {
283 while (steps > 0)
284 {
285 if (getMillis(motor) >= m_stepConfig[motor].stepDelay)
286 {
287 // reset the clock
288 initClock(motor);
289
290 m_stepConfig[motor].currentStep += m_stepConfig[motor].stepDirection;
291
292 if (m_stepConfig[motor].stepDirection == 1)
293 {
294 if (m_stepConfig[motor].currentStep >=
295 m_stepConfig[motor].stepsPerRev)
296 m_stepConfig[motor].currentStep = 0;
297 }
298 else
299 {
300 if (m_stepConfig[motor].currentStep <= 0)
301 m_stepConfig[motor].currentStep =
302 m_stepConfig[motor].stepsPerRev;
303 }
304
305 steps--;
306 stepperStep(motor);
307 }
308 }
309 }
310
311