Skip to content

Commit

Permalink
use dynamic memory allocation for scenes and servos
Browse files Browse the repository at this point in the history
  • Loading branch information
timhendriks93 committed Feb 2, 2024
1 parent eba49d1 commit d546a48
Show file tree
Hide file tree
Showing 15 changed files with 173 additions and 87 deletions.
2 changes: 1 addition & 1 deletion examples/MultiplePCA9685/MultiplePCA9685.ino
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ struct servoMapping {
byte channel;
};

// Define an array of servo mapsf
// Define an array of servo maps
servoMapping servoMappings[] = {
// Servo 0 attached to board A on channel 0
{0, pwmA, 0},
Expand Down
6 changes: 4 additions & 2 deletions examples/MultipleScenes/MultipleScenes.ino
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
#include <Servo.h>
#endif

#define SERVO_PIN 3

// Servo object to send positions
Servo myServo;

Expand All @@ -29,8 +31,8 @@ void move(byte servoID, int position) {
BlenderServoAnimation::Animation animation;

void setup() {
// Attach the servo to pin 12
myServo.attach(12);
// Attach the servo to the defined servo pin
myServo.attach(SERVO_PIN);

// Set the position callback
animation.onPositionChange(move);
Expand Down
61 changes: 50 additions & 11 deletions examples/MultipleScenesSD/MultipleScenesSD.ino
Original file line number Diff line number Diff line change
Expand Up @@ -15,41 +15,80 @@
#include <Servo.h>
#endif

#define SERVO_PIN 3
#define CS_PIN 4

// Servo object to send positions
Servo myServo;

// File object to read animation data
File animationFile;

// We use a struct to map a scene to a filename
struct sceneMapping {
byte fps;
int frames;
String filename;
};

// Define an array of scene maps
sceneMapping sceneMappings[] = {
// Scene 0 = Scene A
{30, 100, "scene-a.bin"},

// Scene 1 = Scene B
{60, 200, "scene-b.bin"},
};

// Calculate the amount of scenes so that we can easily extend the array
const byte sceneAmount = sizeof(sceneMappings) / sizeof(sceneMappings[0]);

// Callback function which is called whenever a servo needs to be moved
void move(byte servoID, int position) {
// Ignore the servoID (there is only one servo) and write the current position
myServo.writeMicroseconds(position);
}

void changeFile(byte prevSceneIndex, byte nextSceneIndex) {
String filename = "test";
void changeSceneFile(byte prevSceneIndex, byte nextSceneIndex) {
String filename = sceneMappings[nextSceneIndex].filename;

animationFile.close();
animationFile = SD.open(filename, FILE_READ);
animationFile = SD.open(filename);

if (!animationFile) {
Serial.println("Opening file failed.");
}
}

// Animation object to represent the original Blender animation
BlenderServoAnimation::Animation animation;

void setup() {
SD.begin(4);
Serial.begin(9600);
while (!Serial);

Serial.println("Initializing SD card...");

if (!SD.begin(CS_PIN)) {
Serial.println("Initialization failed!");
while (true);
}
Serial.println("Initialization done.");

// Attach the servo to pin 12
myServo.attach(12);
myServo.attach(SERVO_PIN);

// Set the position callback
// Set the position and scene change callback
animation.onPositionChange(move);

// Set the scene callback
animation.onSceneChange(changeFile);
animation.onSceneChange(changeSceneFile);

// Add multiple scenes with the same File stream
animation.addScene(animationFile, 30, 100);
animation.addScene(animationFile, 60, 200);
for (byte i = 0; i < sceneAmount; i++) {
byte fps = sceneMappings[i].fps;
int frames = sceneMappings[i].frames;

animation.addScene(animationFile, fps, frames);
}

// Trigger the show loop mode
animation.loop();
Expand Down
File renamed without changes.
File renamed without changes.
41 changes: 35 additions & 6 deletions examples/SDAnimation/SDAnimation.ino
Original file line number Diff line number Diff line change
Expand Up @@ -15,30 +15,59 @@
#include <Servo.h>
#endif

#define SERVO_PIN 3
#define CS_PIN 4
#define FPS 30
#define FRAMES 100

// Servo object to send positions
Servo myServo;

// File object to read animation data
File animationFile;

// Callback function which is called whenever a servo needs to be moved
void move(byte servoID, int position) {
// Ignore the servoID (there is only one servo) and write the current position
myServo.writeMicroseconds(position);
}

void resetFile(byte prevSceneIndex, byte nextSceneIndex) {
animationFile.seek(0);
}

// Animation object to represent the original Blender animation
BlenderServoAnimation::Animation animation;

void setup() {
SD.begin(4);
File myFile = SD.open("test.txt", FILE_READ);
Serial.begin(9600);
while (!Serial);

Serial.println("Initializing SD card...");

if (!SD.begin(CS_PIN)) {
Serial.println("Initialization failed!");
while (true);
}
Serial.println("Initialization done.");

animationFile = SD.open("simple.bin");

if (animationFile) {
Serial.println("File opened successfully.");
} else {
Serial.println("Opening file failed.");
}

// Attach the servo to pin 12
myServo.attach(12);
// Attach the servo to the defined servo pin
myServo.attach(SERVO_PIN);

// Set the position callback
// Set the position and scene change callback
animation.onPositionChange(move);
animation.onSceneChange(resetFile);

// Add a scene with the File stream
animation.addScene(myFile, 30, 100);
animation.addScene(animationFile, FPS, FRAMES);

// Trigger the animation loop mode
animation.loop();
Expand Down
File renamed without changes.
9 changes: 6 additions & 3 deletions examples/SwitchModeButton/SwitchModeButton.ino
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,11 @@
#include <Servo.h>
#endif

#define BUTTON_PIN 2
#define SERVO_PIN 3

// The button instance for switching animation modes
OneButton modeButton(2, true, true);
OneButton modeButton(BUTTON_PIN, true, true);

// Servo object to send positions
Servo myServo;
Expand Down Expand Up @@ -92,8 +95,8 @@ void onLongPressed() {
}

void setup() {
// Attach the servo to pin 12
myServo.attach(12);
// Attach the servo to the defined servo pin
myServo.attach(SERVO_PIN);

// Attach the callbacks to the mode button
modeButton.attachClick(onPressed);
Expand Down
43 changes: 21 additions & 22 deletions src/internal/Animation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,31 +5,21 @@
using namespace BlenderServoAnimation;

Animation::~Animation() {
for (int i = 0; i < this->addIndex; i++) {
if (this->scenes[i]) {
delete this->scenes[i];
}
if (this->scenes) {
delete[] this->scenes;
}

if (this->liveStream != nullptr && this->isOneTimeLiveStream) {
delete this->liveStream;
}
}

int Animation::countScenes() {
return this->addIndex;
}

bool Animation::hasFinished() {
return this->playIndex + 1 >= this->addIndex;
}

bool Animation::hasScenes() {
return this->countScenes() > 0;
}

bool Animation::hasScene(byte index) {
return this->scenes[index] != nullptr;
return this->addIndex > 0;
}

void Animation::addScene(const byte *data, int size, byte fps, int frames) {
Expand All @@ -45,7 +35,17 @@ void Animation::addScene(Stream &stream, byte fps, int frames) {
}

void Animation::registerScene(Scene *scene) {
this->scenes[this->addIndex] = scene;
Scene **newScenes = new Scene *[this->addIndex + 1];

for (int i = 0; i < this->addIndex; i++) {
newScenes[i] = this->scenes[i];
}

newScenes[this->addIndex] = scene;

delete[] this->scenes;

this->scenes = newScenes;
this->addIndex++;
}

Expand All @@ -58,16 +58,14 @@ void Animation::setServoThreshold(byte id, byte value) {
}

void Animation::setScene(byte index) {
Scene *scene = this->scenes[index];

if (!scene) {
if (!this->scenes || index >= this->addIndex) {
return;
}

byte prevIndex = this->playIndex;

this->playIndex = index;
this->scene = scene;
this->scene = this->scenes[index];
this->scene->reset();

if (this->sceneCallback) {
Expand All @@ -78,7 +76,7 @@ void Animation::setScene(byte index) {
void Animation::setRandomScene() {
byte randomIndex = 0;

if (this->countScenes() > 1) {
if (this->addIndex > 1) {
randomIndex = random(this->addIndex);
}

Expand Down Expand Up @@ -109,10 +107,11 @@ void Animation::play() {
}

void Animation::playSingle(byte index) {
Scene *scene = this->scenes[index];
bool indexExists = this->scenes && index < this->addIndex;
bool modeIsAllowed = this->modeIsIn(2, MODE_DEFAULT, MODE_PAUSE);
bool pausedOtherScene = this->mode == MODE_PAUSE && this->playIndex != index;

if (scene == nullptr || !this->modeIsIn(2, MODE_DEFAULT, MODE_PAUSE) ||
(this->mode == MODE_PAUSE && playIndex != index)) {
if (!indexExists || !modeIsAllowed || pausedOtherScene) {
return;
}

Expand Down
7 changes: 1 addition & 6 deletions src/internal/Animation.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,6 @@ class Animation {
byte getMode();
byte getPlayIndex();

int countScenes();

void addScene(const byte *data, int size, byte fps, int frames);
void addScene(Stream &stream, byte fps, int frame);
void onPositionChange(pcb positionCallback);
Expand All @@ -48,16 +46,13 @@ class Animation {

bool hasFinished();
bool hasScenes();
bool hasScene(byte index);

Scene *getCurrentScene();

private:
static const int MAX_SCENE_COUNT = 256;

ServoManager servoManager;

Scene *scenes[MAX_SCENE_COUNT] = {nullptr};
Scene **scenes = nullptr;
Scene *scene = nullptr;

AnimationData *liveStream = nullptr;
Expand Down
4 changes: 4 additions & 0 deletions src/internal/Servo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,3 +68,7 @@ bool Servo::positionExceedsThreshold(int position) {

return this->threshold && positionDiff > this->threshold;
}

byte Servo::getId() {
return this->id;
}
2 changes: 2 additions & 0 deletions src/internal/Servo.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@ class Servo {

bool isNeutral();

byte getId();

private:
const byte STEP_DIVIDER = 10;

Expand Down
Loading

0 comments on commit d546a48

Please sign in to comment.