Diff: main.cpp

Revision:

0:191698f11ceb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 14 11:32:52 2010 +0000
@@ -0,0 +1,133 @@
+#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...");
+
+}
\ No newline at end of file