1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Jon Trulson <jtrulson@ics.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2015 Intel Corporation. 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <unistd.h> 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <math.h> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <iostream> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <string> 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "adafruitms1438.h" 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace std; 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangAdafruitMS1438::AdafruitMS1438(int bus, uint8_t address) : 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685(new PCA9685(bus, address)) 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setupPinMaps(); 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // set a default period of 50Hz 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setPWMPeriod(50); 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // disable all PWM's (4 of them). They are shared with each other 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // (stepper/DC), so just disable the DC motors here 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang disableMotor(MOTOR_M1); 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang disableMotor(MOTOR_M2); 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang disableMotor(MOTOR_M3); 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang disableMotor(MOTOR_M4); 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // Set all 'on time' registers to 0 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledOnTime(PCA9685_ALL_LED, 0); 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // set the default stepper config at 200 steps per rev 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang stepConfig(STEPMOTOR_M12, 200); 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang stepConfig(STEPMOTOR_M34, 200); 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangAdafruitMS1438::~AdafruitMS1438() 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang delete m_pca9685; 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::initClock(STEPMOTORS_T motor) 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&m_stepConfig[motor].startTime, NULL); 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint32_t AdafruitMS1438::getMillis(STEPMOTORS_T motor) 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang struct timeval elapsed, now; 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint32_t elapse; 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // get current time 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&now, NULL); 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang struct timeval startTime = m_stepConfig[motor].startTime; 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // compute the delta since m_startTime 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if( (elapsed.tv_usec = now.tv_usec - startTime.tv_usec) < 0 ) 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_usec += 1000000; 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - startTime.tv_sec - 1; 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - startTime.tv_sec; 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000)); 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // never return 0 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (elapse == 0) 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = 1; 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return elapse; 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang// setup the pin mappings of the pca9685 outputs to the proper motor controls 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setupPinMaps() 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // first the dc motors 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_dcMotors[0] = (DC_PINMAP_T){ 8, 10, 9 }; 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_dcMotors[1] = (DC_PINMAP_T){ 13, 11, 12 }; 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_dcMotors[2] = (DC_PINMAP_T){ 2, 4, 3 }; 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_dcMotors[3] = (DC_PINMAP_T){ 7, 5, 6 }; 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // now the 2 steppers 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepMotors[0] = (STEPPER_PINMAP_T){ 8, 10, 9, 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 13, 11, 12 }; 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepMotors[1] = (STEPPER_PINMAP_T){ 2, 4, 3, 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 7, 5, 6 }; 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setPWMPeriod(float hz) 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // must be in sleep mode to set the prescale register 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->setModeSleep(true); 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->setPrescaleFromHz(hz); 120765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->setModeSleep(false); 121765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 122765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 123765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::enableMotor(DCMOTORS_T motor) 124765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 125765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].pwm, false); 126765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 127765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 128765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::disableMotor(DCMOTORS_T motor) 129765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 130765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].pwm, true); 131765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 132765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 133765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::enableStepper(STEPMOTORS_T motor) 134765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 135765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, false); 136765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, false); 137765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 138765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 139765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::disableStepper(STEPMOTORS_T motor) 140765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 141765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, true); 142765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, true); 143765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 144765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 145765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setMotorSpeed(DCMOTORS_T motor, int speed) 146765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 147765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed < 0) 148765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang speed = 0; 149765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 150765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed > 100) 151765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang speed = 100; 152765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 153765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang float percent = float(speed) / 100.0; 154765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 155765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // make sure that the FullOn bit is turned off, or the speed setting 156765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // (PWM duty cycle) won't have any effect. 157765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_dcMotors[motor].pwm, false); 158765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 159765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // set the PWM duty cycle 160765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledOffTime(m_dcMotors[motor].pwm, int(4095.0 * percent)); 161765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 162765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 163765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setStepperSpeed(STEPMOTORS_T motor, int speed) 164765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 165765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDelay = 60 * 1000 / 166765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepsPerRev / speed; 167765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 168765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 169765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setMotorDirection(DCMOTORS_T motor, DIRECTION_T dir) 170765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 171765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (dir & 0x01) 172765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 173765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_dcMotors[motor].in1, true); 174765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].in1, false); 175765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 176765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 177765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 178765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].in1, true); 179765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_dcMotors[motor].in1, false); 180765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 181765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 182765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (dir & 0x02) 183765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 184765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_dcMotors[motor].in2, true); 185765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].in2, false); 186765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 187765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 188765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 189765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_dcMotors[motor].in2, true); 190765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_dcMotors[motor].in2, false); 191765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 192765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 193765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 194765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::setStepperDirection(STEPMOTORS_T motor, DIRECTION_T dir) 195765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 196765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (dir) 197765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 198765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case DIR_CW: 199765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDirection = 1; 200765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 201765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case DIR_CCW: 202765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDirection = -1; 203765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 204765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang default: // default to 1 if DIR_NONE specified 205765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDirection = 1; 206765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 207765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 208765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 209765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 210765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::stepConfig(STEPMOTORS_T motor, unsigned int stepsPerRev) 211765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 212765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepsPerRev = stepsPerRev; 213765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].currentStep = 0; 214765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDelay = 0; 215765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepDirection = 1; // forward 216765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 217765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // now, setup the control pins - we want both FULL ON and FULL OFF. 218765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // Since FULL OFF has precedence, we can then control the steps by 219765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // just turning on/off the FULL OFF bit for the relevant outputs 220765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 221765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmA, true); 222765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].pwmA, true); 223765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 224765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].pwmB, true); 225765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].pwmB, true); 226765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 227765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true); 228765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].in1A, true); 229765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 230765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true); 231765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].in2A, true); 232765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 233765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true); 234765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].in1B, true); 235765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 236765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true); 237765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOn(m_stepMotors[motor].in2B, true); 238765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 239765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 240765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::stepperStep(STEPMOTORS_T motor) 241765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 242765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int step = m_stepConfig[motor].currentStep % 4; 243765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 244765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // Step I0 I1 I2 I3 245765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 1 1 0 1 0 246765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 2 0 1 1 0 247765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 3 0 1 0 1 248765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 4 1 0 0 1 249765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 250765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // we invert the logic since we are essentially toggling an OFF bit, 251765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // not an ON bit. 252765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (step) 253765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 254765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 0: // 1010 255765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false); 256765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true); 257765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false); 258765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true); 259765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 260765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 1: // 0110 261765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true); 262765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false); 263765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1B, false); 264765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2B, true); 265765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 266765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 2: //0101 267765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1A, true); 268765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2A, false); 269765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true); 270765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false); 271765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 272765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 3: //1001 273765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1A, false); 274765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2A, true); 275765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in1B, true); 276765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pca9685->ledFullOff(m_stepMotors[motor].in2B, false); 277765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 278765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 279765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 280765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 281765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid AdafruitMS1438::stepperSteps(STEPMOTORS_T motor, unsigned int steps) 282765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 283765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang while (steps > 0) 284765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 285765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (getMillis(motor) >= m_stepConfig[motor].stepDelay) 286765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 287765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // reset the clock 288765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang initClock(motor); 289765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 290765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].currentStep += m_stepConfig[motor].stepDirection; 291765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 292765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepConfig[motor].stepDirection == 1) 293765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 294765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepConfig[motor].currentStep >= 295765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepsPerRev) 296765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].currentStep = 0; 297765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 298765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 299765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 300765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepConfig[motor].currentStep <= 0) 301765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].currentStep = 302765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepConfig[motor].stepsPerRev; 303765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 304765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 305765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang steps--; 306765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang stepperStep(motor); 307765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 308765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 309765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 310765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 311