File content as of revision 51:bab6e5eb864a:

#include <vector>

#include "mbed.h"
#include "controlLoops.h"
#include "parser.h"

Ticker lifeTicker;

volatile bool commandFinished = false;

void stopCommand() {
    commandFinished = true;
    lifeTicker.detach();
    controlTicker.detach();
}

void rotate(float r) {
    PRINT("Rotating for %.2f revolutions", goalRevs);
    goalRevs = r;
}

void setVelocity(float v) {
    targetV = v;
    PRINT("Spinning with V=%.2f rad/s", targetV);

    lifeTicker.attach(&stopCommand, 10);
}

void rotateWith(float r, float v) {
    goalRevs = r;
    targetV = v;
    PRINT("Rotating for %.2f revolutions with V=%.2f", goalRevs, targetV);
}

void playTune(float freq) {
    motorPWMPeriod = 1.0f / freq;
    motorOut(0, 0.5);
    Thread::wait(1000);
}

void playTunes(const vector<float>& tunes) {
    PRINT("Playing music");
    for (int i=0; i<tunes.size(); ++i) {
        playTune(tunes[i]);
    }
//    motorPWMPeriod = defaultMotorPWMPeriod;
    stopMotor();
}


void runCommand() {
    char buffer[256];
    PRINT("Ready for command");
    scanf("%s", buffer);
    ParseResult command = parse((char*) buffer);
    if (command.success) {
        switch(command.mode) {
            case 0: rotateWith(command.rotations, command.velocity); break;
            case 1: rotate(command.rotations); break;
            case 2: setVelocity(command.velocity); break;
            case 3: playTunes(command.tunes); break;
            default: return;
        }
    } else {
        PRINT("Invalid command");
    }
}