Raspberries/upm
Public Access
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

md: C implementation; C++ wraps C

Browse Source 
Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson
2016-10-18 17:02:33 -06:00
parent df5b3805c5
commit 8ac8be9e0a
16 changed files with 813 additions and 397 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
src/md/md.cxx
View File

@@ -1,6 +1,6 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2014 Intel Corporation.
* Copyright (c) 2014-2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
@@ -34,211 +34,71 @@ using namespace std;
MD::MD(int bus, uint8_t address) :
m_i2c(bus)
m_md(md_init(bus, address))
{
m_addr = address;
// this board *requires* 100Khz i2c bus only
mraa::Result rv;
if ( (rv = m_i2c.frequency(mraa::I2C_STD)) != mraa::SUCCESS )
{
throw std::invalid_argument(std::string(__FUNCTION__) +
": I2c.frequency(I2C_STD) failed");
return;
}
if (m_i2c.address(m_addr))
{
throw std::runtime_error(std::string(__FUNCTION__) +
": I2c.address() failed");
return;
}
initClock();
// default to mode1 stepper operation, 200 steps per rev.
configStepper(200, STEP_MODE1);
if (!m_md)
throw std::runtime_error(std::string(__FUNCTION__) +
": md_init() failed");
}
MD::~MD()
{
setMotorSpeeds(0, 0);
writePacket(SET_DIRECTION, 0, MD_NOOP);
md_close(m_md);
}
bool MD::writePacket(REG_T reg, uint8_t data1, uint8_t data2)
bool MD::writePacket(MD_REG_T reg, uint8_t data1, uint8_t data2)
{
uint8_t buf[3];
if (!md_write_packet(m_md, reg, data1, data2))
throw std::runtime_error(std::string(__FUNCTION__) +
": md_write_packet() failed");
buf[0] = reg;
buf[1] = data1;
buf[2] = data2;
if ( m_i2c.write(buf, 3) != mraa::SUCCESS )
{
throw std::runtime_error(std::string(__FUNCTION__) +
": I2c.write() failed");
return false;
}
// This sleep appears to be required. Without it, writes randomly
// fail (no ACK received). This happens most often on the SET_SPEED
// packet. I am guessing that there is a timing problem and/or bug
// in the motor driver's firmware.
usleep(100);
return true;
return true;
}
bool MD::setMotorSpeeds(uint8_t speedA, uint8_t speedB)
{
return writePacket(SET_SPEED, speedA, speedB);
if (!md_set_motor_speeds(m_md, speedA, speedB))
throw std::runtime_error(std::string(__FUNCTION__) +
": md_set_motor_speeds() failed");
return true;
}
bool MD::setPWMFrequencyPrescale(uint8_t freq)
{
return writePacket(SET_PWM_FREQ, freq, MD_NOOP);
if (!md_set_pwm_frequency_prescale(m_md, freq))
throw std::runtime_error(std::string(__FUNCTION__) +
": md_set_pwm_frequency_prescale() failed");
return true;
}
bool MD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB)
bool MD::setMotorDirections(MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB)
{
uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
return writePacket(SET_DIRECTION, dir, MD_NOOP);
if (!md_set_motor_directions(m_md, dirA, dirB))
throw std::runtime_error(std::string(__FUNCTION__) +
": md_set_motor_directions() failed");
return true;
}
bool MD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed)
bool MD::enableStepper(MD_STEP_DIRECTION_T dir, uint8_t speed)
{
// If mode 2, send the command and return immediately
if (m_stepMode == STEP_MODE2)
return writePacket(STEPPER_ENABLE, dir, speed);
// otherwise, mode 1, setup the basics and start stepping.
m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
m_stepDirection = ((dir == STEP_DIR_CW) ? 1 : -1);
// seeed says speed should always be 255,255 for stepper operation
setMotorSpeeds(255, 255);
while (m_totalSteps > 0)
{
if (getMillis() >= m_stepDelay)
{
// reset the clock
initClock();
m_currentStep += m_stepDirection;
if (m_stepDirection == 1)
{
if (m_currentStep >= m_stepsPerRev)
m_currentStep = 0;
}
else
{
if (m_currentStep <= 0)
m_currentStep = m_stepsPerRev;
}
m_totalSteps--;
stepperStep();
}
}
// and... we're done
return true;
return md_enable_stepper(m_md, dir, speed);
}
bool MD::disableStepper()
{
if (m_stepMode == STEP_MODE2)
return writePacket(STEPPER_DISABLE, MD_NOOP, MD_NOOP);
// else, mode 1
writePacket(SET_DIRECTION, 0, MD_NOOP);
return setMotorSpeeds(0, 0);
return md_disable_stepper(m_md);
}
bool MD::setStepperSteps(unsigned int steps)
{
if (m_stepMode == STEP_MODE2)
{
if (steps == 0)
{
// invalid
throw std::out_of_range(std::string(__FUNCTION__) +
": invalid number of steps. " +
"Valid values are between 1 and 255.");
return false;
}
return writePacket(STEPPER_NUM_STEPS, steps, MD_NOOP);
}
// for mode one, just store it for future use by enableStepper()
m_totalSteps = steps;
return true;
return md_set_stepper_steps(m_md, steps);
}
void MD::initClock()
void MD::configStepper(unsigned int stepsPerRev, MD_STEP_MODE_T mode)
{
gettimeofday(&m_startTime, NULL);
md_config_stepper(m_md, stepsPerRev, mode);
}
uint32_t MD::getMillis()
{
struct timeval elapsed, now;
uint32_t elapse;
// get current time
gettimeofday(&now, NULL);
// compute the delta since m_startTime
if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
{
elapsed.tv_usec += 1000000;
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
}
else
{
elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
}
elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
// never return 0
if (elapse == 0)
elapse = 1;
return elapse;
}
void MD::configStepper(unsigned int stepsPerRev, STEP_MODE_T mode)
{
m_stepsPerRev = stepsPerRev;
m_stepMode = mode;
m_currentStep = 0;
m_stepDelay = 0;
m_stepDirection = 1;
m_totalSteps = 0;
}
void MD::stepperStep()
{
int step = m_currentStep % 4;
switch (step)
{
case 0:
writePacket(SET_DIRECTION, 0b0101, MD_NOOP);
break;
case 1:
writePacket(SET_DIRECTION, 0b0110, MD_NOOP);
break;
case 2:
writePacket(SET_DIRECTION, 0b1010, MD_NOOP);
break;
case 3:
writePacket(SET_DIRECTION, 0b1001, MD_NOOP);
break;
}
}