You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
224 lines
4.9 KiB
C++
224 lines
4.9 KiB
C++
// Copyright (C) Pololu Corporation. See LICENSE.txt for details.
|
|
|
|
#include "PololuMaestro.h"
|
|
|
|
Maestro::Maestro(Stream &stream,
|
|
uint8_t resetPin,
|
|
uint8_t deviceNumber,
|
|
bool CRCEnabled)
|
|
{
|
|
_stream = &stream;
|
|
_deviceNumber = deviceNumber;
|
|
_resetPin = resetPin;
|
|
_CRCEnabled = CRCEnabled;
|
|
}
|
|
|
|
void Maestro::reset()
|
|
{
|
|
if (_resetPin != noResetPin)
|
|
{
|
|
digitalWrite(_resetPin, LOW);
|
|
pinMode(_resetPin, OUTPUT); // Drive low.
|
|
delay(1);
|
|
pinMode(_resetPin, INPUT); // Return to high-impedance input (reset is
|
|
// internally pulled up on Maestro).
|
|
delay(200); // Wait for Maestro to boot up after reset.
|
|
}
|
|
}
|
|
|
|
void Maestro::setTargetMiniSSC(uint8_t channelNumber, uint8_t target)
|
|
{
|
|
_stream->write(miniSscCommand);
|
|
_stream->write(channelNumber);
|
|
_stream->write(target);
|
|
}
|
|
|
|
void Maestro::goHome()
|
|
{
|
|
writeCommand(goHomeCommand);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::stopScript()
|
|
{
|
|
writeCommand(stopScriptCommand);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::restartScript(uint8_t subroutineNumber)
|
|
{
|
|
writeCommand(restartScriptAtSubroutineCommand);
|
|
write7BitData(subroutineNumber);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::restartScriptWithParameter(uint8_t subroutineNumber,
|
|
uint16_t parameter)
|
|
{
|
|
writeCommand(restartScriptAtSubroutineWithParameterCommand);
|
|
write7BitData(subroutineNumber);
|
|
write14BitData(parameter);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::setTarget(uint8_t channelNumber, uint16_t target)
|
|
{
|
|
writeCommand(setTargetCommand);
|
|
write7BitData(channelNumber);
|
|
write14BitData(target);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::setSpeed(uint8_t channelNumber, uint16_t speed)
|
|
{
|
|
writeCommand(setSpeedCommand);
|
|
write7BitData(channelNumber);
|
|
write14BitData(speed);
|
|
writeCRC();
|
|
}
|
|
|
|
void Maestro::setAcceleration(uint8_t channelNumber, uint16_t acceleration)
|
|
{
|
|
writeCommand(setAccelerationCommand);
|
|
write7BitData(channelNumber);
|
|
write14BitData(acceleration);
|
|
writeCRC();
|
|
}
|
|
|
|
uint16_t Maestro::getPosition(uint8_t channelNumber)
|
|
{
|
|
writeCommand(getPositionCommand);
|
|
write7BitData(channelNumber);
|
|
writeCRC();
|
|
|
|
while (_stream->available() < 2);
|
|
uint8_t lowerByte = _stream->read();
|
|
uint8_t upperByte = _stream->read();
|
|
return (upperByte << 8) | (lowerByte & 0xFF);
|
|
}
|
|
|
|
uint8_t Maestro::getMovingState()
|
|
{
|
|
writeCommand(getMovingStateCommand);
|
|
writeCRC();
|
|
|
|
while (_stream->available() < 1);
|
|
return _stream->read();
|
|
}
|
|
|
|
uint16_t Maestro::getErrors()
|
|
{
|
|
writeCommand(getErrorsCommand);
|
|
writeCRC();
|
|
|
|
while (_stream->available() < 2);
|
|
uint8_t lowerByte = _stream->read();
|
|
uint8_t upperByte = _stream->read();
|
|
return (upperByte << 8) | (lowerByte & 0xFF);
|
|
}
|
|
|
|
uint8_t Maestro::getScriptStatus()
|
|
{
|
|
writeCommand(getScriptStatusCommand);
|
|
writeCRC();
|
|
|
|
while (_stream->available() < 1);
|
|
return _stream->read();
|
|
}
|
|
|
|
void Maestro::writeByte(uint8_t dataByte)
|
|
{
|
|
_stream->write(dataByte);
|
|
|
|
if(_CRCEnabled)
|
|
{
|
|
_CRCByte ^= dataByte;
|
|
for (uint8_t j = 0; j < 8; j++)
|
|
{
|
|
if (_CRCByte & 1)
|
|
{
|
|
_CRCByte ^= CRC7Polynomial;
|
|
}
|
|
_CRCByte >>= 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Maestro::writeCRC()
|
|
{
|
|
if(_CRCEnabled)
|
|
{
|
|
_stream->write(_CRCByte);
|
|
_CRCByte = 0; // Reset CRCByte to initial value.
|
|
}
|
|
}
|
|
|
|
void Maestro::writeCommand(uint8_t commandByte)
|
|
{
|
|
if (_deviceNumber != deviceNumberDefault)
|
|
{
|
|
writeByte(baudRateIndication);
|
|
write7BitData(_deviceNumber);
|
|
write7BitData(commandByte);
|
|
}
|
|
else
|
|
{
|
|
writeByte(commandByte);
|
|
}
|
|
}
|
|
|
|
void Maestro::write7BitData(uint8_t data)
|
|
{
|
|
writeByte(data & 0x7F);
|
|
}
|
|
|
|
void Maestro::write14BitData(uint16_t data)
|
|
{
|
|
writeByte(data & 0x7F);
|
|
writeByte((data >> 7) & 0x7F);
|
|
}
|
|
|
|
MicroMaestro::MicroMaestro(Stream &stream,
|
|
uint8_t resetPin,
|
|
uint8_t deviceNumber,
|
|
bool CRCEnabled) : Maestro(stream,
|
|
resetPin,
|
|
deviceNumber,
|
|
CRCEnabled)
|
|
{
|
|
}
|
|
|
|
MiniMaestro::MiniMaestro(Stream &stream,
|
|
uint8_t resetPin,
|
|
uint8_t deviceNumber,
|
|
bool CRCEnabled) : Maestro(stream,
|
|
resetPin,
|
|
deviceNumber,
|
|
CRCEnabled)
|
|
{
|
|
}
|
|
|
|
void MiniMaestro::setPWM(uint16_t onTime, uint16_t period)
|
|
{
|
|
writeCommand(setPwmCommand);
|
|
write14BitData(onTime);
|
|
write14BitData(period);
|
|
writeCRC();
|
|
}
|
|
|
|
void MiniMaestro::setMultiTarget(uint8_t numberOfTargets,
|
|
uint8_t firstChannel,
|
|
uint16_t *targetList)
|
|
{
|
|
writeCommand(setMultipleTargetsCommand);
|
|
write7BitData(numberOfTargets);
|
|
write7BitData(firstChannel);
|
|
|
|
for (int i = 0; i < numberOfTargets; i++)
|
|
{
|
|
write14BitData(targetList[i]);
|
|
}
|
|
|
|
writeCRC();
|
|
}
|