File content as of revision 0:191698f11ceb:

#include "mbed.h"

#include "AX12.h"
#include <string>
using namespace std;

PwmOut act1(p21);
PwmOut act2(p22);
PwmOut led1(LED1);
AX12 myax12(p9, p10, 6);
Serial pc(USBTX, USBRX);

int command = 0;
#define MAX_BUFFER_SIZE 20
int bufferCounter = 0;

string buffer;
string commandBuffer;
bool validPacket = false;
bool notFinished = true;

void doCommand(string cmdBuffer)
{
    //printf("Command received: %s\n\r",cmdBuffer);

    if (commandBuffer.length() == 0)
        return;
        
    int index = commandBuffer.find("=");
        
    // format should be cmd=value - so check for valid position for the '='
    if (index < 1)
        return;

    string cmd = cmdBuffer.substr(0,index);
    string value = cmdBuffer.substr(index+1,cmdBuffer.length()-index-1);
    
    //printf("Cmd: %s\n\r",cmd);
    //printf("Value: %s\n\r",value);  
    
    if (cmd=="act1")
    {
        act1=strtod(value.c_str(),NULL);
        led1=act1;
        printf("<act1=%s>\n\r",value);  
    }
    if (cmd=="act2")
    {
        act2=strtod(value.c_str(),NULL);
    }
    if (cmd=="servo1")
    {
        myax12.SetGoal(strtol(value.c_str(),NULL,10));  
    }
    if (cmd=="led1")
    {
        led1 = strtod(value.c_str(),NULL);  
    }
    else if (cmd == "quit")
    {
        notFinished = false;
        printf("Quitting ....\n\r");
    }
          
}

void serialHandler() 
{
    char ch = pc.getc();

    // echo back
    pc.putc(ch);
    
    switch(ch)
    {
        case '[':
            buffer = "";
            validPacket = true;
            break;
        case ']':
            commandBuffer = buffer;
            buffer = "";
            validPacket = false;
            doCommand(commandBuffer);
            break;        
        default:
            if (validPacket)
                buffer = buffer + ch;
    
    }
        
}

void demo()
{

        myax12.SetGoal(80);  // go to 60 degrees
        act1 = 0;
        act2=1;
        led1 = 0;
        
        wait (3.5);
        
        myax12.SetGoal(200); // go to 240 degrees
        act1 = 1;
        led1 = .7;
        act2=0;
        wait (3.5);

}

int main() {

    pc.attach(serialHandler);
    act1.period_us(100);
    act1.pulsewidth_us(10);
    act2.period_us(100);
    act2.pulsewidth_us(10);
    led1.period_us(100);
    led1.pulsewidth_us(10);
    act1 = 1.0;
    act2 = 1.0;
    led1 = 0.5;
   
    printf("Starting...");
    
    while (notFinished) 
    {
    }

    printf("Finished...");

}