1 /*
2 * Author: Jon Trulson <jtrulson@ics.com>
3 * Copyright (c) 2014 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 <iostream>
27 #include <string>
28 #include <stdexcept>
29
30 #include "grovemd.h"
31
32 using namespace upm;
33 using namespace std;
34
35
GroveMD(int bus,uint8_t address)36 GroveMD::GroveMD(int bus, uint8_t address) :
37 m_i2c(bus)
38 {
39 m_addr = address;
40
41 // this board *requires* 100Khz i2c bus only
42 mraa::Result rv;
43 if ( (rv = m_i2c.frequency(mraa::I2C_STD)) != mraa::SUCCESS )
44 {
45 throw std::invalid_argument(std::string(__FUNCTION__) +
46 ": I2c.frequency(I2C_STD) failed");
47 return;
48 }
49
50 if (m_i2c.address(m_addr))
51 {
52 throw std::runtime_error(std::string(__FUNCTION__) +
53 ": I2c.address() failed");
54 return;
55 }
56
57 initClock();
58 // default to mode1 stepper operation, 200 steps per rev.
59 configStepper(200, STEP_MODE1);
60 }
61
~GroveMD()62 GroveMD::~GroveMD()
63 {
64 setMotorSpeeds(0, 0);
65 writePacket(SET_DIRECTION, 0, GROVEMD_NOOP);
66 }
67
writePacket(REG_T reg,uint8_t data1,uint8_t data2)68 bool GroveMD::writePacket(REG_T reg, uint8_t data1, uint8_t data2)
69 {
70 uint8_t buf[3];
71
72 buf[0] = reg;
73 buf[1] = data1;
74 buf[2] = data2;
75
76 if ( m_i2c.write(buf, 3) != mraa::SUCCESS )
77 {
78 throw std::runtime_error(std::string(__FUNCTION__) +
79 ": I2c.write() failed");
80 return false;
81 }
82
83 // This sleep appears to be required. Without it, writes randomly
84 // fail (no ACK received). This happens most often on the SET_SPEED
85 // packet. I am guessing that there is a timing problem and/or bug
86 // in the motor driver's firmware.
87
88 usleep(100);
89
90 return true;
91 }
92
setMotorSpeeds(uint8_t speedA,uint8_t speedB)93 bool GroveMD::setMotorSpeeds(uint8_t speedA, uint8_t speedB)
94 {
95 return writePacket(SET_SPEED, speedA, speedB);
96 }
97
setPWMFrequencyPrescale(uint8_t freq)98 bool GroveMD::setPWMFrequencyPrescale(uint8_t freq)
99 {
100 return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP);
101 }
102
setMotorDirections(DC_DIRECTION_T dirA,DC_DIRECTION_T dirB)103 bool GroveMD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB)
104 {
105 uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
106 return writePacket(SET_DIRECTION, dir, GROVEMD_NOOP);
107 }
108
enableStepper(STEP_DIRECTION_T dir,uint8_t speed)109 bool GroveMD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed)
110 {
111 // If mode 2, send the command and return immediately
112 if (m_stepMode == STEP_MODE2)
113 return writePacket(STEPPER_ENABLE, dir, speed);
114
115 // otherwise, mode 1, setup the basics and start stepping.
116
117 m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
118 m_stepDirection = ((dir == STEP_DIR_CW) ? 1 : -1);
119
120 // seeed says speed should always be 255,255 for stepper operation
121 setMotorSpeeds(255, 255);
122
123 while (m_totalSteps > 0)
124 {
125 if (getMillis() >= m_stepDelay)
126 {
127 // reset the clock
128 initClock();
129
130 m_currentStep += m_stepDirection;
131
132 if (m_stepDirection == 1)
133 {
134 if (m_currentStep >= m_stepsPerRev)
135 m_currentStep = 0;
136 }
137 else
138 {
139 if (m_currentStep <= 0)
140 m_currentStep = m_stepsPerRev;
141 }
142
143 m_totalSteps--;
144 stepperStep();
145 }
146 }
147
148 // and... we're done
149 return true;
150 }
151
disableStepper()152 bool GroveMD::disableStepper()
153 {
154 if (m_stepMode == STEP_MODE2)
155 return writePacket(STEPPER_DISABLE, GROVEMD_NOOP, GROVEMD_NOOP);
156
157 // else, mode 1
158 writePacket(SET_DIRECTION, 0, GROVEMD_NOOP);
159 return setMotorSpeeds(0, 0);
160 }
161
setStepperSteps(unsigned int steps)162 bool GroveMD::setStepperSteps(unsigned int steps)
163 {
164 if (m_stepMode == STEP_MODE2)
165 {
166 if (steps == 0)
167 {
168 // invalid
169 throw std::out_of_range(std::string(__FUNCTION__) +
170 ": invalid number of steps. " +
171 "Valid values are between 1 and 255.");
172 return false;
173 }
174 return writePacket(STEPPER_NUM_STEPS, steps, GROVEMD_NOOP);
175 }
176
177 // for mode one, just store it for future use by enableStepper()
178 m_totalSteps = steps;
179 return true;
180 }
181
initClock()182 void GroveMD::initClock()
183 {
184 gettimeofday(&m_startTime, NULL);
185 }
186
getMillis()187 uint32_t GroveMD::getMillis()
188 {
189 struct timeval elapsed, now;
190 uint32_t elapse;
191
192 // get current time
193 gettimeofday(&now, NULL);
194
195 // compute the delta since m_startTime
196 if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
197 {
198 elapsed.tv_usec += 1000000;
199 elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
200 }
201 else
202 {
203 elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
204 }
205
206 elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
207
208 // never return 0
209 if (elapse == 0)
210 elapse = 1;
211
212 return elapse;
213 }
214
configStepper(unsigned int stepsPerRev,STEP_MODE_T mode)215 void GroveMD::configStepper(unsigned int stepsPerRev, STEP_MODE_T mode)
216 {
217 m_stepsPerRev = stepsPerRev;
218 m_stepMode = mode;
219 m_currentStep = 0;
220 m_stepDelay = 0;
221 m_stepDirection = 1;
222 m_totalSteps = 0;
223 }
224
stepperStep()225 void GroveMD::stepperStep()
226 {
227 int step = m_currentStep % 4;
228
229 switch (step)
230 {
231 case 0:
232 writePacket(SET_DIRECTION, 0b0101, GROVEMD_NOOP);
233 break;
234 case 1:
235 writePacket(SET_DIRECTION, 0b0110, GROVEMD_NOOP);
236 break;
237 case 2:
238 writePacket(SET_DIRECTION, 0b1010, GROVEMD_NOOP);
239 break;
240 case 3:
241 writePacket(SET_DIRECTION, 0b1001, GROVEMD_NOOP);
242 break;
243 }
244 }
245