initial commit
This commit is contained in:
300
libraries/ConfigurableFirmata/src/FirmataScheduler.cpp
Normal file
300
libraries/ConfigurableFirmata/src/FirmataScheduler.cpp
Normal file
@@ -0,0 +1,300 @@
|
||||
/*
|
||||
FirmataScheduler.cpp - Firmata library
|
||||
Copyright (C) 2012-2013 Norbert Truchsess. All rights reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
See file LICENSE.txt for further informations on licensing terms.
|
||||
*/
|
||||
|
||||
#include <ConfigurableFirmata.h>
|
||||
#include "FirmataFeature.h"
|
||||
#include "Encoder7Bit.h"
|
||||
#include "FirmataScheduler.h"
|
||||
#include "FirmataExt.h"
|
||||
|
||||
FirmataScheduler *FirmataSchedulerInstance;
|
||||
|
||||
void delayTaskCallback(long delay)
|
||||
{
|
||||
FirmataSchedulerInstance->delayTask(delay);
|
||||
}
|
||||
|
||||
FirmataScheduler::FirmataScheduler()
|
||||
{
|
||||
FirmataSchedulerInstance = this;
|
||||
tasks = NULL;
|
||||
running = NULL;
|
||||
Firmata.attachDelayTask(delayTaskCallback);
|
||||
}
|
||||
|
||||
void FirmataScheduler::handleCapability(byte pin)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
boolean FirmataScheduler::handlePinMode(byte pin, int mode)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
boolean FirmataScheduler::handleSysex(byte command, byte argc, byte* argv)
|
||||
{
|
||||
if (command == SCHEDULER_DATA) {
|
||||
if (argc > 0) {
|
||||
switch (argv[0]) {
|
||||
case CREATE_FIRMATA_TASK:
|
||||
{
|
||||
if (argc == 4) {
|
||||
createTask(argv[1], argv[2] | argv[3] << 7);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case DELETE_FIRMATA_TASK:
|
||||
{
|
||||
if (argc == 2) {
|
||||
deleteTask(argv[1]);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ADD_TO_FIRMATA_TASK:
|
||||
{
|
||||
if (argc > 2) {
|
||||
int len = num7BitOutbytes(argc - 2);
|
||||
Encoder7Bit.readBinary(len, argv + 2, argv + 2); //decode inplace
|
||||
addToTask(argv[1], len, argv + 2); //addToTask copies data...
|
||||
}
|
||||
break;
|
||||
}
|
||||
case DELAY_FIRMATA_TASK:
|
||||
{
|
||||
if (argc == 6) {
|
||||
argv++;
|
||||
Encoder7Bit.readBinary(4, argv, argv); //decode inplace
|
||||
delayTask(*(long*)((byte*)argv));
|
||||
}
|
||||
break;
|
||||
}
|
||||
case SCHEDULE_FIRMATA_TASK:
|
||||
{
|
||||
if (argc == 7) { //one byte taskid, 5 bytes to encode 4 bytes of long
|
||||
Encoder7Bit.readBinary(4, argv + 2, argv + 2); //decode inplace
|
||||
schedule(argv[1], *(long*)((byte*)argv + 2)); //argv[1] | argv[2]<<8 | argv[3]<<16 | argv[4]<<24
|
||||
}
|
||||
break;
|
||||
}
|
||||
case QUERY_ALL_FIRMATA_TASKS:
|
||||
{
|
||||
queryAllTasks();
|
||||
break;
|
||||
}
|
||||
case QUERY_FIRMATA_TASK:
|
||||
{
|
||||
if (argc == 2) {
|
||||
queryTask(argv[1]);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case RESET_FIRMATA_TASKS:
|
||||
{
|
||||
reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
|
||||
void FirmataScheduler::createTask(byte id, int len)
|
||||
{
|
||||
firmata_task *existing = findTask(id);
|
||||
if (existing) {
|
||||
reportTask(id, existing, true);
|
||||
}
|
||||
else {
|
||||
firmata_task *newTask = (firmata_task*)malloc(sizeof(firmata_task) + len);
|
||||
newTask->id = id;
|
||||
newTask->time_ms = 0;
|
||||
newTask->len = len;
|
||||
newTask->nextTask = tasks;
|
||||
newTask->pos = 0;
|
||||
tasks = newTask;
|
||||
}
|
||||
};
|
||||
|
||||
void FirmataScheduler::deleteTask(byte id)
|
||||
{
|
||||
firmata_task *current = tasks;
|
||||
firmata_task *previous = NULL;
|
||||
while (current) {
|
||||
if (current->id == id) {
|
||||
if (previous) {
|
||||
previous->nextTask = current->nextTask;
|
||||
}
|
||||
else {
|
||||
tasks = current->nextTask;
|
||||
}
|
||||
free (current);
|
||||
return;
|
||||
}
|
||||
else {
|
||||
previous = current;
|
||||
current = current->nextTask;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void FirmataScheduler::addToTask(byte id, int additionalBytes, byte *message)
|
||||
{
|
||||
firmata_task *existing = findTask(id);
|
||||
if (existing) { //task exists and has not been fully loaded yet
|
||||
if (existing->pos + additionalBytes <= existing->len) {
|
||||
for (int i = 0; i < additionalBytes; i++) {
|
||||
existing->messages[existing->pos++] = message[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
reportTask(id, NULL, true);
|
||||
}
|
||||
};
|
||||
|
||||
void FirmataScheduler::schedule(byte id, long delay_ms)
|
||||
{
|
||||
firmata_task *existing = findTask(id);
|
||||
if (existing) {
|
||||
existing->pos = 0;
|
||||
existing->time_ms = millis() + delay_ms;
|
||||
}
|
||||
else {
|
||||
reportTask(id, NULL, true);
|
||||
}
|
||||
};
|
||||
|
||||
void FirmataScheduler::delayTask(long delay_ms)
|
||||
{
|
||||
if (running) {
|
||||
long now = millis();
|
||||
running->time_ms += delay_ms;
|
||||
if (running->time_ms < now) { //if delay time allready passed by schedule to 'now'.
|
||||
running->time_ms = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FirmataScheduler::queryAllTasks()
|
||||
{
|
||||
Firmata.write(START_SYSEX);
|
||||
Firmata.write(SCHEDULER_DATA);
|
||||
Firmata.write(QUERY_ALL_TASKS_REPLY);
|
||||
firmata_task *task = tasks;
|
||||
while (task) {
|
||||
Firmata.write(task->id);
|
||||
task = task->nextTask;
|
||||
}
|
||||
Firmata.write(END_SYSEX);
|
||||
};
|
||||
|
||||
void FirmataScheduler::queryTask(byte id)
|
||||
{
|
||||
firmata_task *task = findTask(id);
|
||||
reportTask(id, task, false);
|
||||
}
|
||||
|
||||
void FirmataScheduler::reportTask(byte id, firmata_task *task, boolean error)
|
||||
{
|
||||
Firmata.write(START_SYSEX);
|
||||
Firmata.write(SCHEDULER_DATA);
|
||||
if (error) {
|
||||
Firmata.write(ERROR_TASK_REPLY);
|
||||
} else {
|
||||
Firmata.write(QUERY_TASK_REPLY);
|
||||
}
|
||||
Firmata.write(id);
|
||||
if (task) {
|
||||
Encoder7Bit.startBinaryWrite();
|
||||
for (int i = 3; i < firmata_task_len(task); i++) {
|
||||
Encoder7Bit.writeBinary(((byte *)task)[i]); //don't write first 3 bytes (firmata_task*, byte); makes use of AVR byteorder (LSB first)
|
||||
}
|
||||
Encoder7Bit.endBinaryWrite();
|
||||
}
|
||||
Firmata.write(END_SYSEX);
|
||||
};
|
||||
|
||||
void FirmataScheduler::runTasks()
|
||||
{
|
||||
if (tasks) {
|
||||
long now = millis();
|
||||
firmata_task *current = tasks;
|
||||
firmata_task *previous = NULL;
|
||||
while (current) {
|
||||
if (current->time_ms > 0 && current->time_ms < now) { // TODO handle overflow
|
||||
if (execute(current)) {
|
||||
previous = current;
|
||||
current = current->nextTask;
|
||||
}
|
||||
else {
|
||||
if (previous) {
|
||||
previous->nextTask = current->nextTask;
|
||||
free(current);
|
||||
current = previous->nextTask;
|
||||
}
|
||||
else {
|
||||
tasks = current->nextTask;
|
||||
free(current);
|
||||
current = tasks;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
current = current->nextTask;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
void FirmataScheduler::reset()
|
||||
{
|
||||
while (tasks) {
|
||||
firmata_task *nextTask = tasks->nextTask;
|
||||
free(tasks);
|
||||
tasks = nextTask;
|
||||
}
|
||||
};
|
||||
|
||||
//private
|
||||
boolean FirmataScheduler::execute(firmata_task *task)
|
||||
{
|
||||
long start = task->time_ms;
|
||||
int pos = task->pos;
|
||||
int len = task->len;
|
||||
byte *messages = task->messages;
|
||||
running = task;
|
||||
while (pos < len) {
|
||||
Firmata.parse(messages[pos++]);
|
||||
if (start != task->time_ms) { // return true if task got rescheduled during run.
|
||||
task->pos = ( pos == len ? 0 : pos ); // last message executed? -> start over next time
|
||||
running = NULL;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
running = NULL;
|
||||
return false;
|
||||
}
|
||||
|
||||
firmata_task *FirmataScheduler::findTask(byte id)
|
||||
{
|
||||
firmata_task *currentTask = tasks;
|
||||
while (currentTask) {
|
||||
if (id == currentTask->id) {
|
||||
return currentTask;
|
||||
} else {
|
||||
currentTask = currentTask->nextTask;
|
||||
}
|
||||
};
|
||||
return NULL;
|
||||
}
|
||||
Reference in New Issue
Block a user