vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial PlatformIO code.

Browse Source
This commit is contained in:
Piv
2020-09-08 21:23:21 +09:30
parent 756e2741be
commit b72781e322
2 changed files with 26 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
esp32/src/main.cpp
View File

@@ -3,7 +3,14 @@
#include <map>
#include "Servo.h"
std::map<uint8_t, Servo> servos;
// Min/max widths, used to calculate min/max duty cycles.
#define MIN_PULSE_WIDTH 1000
#define MAX_PULSE_WIDTH 2000
#define MIN_ANGLE 0
#define MAX_ANGLE 255
std::map<uint8_t, Servo *> servos;
void setup()
{
@@ -15,13 +22,22 @@ void setupServos(uint8_t size, uint8_t *calibrationValues)
// 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)
for (int i = 0; i < size; i += 2)
{
servos.insert(std::make_pair(calibrationValues[i], new Servo()));
Servo *newServo = new Servo();
newServo->attach(calibrationValues[i + 1], calibrationValues[i], MIN_ANGLE, MAX_ANGLE, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
servos.insert(std::make_pair(calibrationValues[i], newServo));
}
}
}
void modifyServo(uint8_t channel, uint8_t newAngle)
{
if(servos.count(channel) > 0){
servos[channel]->write(newAngle);
}
}
void loop()
{
uint8_t *header = new uint8_t[2];
@@ -29,11 +45,15 @@ void loop()
if (header[0] == 0)
{
uint8_t *calibration = new uint8_t[3 * header[1]];
uint8_t *calibration = new uint8_t[2 * header[1]];
Serial.readBytes(calibration, header[1]);
setupServos(header[1], calibration);
delete calibration;
delete [] calibration;
}
else
{
modifyServo(header[0], header[1]);
}
delete header;
delete [] header;
}