Maestro Servo Controller library
 All Classes Files Functions Variables
PololuMaestro.cpp
1 // Copyright (C) Pololu Corporation. See LICENSE.txt for details.
2 
3 #include "PololuMaestro.h"
4 
5 Maestro::Maestro(Stream &stream,
6  uint8_t resetPin,
7  uint8_t deviceNumber,
8  bool CRCEnabled)
9 {
10  _stream = &stream;
11  _deviceNumber = deviceNumber;
12  _resetPin = resetPin;
13  _CRCEnabled = CRCEnabled;
14 }
15 
17 {
18  if (_resetPin != noResetPin)
19  {
20  digitalWrite(_resetPin, LOW);
21  pinMode(_resetPin, OUTPUT); // Drive low.
22  delay(1);
23  pinMode(_resetPin, INPUT); // Return to high-impedance input (reset is
24  // internally pulled up on Maestro).
25  delay(200); // Wait for Maestro to boot up after reset.
26  }
27 }
28 
29 void Maestro::setTargetMiniSSC(uint8_t channelNumber, uint8_t target)
30 {
31  _stream->write(miniSscCommand);
32  _stream->write(channelNumber);
33  _stream->write(target);
34 }
35 
37 {
38  writeCommand(goHomeCommand);
39  writeCRC();
40 }
41 
43 {
44  writeCommand(stopScriptCommand);
45  writeCRC();
46 }
47 
48 void Maestro::restartScript(uint8_t subroutineNumber)
49 {
50  writeCommand(restartScriptAtSubroutineCommand);
51  write7BitData(subroutineNumber);
52  writeCRC();
53 }
54 
55 void Maestro::restartScriptWithParameter(uint8_t subroutineNumber,
56  uint16_t parameter)
57 {
58  writeCommand(restartScriptAtSubroutineWithParameterCommand);
59  write7BitData(subroutineNumber);
60  write14BitData(parameter);
61  writeCRC();
62 }
63 
64 void Maestro::setTarget(uint8_t channelNumber, uint16_t target)
65 {
66  writeCommand(setTargetCommand);
67  write7BitData(channelNumber);
68  write14BitData(target);
69  writeCRC();
70 }
71 
72 void Maestro::setSpeed(uint8_t channelNumber, uint16_t speed)
73 {
74  writeCommand(setSpeedCommand);
75  write7BitData(channelNumber);
76  write14BitData(speed);
77  writeCRC();
78 }
79 
80 void Maestro::setAcceleration(uint8_t channelNumber, uint16_t acceleration)
81 {
82  writeCommand(setAccelerationCommand);
83  write7BitData(channelNumber);
84  write14BitData(acceleration);
85  writeCRC();
86 }
87 
88 uint16_t Maestro::getPosition(uint8_t channelNumber)
89 {
90  writeCommand(getPositionCommand);
91  write7BitData(channelNumber);
92  writeCRC();
93 
94  while (_stream->available() < 2);
95  uint8_t lowerByte = _stream->read();
96  uint8_t upperByte = _stream->read();
97  return (upperByte << 8) | (lowerByte & 0xFF);
98 }
99 
101 {
102  writeCommand(getMovingStateCommand);
103  writeCRC();
104 
105  while (_stream->available() < 1);
106  return _stream->read();
107 }
108 
110 {
111  writeCommand(getErrorsCommand);
112  writeCRC();
113 
114  while (_stream->available() < 2);
115  uint8_t lowerByte = _stream->read();
116  uint8_t upperByte = _stream->read();
117  return (upperByte << 8) | (lowerByte & 0xFF);
118 }
119 
121 {
122  writeCommand(getScriptStatusCommand);
123  writeCRC();
124 
125  while (_stream->available() < 1);
126  return _stream->read();
127 }
128 
129 void Maestro::writeByte(uint8_t dataByte)
130 {
131  _stream->write(dataByte);
132 
133  if(_CRCEnabled)
134  {
135  _CRCByte ^= dataByte;
136  for (uint8_t j = 0; j < 8; j++)
137  {
138  if (_CRCByte & 1)
139  {
140  _CRCByte ^= CRC7Polynomial;
141  }
142  _CRCByte >>= 1;
143  }
144  }
145 }
146 
147 void Maestro::writeCRC()
148 {
149  if(_CRCEnabled)
150  {
151  _stream->write(_CRCByte);
152  _CRCByte = 0; // Reset CRCByte to initial value.
153  }
154 }
155 
156 void Maestro::writeCommand(uint8_t commandByte)
157 {
158  if (_deviceNumber != deviceNumberDefault)
159  {
160  writeByte(baudRateIndication);
161  write7BitData(_deviceNumber);
162  write7BitData(commandByte);
163  }
164  else
165  {
166  writeByte(commandByte);
167  }
168 }
169 
170 void Maestro::write7BitData(uint8_t data)
171 {
172  writeByte(data & 0x7F);
173 }
174 
175 void Maestro::write14BitData(uint16_t data)
176 {
177  writeByte(data & 0x7F);
178  writeByte((data >> 7) & 0x7F);
179 }
180 
182  uint8_t resetPin,
183  uint8_t deviceNumber,
184  bool CRCEnabled) : Maestro(stream,
185  resetPin,
186  deviceNumber,
187  CRCEnabled)
188 {
189 }
190 
192  uint8_t resetPin,
193  uint8_t deviceNumber,
194  bool CRCEnabled) : Maestro(stream,
195  resetPin,
196  deviceNumber,
197  CRCEnabled)
198 {
199 }
200 
201 void MiniMaestro::setPWM(uint16_t onTime, uint16_t period)
202 {
203  writeCommand(setPwmCommand);
204  write14BitData(onTime);
205  write14BitData(period);
206  writeCRC();
207 }
208 
209 void MiniMaestro::setMultiTarget(uint8_t numberOfTargets,
210  uint8_t firstChannel,
211  uint16_t *targetList)
212 {
213  writeCommand(setMultipleTargetsCommand);
214  write7BitData(numberOfTargets);
215  write7BitData(firstChannel);
216 
217  for (int i = 0; i < numberOfTargets; i++)
218  {
219  write14BitData(targetList[i]);
220  }
221 
222  writeCRC();
223 }
uint16_t getErrors()
Gets the error register.
uint8_t getScriptStatus()
Gets if the script is running or stopped.
void setMultiTarget(uint8_t numberOfTargets, uint8_t firstChannel, uint16_t *targetList)
Sets multiple targets starting with the channel specified by firstChannel to a list of values listed ...
void stopScript()
Stops the script.
void goHome()
Sends the servos and outputs to home position.
MiniMaestro(Stream &stream, uint8_t resetPin=noResetPin, uint8_t deviceNumber=deviceNumberDefault, bool CRCEnabled=false)
Create a MiniMaestro object.
static const uint8_t deviceNumberDefault
The default device number, used to construct a MicroMaestro or MiniMaestro object that will use the c...
Definition: PololuMaestro.h:32
void reset()
Resets the Maestro by toggling the resetPin, if a resetPin was given.
static const uint8_t noResetPin
The default reset pin is no reset pin, used to construct a MicroMaestro or MiniMaestro object that wi...
Definition: PololuMaestro.h:37
void restartScript(uint8_t subroutineNumber)
Starts loaded script at specified subroutineNumber location.
MicroMaestro(Stream &stream, uint8_t resetPin=noResetPin, uint8_t deviceNumber=deviceNumberDefault, bool CRCEnabled=false)
Create a MicroMaestro object.
void setAcceleration(uint8_t channelNumber, uint16_t acceleration)
Sets the acceleration limit of channelNumber.
uint8_t getMovingState()
Gets the moving state for all configured servo channels.
void restartScriptWithParameter(uint8_t subroutineNumber, uint16_t parameter)
Starts loaded script at specified subroutineNumber location after loading parameter on to the stack...
void setSpeed(uint8_t channelNumber, uint16_t speed)
Sets the speed limit of channelNumber.
uint16_t getPosition(uint8_t channelNumber)
Gets the position of channelNumber.
Main Maestro class that handles common functions between the Micro Maestro and Mini Maestro...
Definition: PololuMaestro.h:26
void setTarget(uint8_t channelNumber, uint16_t target)
Sets the target of the servo on channelNumber.
void setTargetMiniSSC(uint8_t channelNumber, uint8_t target)
Sets the target of the servo on channelNumber using the Mini SSC protocol.
void setPWM(uint16_t onTime, uint16_t period)
Sets the PWM specified by onTime and period in units of 1/48 microseconds.