Robert Oates / Mbed 2 deprecated Nottingham

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:5de67898f1cc, 2010-12-14 (annotated)

Committer:
DrCyberBob
Date:
Tue Dec 14 15:08:27 2010 +0000
Revision:
0:5de67898f1cc
Update to use constant for the servo ID (Robotis Servos default to 1)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DrCyberBob 0:5de67898f1cc 1 #include "mbed.h"
DrCyberBob 0:5de67898f1cc 2
DrCyberBob 0:5de67898f1cc 3 #include "AX12.h"
DrCyberBob 0:5de67898f1cc 4 #include <string>
DrCyberBob 0:5de67898f1cc 5 using namespace std;
DrCyberBob 0:5de67898f1cc 6 #define SERVO_ID 1
DrCyberBob 0:5de67898f1cc 7
DrCyberBob 0:5de67898f1cc 8 PwmOut act1(p21);
DrCyberBob 0:5de67898f1cc 9 PwmOut act2(p22);
DrCyberBob 0:5de67898f1cc 10 PwmOut led1(LED1);
DrCyberBob 0:5de67898f1cc 11 AX12 myax12(p9, p10, SERVO_ID);
DrCyberBob 0:5de67898f1cc 12 Serial pc(USBTX, USBRX);
DrCyberBob 0:5de67898f1cc 13
DrCyberBob 0:5de67898f1cc 14 int command = 0;
DrCyberBob 0:5de67898f1cc 15 #define MAX_BUFFER_SIZE 20
DrCyberBob 0:5de67898f1cc 16 int bufferCounter = 0;
DrCyberBob 0:5de67898f1cc 17
DrCyberBob 0:5de67898f1cc 18 string buffer;
DrCyberBob 0:5de67898f1cc 19 string commandBuffer;
DrCyberBob 0:5de67898f1cc 20 bool validPacket = false;
DrCyberBob 0:5de67898f1cc 21 bool notFinished = true;
DrCyberBob 0:5de67898f1cc 22
DrCyberBob 0:5de67898f1cc 23 void doCommand(string cmdBuffer)
DrCyberBob 0:5de67898f1cc 24 {
DrCyberBob 0:5de67898f1cc 25 //printf("Command received: %s\n\r",cmdBuffer);
DrCyberBob 0:5de67898f1cc 26
DrCyberBob 0:5de67898f1cc 27 if (commandBuffer.length() == 0)
DrCyberBob 0:5de67898f1cc 28 return;
DrCyberBob 0:5de67898f1cc 29
DrCyberBob 0:5de67898f1cc 30 int index = commandBuffer.find("=");
DrCyberBob 0:5de67898f1cc 31
DrCyberBob 0:5de67898f1cc 32 // format should be cmd=value - so check for valid position for the '='
DrCyberBob 0:5de67898f1cc 33 if (index < 1)
DrCyberBob 0:5de67898f1cc 34 return;
DrCyberBob 0:5de67898f1cc 35
DrCyberBob 0:5de67898f1cc 36 string cmd = cmdBuffer.substr(0,index);
DrCyberBob 0:5de67898f1cc 37 string value = cmdBuffer.substr(index+1,cmdBuffer.length()-index-1);
DrCyberBob 0:5de67898f1cc 38
DrCyberBob 0:5de67898f1cc 39 //printf("Cmd: %s\n\r",cmd);
DrCyberBob 0:5de67898f1cc 40 //printf("Value: %s\n\r",value);
DrCyberBob 0:5de67898f1cc 41
DrCyberBob 0:5de67898f1cc 42 if (cmd=="act1")
DrCyberBob 0:5de67898f1cc 43 {
DrCyberBob 0:5de67898f1cc 44 act1=strtod(value.c_str(),NULL);
DrCyberBob 0:5de67898f1cc 45 led1=act1;
DrCyberBob 0:5de67898f1cc 46 printf("<act1=%s>\n\r",value);
DrCyberBob 0:5de67898f1cc 47 }
DrCyberBob 0:5de67898f1cc 48 if (cmd=="act2")
DrCyberBob 0:5de67898f1cc 49 {
DrCyberBob 0:5de67898f1cc 50 act2=strtod(value.c_str(),NULL);
DrCyberBob 0:5de67898f1cc 51 printf("<act2=%s>\n\r",value);
DrCyberBob 0:5de67898f1cc 52 }
DrCyberBob 0:5de67898f1cc 53 if (cmd=="servo1")
DrCyberBob 0:5de67898f1cc 54 {
DrCyberBob 0:5de67898f1cc 55 myax12.SetGoal(strtol(value.c_str(),NULL,10));
DrCyberBob 0:5de67898f1cc 56 printf("<servo1=%s>\n\r",value);
DrCyberBob 0:5de67898f1cc 57 }
DrCyberBob 0:5de67898f1cc 58 if (cmd=="led1")
DrCyberBob 0:5de67898f1cc 59 {
DrCyberBob 0:5de67898f1cc 60 led1 = strtod(value.c_str(),NULL);
DrCyberBob 0:5de67898f1cc 61 printf("<led1=%s>\n\r",value);
DrCyberBob 0:5de67898f1cc 62 }
DrCyberBob 0:5de67898f1cc 63 else if (cmd == "quit")
DrCyberBob 0:5de67898f1cc 64 {
DrCyberBob 0:5de67898f1cc 65 notFinished = false;
DrCyberBob 0:5de67898f1cc 66 printf("Quitting ....\n\r");
DrCyberBob 0:5de67898f1cc 67 }
DrCyberBob 0:5de67898f1cc 68
DrCyberBob 0:5de67898f1cc 69 }
DrCyberBob 0:5de67898f1cc 70
DrCyberBob 0:5de67898f1cc 71 void serialHandler()
DrCyberBob 0:5de67898f1cc 72 {
DrCyberBob 0:5de67898f1cc 73 char ch = pc.getc();
DrCyberBob 0:5de67898f1cc 74
DrCyberBob 0:5de67898f1cc 75 // echo back
DrCyberBob 0:5de67898f1cc 76 pc.putc(ch);
DrCyberBob 0:5de67898f1cc 77
DrCyberBob 0:5de67898f1cc 78 switch(ch)
DrCyberBob 0:5de67898f1cc 79 {
DrCyberBob 0:5de67898f1cc 80 case '[':
DrCyberBob 0:5de67898f1cc 81 buffer = "";
DrCyberBob 0:5de67898f1cc 82 validPacket = true;
DrCyberBob 0:5de67898f1cc 83 break;
DrCyberBob 0:5de67898f1cc 84 case ']':
DrCyberBob 0:5de67898f1cc 85 commandBuffer = buffer;
DrCyberBob 0:5de67898f1cc 86 buffer = "";
DrCyberBob 0:5de67898f1cc 87 validPacket = false;
DrCyberBob 0:5de67898f1cc 88 doCommand(commandBuffer);
DrCyberBob 0:5de67898f1cc 89 break;
DrCyberBob 0:5de67898f1cc 90 default:
DrCyberBob 0:5de67898f1cc 91 if (validPacket)
DrCyberBob 0:5de67898f1cc 92 buffer = buffer + ch;
DrCyberBob 0:5de67898f1cc 93
DrCyberBob 0:5de67898f1cc 94 }
DrCyberBob 0:5de67898f1cc 95
DrCyberBob 0:5de67898f1cc 96 }
DrCyberBob 0:5de67898f1cc 97
DrCyberBob 0:5de67898f1cc 98 void demo()
DrCyberBob 0:5de67898f1cc 99 {
DrCyberBob 0:5de67898f1cc 100
DrCyberBob 0:5de67898f1cc 101 myax12.SetGoal(80); // go to 60 degrees
DrCyberBob 0:5de67898f1cc 102 act1 = 0;
DrCyberBob 0:5de67898f1cc 103 act2=1;
DrCyberBob 0:5de67898f1cc 104 led1 = 0;
DrCyberBob 0:5de67898f1cc 105
DrCyberBob 0:5de67898f1cc 106 wait (3.5);
DrCyberBob 0:5de67898f1cc 107
DrCyberBob 0:5de67898f1cc 108 myax12.SetGoal(200); // go to 240 degrees
DrCyberBob 0:5de67898f1cc 109 act1 = 1;
DrCyberBob 0:5de67898f1cc 110 led1 = .7;
DrCyberBob 0:5de67898f1cc 111 act2=0;
DrCyberBob 0:5de67898f1cc 112 wait (3.5);
DrCyberBob 0:5de67898f1cc 113
DrCyberBob 0:5de67898f1cc 114 }
DrCyberBob 0:5de67898f1cc 115
DrCyberBob 0:5de67898f1cc 116 int main() {
DrCyberBob 0:5de67898f1cc 117
DrCyberBob 0:5de67898f1cc 118 pc.attach(serialHandler);
DrCyberBob 0:5de67898f1cc 119 act1.period_us(100);
DrCyberBob 0:5de67898f1cc 120 act1.pulsewidth_us(10);
DrCyberBob 0:5de67898f1cc 121 act2.period_us(100);
DrCyberBob 0:5de67898f1cc 122 act2.pulsewidth_us(10);
DrCyberBob 0:5de67898f1cc 123 led1.period_us(100);
DrCyberBob 0:5de67898f1cc 124 led1.pulsewidth_us(10);
DrCyberBob 0:5de67898f1cc 125 act1 = 1.0;
DrCyberBob 0:5de67898f1cc 126 act2 = 1.0;
DrCyberBob 0:5de67898f1cc 127 led1 = 0.5;
DrCyberBob 0:5de67898f1cc 128
DrCyberBob 0:5de67898f1cc 129 printf("Starting...");
DrCyberBob 0:5de67898f1cc 130
DrCyberBob 0:5de67898f1cc 131 while (notFinished)
DrCyberBob 0:5de67898f1cc 132 {
DrCyberBob 0:5de67898f1cc 133 }
DrCyberBob 0:5de67898f1cc 134
DrCyberBob 0:5de67898f1cc 135 printf("Finished...");
DrCyberBob 0:5de67898f1cc 136
DrCyberBob 0:5de67898f1cc 137 }