37 lines
724 B
C++
37 lines
724 B
C++
#include <map>
|
|
#include "Servo.h"
|
|
|
|
std::map<uint8_t, Servo> servos;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
}
|
|
|
|
void setupServos(uint8_t *calibrationValues, uint8_t size)
|
|
{
|
|
// We assume there are 3 bytes per servo. Ignore if there aren't.
|
|
if (size % 3 == 0)
|
|
{
|
|
for (int i = 0; i < size; i += 3)
|
|
{
|
|
servos.insert(std::make_pair(calibrationValues[i], new Servo()))
|
|
}
|
|
}
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
uint8_t *header = new uint8_t[2];
|
|
Serial.readBytes(header, 2);
|
|
|
|
if (header[0] == 0)
|
|
{
|
|
uint8_t *calibration = new uint8_t[3 * header[1]];
|
|
Serial.readBytes(calibration, header[1]);
|
|
setupServos();
|
|
delete calibration;
|
|
}
|
|
|
|
delete header;
|
|
} |