1 /* 2 * Author: Jon Trulson <jtrulson (at) 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 // 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 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 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 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 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 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 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
