robot arm demo team
demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Revision 4:36a4eceb1b7f, committed 2015-12-23
- Comitter:
- henryrawas
- Date:
- Wed Dec 23 18:34:06 2015 +0000
- Parent:
- 3:3f26dd87d6f9
- Child:
- 5:36916b1c5a06
- Commit message:
- RobotArm plus publish IoTHub status
Changed in this revision
--- a/AX-12A.lib Wed Dec 16 23:42:59 2015 +0000 +++ b/AX-12A.lib Wed Dec 23 18:34:06 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#d7642b2e155d +http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#7b970151bf19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ActionBuf.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include "ActionBuf.h"
+
+
+
+SafeCircBuf<ActionGroup, ActionBufSize, uint32_t> ActionBuf;
+
+bool ActionGroup::SetAction(ArmAction aId, char* args)
+{
+ if (strlen(args) < ActionArgSize)
+ {
+ ActionId = aId;
+
+ strcpy(ActionArg, args);
+
+ return true;
+ }
+ return false;
+}
+
+
+ActionSequence::ActionSequence()
+{
+};
+
+ActionSequence::ActionSequence(SequenceAction aType)
+{
+ ActionType = aType;
+};
+
+ActionSequence::ActionSequence(SequenceAction aType, vector<float>& vals, int ms)
+{
+ ActionType = aType;
+
+ if (aType == SA_SetGoal)
+ {
+ GoalVals = vals;
+ Ms = ms;
+ }
+ else if (aType == SA_Delay)
+ {
+ Ms = ms;
+ }
+}
+
+void ActionSequence::SetGoal(vector<float>& vals)
+{
+ GoalVals = vals;
+}
+
+void ActionSequence::SetDelay(int delay)
+{
+ Ms = delay;
+}
+
+void ActionSequence::SetAction(SequenceAction aType)
+{
+ ActionType = aType;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ActionBuf.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,63 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __ACTIONBUF_H__
+#define __ACTIONBUF_H__
+
+#include "mbed.h"
+#include "SafeCircBuf.h"
+#include "RobotArm.h"
+
+
+#define ActionBufSize 100
+#define ActionArgSize 40
+
+class ActionGroup
+{
+public:
+ ActionGroup() {};
+
+ bool SetAction(ArmAction aId, char* args);
+
+ ArmAction ActionId;
+
+ char ActionArg[ActionArgSize];
+};
+
+extern SafeCircBuf<ActionGroup, ActionBufSize, uint32_t> ActionBuf;
+
+
+enum SequenceAction
+{
+ SA_SetGoal = 0x1,
+ SA_Delay = 0x2,
+ SA_Status = 0x3,
+ SA_Repeat = 0x4
+};
+
+class ActionSequence
+{
+public:
+ ActionSequence();
+
+ ActionSequence(SequenceAction aType);
+
+ ActionSequence(SequenceAction aType, vector<float>& vals, int ms);
+
+ void SetGoal(vector<float>& vals);
+
+ void SetDelay(int ms);
+
+ void SetAction(SequenceAction aType);
+
+ SequenceAction ActionType;
+
+ vector<float> GoalVals;
+
+ int Ms;
+
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/EthernetInterface/#2fc406e2553f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IothubRobotArm.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,254 @@
+// Copyright (c) Microsoft. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full license information.
+
+#include "mbed.h"
+#include "rtos.h"
+
+#include "iothub_client.h"
+#include "iothub_message.h"
+#include "crt_abstractions.h"
+#include "iothubtransportamqp.h"
+#include "MeasureBuf.h"
+#include "IothubRobotArm.h"
+#include "IothubSerial.h"
+
+#ifdef MBED_BUILD_TIMESTAMP
+#include "certs.h"
+#endif // MBED_BUILD_TIMESTAMP
+
+static const char* connectionString = "HostName=HenryrIot.azure-devices.net;DeviceId=RobotArm;SharedAccessKey=FUTqsobGrV0ldHbnmOKluN6W90FG1G5z/jnhz+Gr53k=";
+static int callbackCounter;
+static int msgNumber;
+
+DEFINE_ENUM_STRINGS(IOTHUB_CLIENT_CONFIRMATION_RESULT, IOTHUB_CLIENT_CONFIRMATION_RESULT_VALUES);
+
+typedef struct EVENT_INSTANCE_TAG
+{
+ IOTHUB_MESSAGE_HANDLE messageHandle;
+ int messageTrackingId; // For tracking the messages within the user callback.
+} EVENT_INSTANCE;
+
+static IOTHUBMESSAGE_DISPOSITION_RESULT ReceiveMessageCallback(IOTHUB_MESSAGE_HANDLE message, void* userContextCallback)
+{
+ int* counter = (int*)userContextCallback;
+ const char* buffer;
+ size_t size;
+
+ if (IoTHubMessage_GetByteArray(message, (const unsigned char**)&buffer, &size) == IOTHUB_MESSAGE_OK)
+ {
+ (void)printf("Received Message [%d] with Data: <<<%.*s>>> & Size=%d\r\n", *counter, (int)size, buffer, (int)size);
+ }
+
+ // Retrieve properties from the message
+ MAP_HANDLE mapProperties = IoTHubMessage_Properties(message);
+ if (mapProperties != NULL)
+ {
+ const char*const* keys;
+ const char*const* values;
+ size_t propertyCount = 0;
+ if (Map_GetInternals(mapProperties, &keys, &values, &propertyCount) == MAP_OK)
+ {
+ if (propertyCount > 0)
+ {
+ printf("Message Properties:\r\n");
+ for (size_t index = 0; index < propertyCount; index++)
+ {
+ printf("\tKey: %s Value: %s\r\n", keys[index], values[index]);
+ }
+ printf("\r\n");
+ }
+ }
+ }
+
+ /* Some device specific action code goes here... */
+ (*counter)++;
+ return IOTHUBMESSAGE_ACCEPTED;
+}
+
+static void SendConfirmationCallback(IOTHUB_CLIENT_CONFIRMATION_RESULT result, void* userContextCallback)
+{
+ EVENT_INSTANCE* eventInstance = (EVENT_INSTANCE*)userContextCallback;
+ (void)printf("Confirmation[%d] received for message tracking id = %d with result = %s\r\n", callbackCounter, eventInstance->messageTrackingId, ENUM_TO_STRING(IOTHUB_CLIENT_CONFIRMATION_RESULT, result));
+ /* Some device specific action code goes here... */
+ callbackCounter++;
+ IoTHubMessage_Destroy(eventInstance->messageHandle);
+
+ if (callbackCounter == msgNumber)
+ {
+ // call SendMeasurements again in case more to send
+ }
+}
+
+// IoT Hub thread
+static Thread* IotThread = NULL;
+static bool IotThreadClose;
+
+// entry point for ITHub sending thread
+void IothubThread(void const *args)
+{
+ (void)printf("Iothub thread start\r\n");
+ IotThreadClose = false;
+ IothubRobotArm iotRobot;
+ iotRobot.Init();
+
+ while (1)
+ {
+ (void)printf("Iothub thread wait signal\r\n");
+ osEvent ev = Thread::signal_wait(IS_SendStatus);
+ //if ((ev.value.signals & IS_Close) == IS_Close)
+ if (IotThreadClose)
+ {
+ (void)printf("Iothub thread close signal\r\n");
+ iotRobot.Terminate();
+ break;
+ }
+ else
+ {
+ if ((ev.value.signals & IS_SendStatus) == IS_SendStatus)
+ {
+ (void)printf("Iothub thread send status signal\r\n");
+ iotRobot.SendMeasurements();
+ }
+ else
+ {
+ (void)printf("Iothub thread unknown signal\r\n");
+ }
+ }
+ }
+}
+
+
+bool StartIothubThread()
+{
+ IotThread = new Thread(IothubThread, NULL, osPriorityLow);
+ return true;
+}
+
+bool SendIothubMeasurements()
+{
+ (void)printf("Iothub thread signal to send data\r\n");
+ IotThread->signal_set(IS_SendStatus);
+ return true;
+}
+
+void EndIothubThread()
+{
+ IotThreadClose = true;
+ IotThread->signal_set(IS_SendStatus);
+}
+
+IothubRobotArm::IothubRobotArm()
+{
+ iotHubClientHandle = NULL;
+}
+
+bool IothubRobotArm::Init()
+{
+ int receiveContext = 0;
+ callbackCounter = 0;
+ msgNumber = 0;
+
+ // in case calling init twice without terminate
+ Terminate();
+
+ (void)printf("Starting the IoTHub RobotArm sample AMQP...\r\n");
+
+ if ((iotHubClientHandle = IoTHubClient_CreateFromConnectionString(connectionString, AMQP_Protocol)) == NULL)
+ {
+ (void)printf("ERROR: iotHubClientHandle is NULL!\r\n");
+ return false;
+ }
+ else
+ {
+#ifdef MBED_BUILD_TIMESTAMP
+ (void)printf("INFO: IoTHubClient_SetOption\r\n");
+ // For mbed add the certificate information
+ if (IoTHubClient_SetOption(iotHubClientHandle, "TrustedCerts", certificates) != IOTHUB_CLIENT_OK)
+ {
+ printf("failure to set option \"TrustedCerts\"\r\n");
+ return false;
+ }
+#endif // MBED_BUILD_TIMESTAMP
+
+ (void)printf("INFO: IoTHubClient_SetMessageCallback\r\n");
+ /* Setting Message call back, so we can receive Commands. */
+ if (IoTHubClient_SetMessageCallback(iotHubClientHandle, ReceiveMessageCallback, &receiveContext) != IOTHUB_CLIENT_OK)
+ {
+ (void)printf("ERROR: IoTHubClient_SetMessageCallback..........FAILED!\r\n");
+ return false;
+ }
+ else
+ {
+ (void)printf("IoTHubClient_SetMessageCallback...successful.\r\n");
+ }
+ }
+ return true;
+}
+
+void IothubRobotArm::Terminate()
+{
+ if (iotHubClientHandle != NULL)
+ {
+ IoTHubClient_Destroy(iotHubClientHandle);
+ iotHubClientHandle = NULL;
+ }
+}
+
+#define MESSAGE_LEN 1024
+static char msgText[MESSAGE_LEN];
+static char propText[MESSAGE_LEN];
+#define MESSAGE_COUNT 10
+
+void IothubRobotArm::SendMeasurements(void)
+{
+ EVENT_INSTANCE messages[MESSAGE_COUNT];
+
+ int i = 0;
+
+ // send until circular buf empty or send buffer max
+ while (i < MESSAGE_COUNT)
+ {
+ int msglen = msgSerialize.MeasureBufToString(msgText, MESSAGE_LEN);
+
+ if (msglen > 0)
+ {
+ (void)printf("Sending data bytes %d\r\n", msglen);
+ if ((messages[i].messageHandle = IoTHubMessage_CreateFromByteArray((const unsigned char*)msgText, msglen)) == NULL)
+ {
+ (void)printf("ERROR: iotHubMessageHandle is NULL!\r\n");
+ }
+ else
+ {
+ messages[i].messageTrackingId = msgNumber;
+
+ MAP_HANDLE propMap = IoTHubMessage_Properties(messages[i].messageHandle);
+ sprintf_s(propText, sizeof(propText), "PropMsg_%d", msgNumber);
+ if (Map_AddOrUpdate(propMap, "PropName", propText) != MAP_OK)
+ {
+ (void)printf("ERROR: Map_AddOrUpdate Failed!\r\n");
+ }
+
+ if (IoTHubClient_SendEventAsync(iotHubClientHandle, messages[i].messageHandle, SendConfirmationCallback, &messages[i]) != IOTHUB_CLIENT_OK)
+ {
+ (void)printf("ERROR: IoTHubClient_SendEventAsync..........FAILED!\r\n");
+ }
+ else
+ {
+ (void)printf("IoTHubClient_SendEventAsync accepted data for transmission to IoT Hub. bytes: %d\r\n", msglen);
+ }
+ msgNumber++;
+ }
+ }
+ else if (msglen == 0)
+ {
+ (void)printf("No more data to send \r\n");
+ break;
+ }
+ else if (msglen < 0)
+ {
+ (void)printf("ERROR: Serialized message too big for buffer\r\n");
+ break;
+ }
+ } // while
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IothubRobotArm.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,34 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef IOTHUB_ROBOTARM_H
+#define IOTHUB_ROBOTARM_H
+
+#include "iothub_client.h"
+#include "IothubSerial.h"
+
+class IothubRobotArm
+{
+public:
+ IothubRobotArm();
+
+ bool Init();
+
+ void Terminate();
+
+ void SendMeasurements(void);
+
+private:
+ IOTHUB_CLIENT_HANDLE iotHubClientHandle;
+
+ IothubSerial msgSerialize;
+};
+
+extern bool StartIothubThread();
+
+extern bool SendIothubMeasurements();
+
+extern void EndIothubThread();
+
+#endif /* IOTHUB_ROBOTARM_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IothubSerial.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "IothubSerial.h"
+#include "crt_abstractions.h"
+
+char* nametemp = "temp";
+char* namevolt = "volt";
+char* namedeg = "rot";
+
+IothubSerial::IothubSerial()
+{
+ _hasPending = false;
+}
+
+// try to serialize the measurements into the buffer
+// return bytes used or -1 if buffer too small or other error
+// current serialization is a json array with ',' at end
+// eg: temp: [22.0, 23.1, 22.3],
+int IothubSerial::MeasureGroupToString(MeasureGroup& mg, char* buf, int bufsize)
+{
+ char* name;
+ int slen;
+ int startlen = bufsize;
+
+ switch (mg.MeasId)
+ {
+ case NM_Temperature:
+ name = nametemp;
+ break;
+
+ case NM_Voltage:
+ name = namevolt;
+ break;
+
+ case NM_Degrees:
+ name = namedeg;
+ break;
+
+ default:
+ return -1;
+ }
+
+ if (bufsize > strlen(name) + 5)
+ {
+ slen = sprintf_s(buf, bufsize, "\"%s\": [", name);
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ }
+ else
+ return -1;
+ }
+ else
+ return -1;
+
+ for (int i = 0; i < mg.NumVals; i++)
+ {
+ if (bufsize > 9)
+ {
+ if (i < mg.NumVals - 1)
+ slen = sprintf_s(buf, bufsize, "%7.2f, ", mg.MeasVals[i]);
+ else
+ slen = sprintf_s(buf, bufsize, "%7.2f ", mg.MeasVals[i]);
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ }
+ else
+ return -1;
+ }
+ else
+ return -1;
+ }
+
+ if (bufsize > 2)
+ {
+ slen = sprintf_s(buf, bufsize, "],");
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ }
+ else
+ return -1;
+ }
+ else
+ return -1;
+
+ return startlen - bufsize;
+}
+
+// try to serialize one or more measurements into the buffer
+// return bytes used or -1 if buffer too small or other error. 0 if no data
+// current serialization is a json object with entry per measure
+// eg: { temp: [1, 2], volt: [12.1, 12.2] }
+int IothubSerial::MeasureBufToString(char* buf, int bufsize)
+{
+ int slen;
+ bool hasdata = false;
+ bool copydata = false;
+ int startlen = bufsize;
+
+ if (bufsize > 1)
+ {
+ slen = sprintf_s(buf, bufsize, "{");
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ }
+ else
+ return -1;
+ }
+ else
+ return -1;
+
+ if (_hasPending)
+ {
+ hasdata = true;
+ slen = MeasureGroupToString(_pending, buf, bufsize);
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ _hasPending = false;
+ copydata = true;
+ }
+ else
+ return -1; // no room for pending record
+ }
+
+ while (!MeasureBuf.empty())
+ {
+ if (!MeasureBuf.pop(_pending))
+ {
+ break;
+ }
+ hasdata = true;
+ _hasPending = true;
+ slen = MeasureGroupToString(_pending, buf, bufsize);
+ if (slen > 0)
+ {
+ bufsize -= slen;
+ buf += slen;
+ _hasPending = false;
+ copydata = true;
+ }
+ else
+ break; // no room to serialize, leave pending for mext message
+ }
+
+ if (!hasdata)
+ return 0; // no data
+
+ if (!copydata)
+ return -1; // have data but buffer too small
+
+ // replace final ',' with '}'
+ *(buf - 1) = '}';
+
+ return startlen - bufsize;
+}
+
+bool IothubSerial::HasMeasureGroup()
+{
+ return _hasPending;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IothubSerial.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,29 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __IOTHUB_SERIAL_H__
+#define __IOTHUB_SERIAL_H__
+
+#include "mbed.h"
+#include "RobotNode.h"
+#include "MeasureBuf.h"
+
+class IothubSerial
+{
+public:
+ IothubSerial();
+
+ int MeasureGroupToString(MeasureGroup& mg, char* buf, int bufsize);
+
+ int MeasureBufToString(char* buf, int bufsize);
+
+ bool HasMeasureGroup();
+
+private:
+ MeasureGroup _pending;
+
+ bool _hasPending;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MeasureBuf.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,20 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include "MeasureBuf.h"
+
+
+
+SafeCircBuf<MeasureGroup, MeasureBufSize, uint32_t> MeasureBuf;
+
+void MeasureGroup::SetMeasure(NodeMeasure mId, vector<float>& vals)
+{
+ MeasId = mId;
+ NumVals = MAX_PARTS < vals.size() ? MAX_PARTS : vals.size();
+
+ for (int ix = 0; ix < NumVals; ix++)
+ {
+ MeasVals[ix] = vals[ix];
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MeasureBuf.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,32 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __MEASUREBUF_H__
+#define __MEASUREBUF_H__
+
+#include "mbed.h"
+#include "SafeCircBuf.h"
+#include "RobotArm.h"
+
+
+#define MeasureBufSize 100
+
+
+class MeasureGroup
+{
+public:
+ MeasureGroup() {};
+
+ void SetMeasure(NodeMeasure mId, vector<float>& vals);
+
+ NodeMeasure MeasId;
+
+ int NumVals;
+
+ float MeasVals[MAX_PARTS];
+};
+
+extern SafeCircBuf<MeasureGroup, MeasureBufSize, uint32_t> MeasureBuf;
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/NTPClient.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +NTPClient#9a7b8df5fad7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotArm.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,358 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include <DynamixelBus.h>
+#include <AX12.h>
+#include <Terminal.h>
+#include <vector>
+#include <RobotArm.h>
+
+using namespace std;
+
+DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 );
+Terminal* RobotArmPc = NULL;
+
+// move arm thread timer routine
+void NextMove(void const * tid)
+{
+ osSignalSet((osThreadId)tid, AS_NextStep);
+}
+
+
+// set the id for each part in the chain, in order
+int PartIds[] = { 1, 2, 3, 4, 6 };
+
+RobotArm::RobotArm()
+{
+ // build arm
+ int i = 0;
+ RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
+ AX12* servo1 = new AX12( &dynbus, PartIds[i] );
+ servo1->TorqueEnable(true);
+ _armParts[i] = dynamic_cast<RobotNode*>(servo1);
+
+ i++;
+ RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
+ AX12* servo2 = new AX12( &dynbus, PartIds[i] );
+ servo2->TorqueEnable(true);
+ _armParts[i] = dynamic_cast<RobotNode*>(servo2);
+
+ i++;
+ RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
+ AX12* servo3 = new AX12( &dynbus, PartIds[i] );
+ servo3->TorqueEnable(true);
+ _armParts[i] = dynamic_cast<RobotNode*>(servo3);
+
+ i++;
+ RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
+ AX12* servo4 = new AX12( &dynbus, PartIds[i] );
+ servo4->TorqueEnable(true);
+ _armParts[i] = dynamic_cast<RobotNode*>(servo4);
+
+ i++;
+ RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);
+ AX12* servo6 = new AX12( &dynbus, PartIds[i] );
+ servo6->TorqueEnable(true);
+ _armParts[i] = dynamic_cast<RobotNode*>(servo6);
+
+ i++;
+ _numParts = i;
+
+ numsteps = 0;
+ _stepms = 20; // move every 20 ms
+}
+
+// move all parts to specified postions in ms time
+bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps)
+{
+ _lastErrorPart = 0;
+
+ GetArmLastPositions(lastpos);
+
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float difference = positions[ix]- lastpos[ix];
+ differentials.push_back( difference );
+ }
+
+ if (steps > 1000) steps = 1000;
+ if (ms <= 0) ms = 1;
+
+ delayms = ms / steps;
+ expDelay = delayms;
+
+ elapseTimer.start();
+ bool ok = true;
+
+ // move arm to new position in steps
+ for( int step = 1; step <= steps; step++)
+ {
+ //pc.foreground(Red);
+ //pc.background(Black);
+ //pc.locate( 0 , 24 );
+ float incr = (float)step / (float)steps;
+ for( int ix = 0; ix < _numParts; ix++ )
+ {
+ //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
+ if (_armParts[ix]->HasAction(NA_Rotate))
+ {
+ float goal = lastpos[ix] + (differentials[ix] * incr);
+ bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
+ if (!ok)
+ {
+ _lastErrorPart = ix;
+ break;
+ }
+ }
+ }
+ if (!ok)
+ break;
+ // adjust delay
+ int elapsed = (int)elapseTimer.read_ms();
+ expDelay += delayms;
+ if (elapsed > expDelay && elapsed - expDelay < delayms)
+ wait_ms(elapsed - expDelay);
+ else
+ wait_ms(delayms);
+ }
+
+ elapseTimer.stop();
+ return ok;
+}
+
+// move all parts to specified postions in ms time
+bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
+{
+ _lastErrorPart = 0;
+
+ MoveArmPositionsEnd();
+ GetArmLastPositions(lastpos);
+
+ differentials.clear();
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float difference = positions[ix] - lastpos[ix];
+ differentials.push_back( difference );
+ RobotArmPc->printf( "diff %d = %f\r\n", ix, difference);
+ }
+
+ numsteps = totms / _stepms;
+ if (numsteps == 0) numsteps = 1;
+
+ curstep = 1;
+
+ delayms = _stepms;
+ expDelay = _stepms;
+
+ elapseTimer.start();
+
+ return true;
+}
+
+bool RobotArm::MoveArmPositionsHasNext()
+{
+ return (curstep <= numsteps);
+}
+
+bool RobotArm::MoveArmPositionsNext(int& nextdelay)
+{
+ _lastErrorPart = 0;
+
+ if (curstep > numsteps)
+ {
+ // no more steps
+ MoveArmPositionsEnd();
+ return true;
+ }
+
+ bool ok = true;
+ float incr = (float)curstep / (float)numsteps;
+ for( int ix = 0; ix < _numParts; ix++ )
+ {
+ if (_armParts[ix]->HasAction(NA_Rotate))
+ {
+ float goal = lastpos[ix] + (differentials[ix] * incr);
+ bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
+ if (!ok)
+ {
+ _lastErrorPart = ix;
+ break;
+ }
+ }
+ }
+
+ if (!ok)
+ {
+ return false;
+ }
+
+ curstep++;
+ if (curstep > numsteps)
+ {
+ MoveArmPositionsEnd();
+ }
+ else
+ {
+ // adjust delay
+ int elapsed = (int)elapseTimer.read_ms();
+ expDelay += delayms;
+
+ if (elapsed > expDelay)
+ {
+ if (elapsed - expDelay < delayms)
+ nextdelay = elapsed - expDelay;
+ else
+ nextdelay = delayms;
+ }
+ else
+ {
+ // no delay before next step
+ nextdelay = 0;
+ }
+ }
+
+ return true;
+}
+
+bool RobotArm::MoveArmPositionsEnd()
+{
+ if (numsteps > 0)
+ {
+ elapseTimer.stop();
+ numsteps = 0;
+ }
+ return true;
+}
+
+// move one part to specified postion in ms time
+bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps)
+{
+ _lastErrorPart = 0;
+ if (!_armParts[partIx]->HasAction(NA_Rotate))
+ return false;
+
+ float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees);
+
+ float difference = position- lastpos;
+
+ if (steps > 1000) steps = 1000;
+ if (ms <= 0) ms = 1;
+
+ int delayms = ms / steps;
+
+ bool ok = true;
+
+ // move arm to new position in steps
+ for( int step = 1; step <= steps; step++)
+ {
+ //pc.foreground(Red);
+ //pc.background(Black);
+ //pc.locate( 0 , 24 );
+ float incr = (float)step / (float)steps;
+
+ //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
+ float goal = lastpos + (difference * incr);
+ bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal);
+ if (!ok)
+ {
+ _lastErrorPart = partIx;
+ break;
+ }
+
+ wait_ms(delayms);
+ }
+ return ok;
+}
+
+// get all parts positions
+bool RobotArm::GetArmPositions(vector<float>& outPos)
+{
+ outPos.clear();
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float pos = _armParts[ix]->GetMeasure(NM_Degrees);
+ outPos.push_back( pos );
+ }
+ return true;
+}
+
+// get all parts positions
+bool RobotArm::GetArmLastPositions(vector<float>& outPos)
+{
+ outPos.clear();
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
+ outPos.push_back( pos );
+ }
+ return true;
+}
+
+// get one part position
+float RobotArm::GetPartPosition(int partIx)
+{
+ return _armParts[partIx]->GetMeasure(NM_Degrees);
+}
+
+
+// get all parts positions
+bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
+{
+ outPos.clear();
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float pos = _armParts[ix]->GetMeasure(measureId);
+ outPos.push_back( pos );
+ }
+ return true;
+}
+
+// get all parts positions
+bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
+{
+ outPos.clear();
+ for( int ix = 0; ix < _numParts; ix++)
+ {
+ float pos = _armParts[ix]->GetLastMeasure(measureId);
+ outPos.push_back( pos );
+ }
+ return true;
+}
+
+// get one part position
+float RobotArm::GetPartMeasure(int measureId, int partIx)
+{
+ return _armParts[partIx]->GetMeasure(measureId);
+}
+
+int RobotArm::GetNumParts()
+{
+ return _numParts;
+}
+
+void RobotArm::SetStepMs(int stepms)
+{
+ if (stepms > 0 && stepms < 5000)
+ _stepms = stepms;
+}
+
+void RobotArm::SetThreadId(osThreadId tid)
+{
+ _tid = tid;
+}
+
+// get part by position
+RobotNode* RobotArm::GetArmPart(int partIx)
+{
+ return _armParts[partIx];
+}
+
+unsigned char RobotArm::GetLastError()
+{
+ return _armParts[_lastErrorPart]->GetLastError();
+}
+
+int RobotArm::GetLastErrorPart()
+{
+ return _lastErrorPart;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotArm.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,123 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __ROBOT_ARM_H__
+#define __ROBOT_ARM_H__
+
+#include "mbed.h"
+#include "rtos.h"
+
+#include "DynamixelBus.h"
+#include "RobotNode.h"
+
+#define MAX_PARTS 8
+
+
+enum ArmAction
+{
+ AA_RunSeq = 0x1,
+ AA_Status = 0x2,
+ AA_Alert = 0x3
+};
+
+enum ArmThreadSignals
+{
+ AS_Action = 0x1,
+ AS_LocalAlert = 0x2,
+ AS_Cancel = 0x4,
+ AS_NextStep = 0x8,
+ AS_NextSeq = 0x10
+};
+
+enum IothubThreadSignals
+{
+ IS_Close = 0x1,
+ IS_SendStatus = 0x2
+};
+
+class RobotArm
+{
+public:
+
+ RobotArm();
+
+ // move all parts to specified postions in ms time
+ bool MoveArmPositions(vector<float> positions, int ms, int steps);
+
+ // start - move all parts to specified postions in ms time
+ bool MoveArmPositionsStart(vector<float> positions, int ms);
+
+ // start - move all parts to specified postions - test if done
+ bool MoveArmPositionsHasNext();
+
+ // start - move all parts to specified postions - next step
+ bool MoveArmPositionsNext(int& nextdelay);
+
+ // start - move all parts to specified postions - finish
+ bool MoveArmPositionsEnd();
+
+ // move one part to specified postion in ms time
+ bool MovePartPosition(int partIx, float position, int ms, int steps);
+
+ // get all parts positions
+ bool GetArmPositions(vector<float>& outPos);
+
+ // get all parts last positions
+ bool GetArmLastPositions(vector<float>& outPos);
+
+ // get one part position
+ float GetPartPosition(int partIx);
+
+ // get all parts for a measurement
+ bool GetArmMeasure(int measureId, vector<float>& outPos);
+
+ // get all parts last measurement
+ bool GetArmLastMeasure(int measureId, vector<float>& outPos);
+
+ // get one part measurement
+ float GetPartMeasure(int measureId, int partIx);
+
+ int GetNumParts();
+
+ // set arm speed as ms between steps
+ void SetStepMs(int stepms);
+
+ // set ThreadId for signals
+ void SetThreadId(osThreadId tid);
+
+ // get the part object
+ RobotNode* GetArmPart(int partIx);
+
+ // get last error code from action
+ unsigned char GetLastError();
+
+ // get index of part with error
+ int GetLastErrorPart();
+
+private:
+ // sensors and actuators
+ RobotNode* _armParts[MAX_PARTS];
+
+ int _numParts;
+
+ // #ms between steps
+ int _stepms;
+
+ // thread id
+ osThreadId _tid;
+
+ // part ix for last error
+ int _lastErrorPart;
+
+ // for thread friendly moves
+ vector<float> lastpos;
+ vector<float> differentials;
+ int numsteps;
+ int curstep;
+ int delayms;
+ int expDelay;
+ Timer elapseTimer;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotNode/RobotNode.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,44 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __ROBOT_NODE_H__
+#define __ROBOT_NODE_H__
+
+enum NodeMeasure
+{
+ NM_Temperature = 0x1,
+ NM_Voltage = 0x2,
+ NM_Degrees = 0x3
+};
+
+enum NodeAction
+{
+ NA_Rotate = 0x1
+};
+
+enum NodePartType
+{
+ NT_AX12 = 0x1
+};
+
+
+class RobotNode
+{
+public:
+ virtual bool HasMeasure(int measureId) = 0;
+
+ virtual float GetMeasure(int measureId) = 0;
+
+ virtual float GetLastMeasure(int measureId) = 0;
+
+ virtual bool HasAction(int actionId) = 0;
+
+ virtual bool DoAction(int actionId, float actionValue) = 0;
+
+ virtual unsigned char GetLastError() = 0;
+
+ virtual NodePartType GetNodeType() = 0;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SafeCircBuf.h Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __SAFECIRCBUF_H__
+#define __SAFECIRCBUF_H__
+
+#include "mbed.h"
+#include "rtos.h"
+
+#include <CircularBuffer.h>
+
+template<typename T, uint32_t BufferSize, typename CounterType = uint32_t>
+class SafeCircBuf
+{
+public:
+ SafeCircBuf() {
+ }
+
+ ~SafeCircBuf() {
+ }
+
+ /** Push the transaction to the buffer. This overwrites the buffer if it's
+ * full
+ *
+ * @param data Data to be pushed to the buffer
+ */
+ void push(const T& data) {
+ _mutex.lock();
+ _circBuf.push(data);
+ _mutex.unlock();
+ }
+
+ /** Pop the transaction from the buffer
+ *
+ * @param data Data to be pushed to the buffer
+ * @return True if the buffer is not empty and data contains a transaction, false otherwise
+ */
+ bool pop(T& data) {
+ bool rc;
+ _mutex.lock();
+ rc = _circBuf.pop(data);
+ _mutex.unlock();
+ return rc;
+ }
+
+ /** Check if the buffer is empty
+ *
+ * @return True if the buffer is empty, false if not
+ */
+ bool empty() {
+ bool rc;
+ _mutex.lock();
+ rc = _circBuf.empty();
+ _mutex.unlock();
+ return rc;
+ }
+
+ /** Check if the buffer is full
+ *
+ * @return True if the buffer is full, false if not
+ */
+ bool full() {
+ bool rc;
+ _mutex.lock();
+ rc = _circBuf.full();
+ _mutex.unlock();
+ return rc;
+ }
+
+ /** Reset the buffer
+ *
+ */
+ void reset() {
+ _mutex.lock();
+ _circBuf.reset();
+ _mutex.unlock();
+ }
+
+private:
+ Mutex _mutex;
+ CircularBuffer<T, BufferSize, CounterType> _circBuf;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/iothub_amqp_transport.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AzureIoTClient/code/iothub_amqp_transport/#71bb8c60ca57
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/iothub_client.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AzureIoTClient/code/iothub_client/#02a02ab6402f
--- a/main.cpp Wed Dec 16 23:42:59 2015 +0000
+++ b/main.cpp Wed Dec 23 18:34:06 2015 +0000
@@ -1,15 +1,409 @@
#include "mbed.h"
+#include "rtos.h"
+
+#include "EthernetInterface.h"
+#include "mbed/logging.h"
+#include "mbed/mbedtime.h"
+#include <NTPClient.h>
#include <DynamixelBus.h>
#include <AX12.h>
#include <Terminal.h>
#include <vector>
+#include <RobotArm.h>
+#include <MeasureBuf.h>
+#include <IothubRobotArm.h>
+#include <ActionBuf.h>
+
using namespace std;
DigitalOut myled(LED_GREEN);
Terminal pc(USBTX, USBRX);
-DynamixelBus bus( PTC17, PTC16, D7, D6, 1000000 );
-vector<AX12> servos;
+
+extern Terminal* RobotArmPc;
+extern Terminal* AX12Pc;
+
+const float UpPos = 150.0f;
+
+
+void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
+{
+ vector<float> lastVals;
+
+ robotArm.GetArmMeasure(NM_Temperature, lastVals);
+ pc.printf( "Temperatures: ");
+ for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
+ {
+ pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
+ }
+ pc.printf( "\r\n");
+ measureGroup.SetMeasure(NM_Temperature, lastVals);
+ MeasureBuf.push(measureGroup);
+
+ robotArm.GetArmMeasure(NM_Voltage, lastVals);
+ pc.printf( "Voltage: ");
+ for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
+ {
+ pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
+ }
+ pc.printf( "\r\n");
+ measureGroup.SetMeasure(NM_Voltage, lastVals);
+ MeasureBuf.push(measureGroup);
+
+ robotArm.GetArmMeasure(NM_Degrees, lastVals);
+ pc.printf( "Rotation: ");
+ for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
+ {
+ pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
+ }
+ pc.printf( "\r\n");
+ measureGroup.SetMeasure(NM_Degrees, lastVals);
+ MeasureBuf.push(measureGroup);
+}
+
+
+int setupRealTime(void)
+{
+ int result;
+
+ (void)printf("setupRealTime begin\r\n");
+ if (EthernetInterface::connect())
+ {
+ (void)printf("Error initializing EthernetInterface.\r\n");
+ result = __LINE__;
+ }
+ else
+ {
+ (void)printf("setupRealTime NTP begin\r\n");
+ NTPClient ntp;
+ if (ntp.setTime("0.pool.ntp.org") != 0)
+ {
+ (void)printf("Failed setting time.\r\n");
+ result = __LINE__;
+ }
+ else
+ {
+ (void)printf("set time correctly!\r\n");
+ result = 0;
+ }
+ (void)printf("setupRealTime NTP end\r\n");
+ EthernetInterface::disconnect();
+ }
+ (void)printf("setupRealTime end\r\n");
+
+ return result;
+}
+
+int InitEthernet()
+{
+ (void)printf("Initializing mbed specific things...\r\n");
+
+ /* These are needed in order to initialize the time provider for Proton-C */
+ mbed_log_init();
+ mbedtime_init();
+
+ if (EthernetInterface::init())
+ {
+ (void)printf("Error initializing EthernetInterface.\r\n");
+ return -1;
+ }
+
+ if (setupRealTime() != 0)
+ {
+ (void)printf("Failed setting up real time clock\r\n");
+ return -1;
+ }
+
+ if (EthernetInterface::connect())
+ {
+ (void)printf("Error connecting EthernetInterface.\r\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+
+vector<float> upPositions;
+vector<float> homePositions;
+
+void MakeDemoSeq(vector<ActionSequence>& actions)
+{
+ // define actions
+ ActionSequence moveUp(SA_SetGoal, upPositions, 3000);
+ ActionSequence report(SA_Status);
+ ActionSequence pause(SA_Delay);
+ pause.SetDelay(5000);
+ ActionSequence moveDown(SA_SetGoal, homePositions, 3000);
+ ActionSequence rep(SA_Repeat);
+
+ // add actions into a sequence
+ actions.push_back(moveUp);
+ actions.push_back(report);
+ actions.push_back(pause);
+ actions.push_back(moveDown);
+ actions.push_back(report);
+ actions.push_back(pause);
+ actions.push_back(rep);
+}
+
+
+enum MainStates
+{
+ MS_Idle = 0x0,
+ MS_Running = 0x1,
+ MS_Paused = 0x2,
+ MS_Stopping = 0x3,
+ MS_Error = 0x4,
+ MS_CancelOne = 0x5,
+ MS_CancelAll = 0x6,
+ MS_CancelToM = 0x7
+};
+
+Queue<vector<ActionSequence>, 16> SequenceQ;
+MainStates MainState;
+bool RunInSequence;
+bool RunInMoveStep;
+bool RunInDelay;
+void* CancelToMatch;
+
+
+// run sequence thread timer routine
+void NextSeq(void const * tid)
+{
+ RunInDelay = false;
+ osSignalSet((osThreadId)tid, AS_Action);
+}
+
+void run()
+{
+ // init robot arm
+ RobotArm robotArm;
+
+ int partSize = robotArm.GetNumParts();
+ for( int servoIndex = 0; servoIndex < partSize; servoIndex++)
+ {
+ upPositions.push_back(UpPos);
+ }
+
+ // set running state
+ MainState = MS_Idle;
+ RunInSequence = false;
+ RunInMoveStep = false;
+ RunInDelay = false;
+
+ // get initial positions
+ MeasureGroup measureGroup;
+ robotArm.GetArmPositions(homePositions);
+
+ vector<float> lastVals;
+
+ // do inital status report
+ PushMeasurements(measureGroup, partSize, robotArm);
+ SendIothubMeasurements();
+
+ // Prepare main thread
+ osThreadId mainTid = osThreadGetId();
+ int32_t signals = AS_Action;
+
+ // create a sequence for demo
+ vector<ActionSequence> actions;
+ MakeDemoSeq(actions);
+
+ // add to queue
+ SequenceQ.put(&actions);
+
+ // state for sequence
+ vector<ActionSequence> *curseq = NULL;
+ int curseqIx = 0;
+
+ // signal to run the default action for demo
+ osSignalSet(mainTid, AS_Action);
+
+ RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());
+
+ MainState = MS_Running;
+
+ while( true )
+ {
+ osEvent mainEvent = osSignalWait(signals, osWaitForever);
+
+ switch (MainState)
+ {
+ case MS_Idle:
+ break;
+
+ case MS_Paused:
+ pc.printf( "Paused\r\n");
+ break;
+
+ case MS_Stopping:
+ pc.printf( "Stopping\r\n");
+ break;
+
+ case MS_Error:
+ pc.printf( "Error\r\n");
+ break;
+
+ case MS_CancelOne:
+ pc.printf( "Cancel sequence\r\n");
+ RunInSequence = false;
+ RunInMoveStep = false;
+ if (RunInDelay)
+ {
+ RunInDelay = false;
+ delayTimer.stop();
+ }
+ MainState = MS_Running;
+ osSignalSet(mainTid, AS_Action);
+ break;
+
+ case MS_CancelAll:
+ RunInSequence = false;
+ RunInMoveStep = false;
+ if (RunInDelay)
+ {
+ RunInDelay = false;
+ delayTimer.stop();
+ }
+ while (1) {
+ osEvent evt = SequenceQ.get(0);
+ if (evt.status != osEventMessage)
+ break;
+ }
+ MainState = MS_Running;
+ break;
+
+ case MS_CancelToM:
+ RunInSequence = false;
+ RunInMoveStep = false;
+ if (RunInDelay)
+ {
+ RunInDelay = false;
+ delayTimer.stop();
+ }
+ while (1) {
+ osEvent evt = SequenceQ.get(0);
+ if (evt.status != osEventMessage)
+ break;
+ else if (evt.value.p == CancelToMatch)
+ {
+ curseq = (vector<ActionSequence> *)evt.value.p;
+ curseqIx = 0;
+ RunInSequence = true;
+ }
+ }
+ MainState = MS_Running;
+ osSignalSet(mainTid, AS_Action);
+ break;
+
+ case MS_Running:
+ if (RunInDelay)
+ {
+ // waiting for timer to fire. do nothing
+ break;
+ }
+ if (!RunInSequence)
+ {
+ osEvent evt = SequenceQ.get(0);
+ if (evt.status == osEventMessage)
+ {
+ pc.printf( "New Seq \r\n");
+ curseq = (vector<ActionSequence> *)evt.value.p;
+ curseqIx = 0;
+ RunInSequence = true;
+ RunInMoveStep = false;
+ }
+ }
+
+ if (RunInSequence)
+ {
+ if (RunInMoveStep)
+ {
+ if (robotArm.MoveArmPositionsHasNext())
+ {
+ //pc.printf( "Next Step\r\n");
+ int delaystep = 0;
+ bool ok = robotArm.MoveArmPositionsNext(delaystep);
+ if (ok)
+ {
+ if (delaystep > 0)
+ {
+ RunInDelay = true;
+ delayTimer.start(delaystep);
+ }
+ else
+ osSignalSet(mainTid, AS_Action);
+ }
+ }
+ else
+ {
+ pc.printf( "No more Step\r\n");
+ RunInMoveStep = false;
+ }
+ }
+ if (!RunInMoveStep)
+ {
+ pc.printf( "Next Seq %d\r\n", curseqIx);
+
+ if (curseq != NULL)
+ {
+ ActionSequence aseq = (*curseq)[curseqIx];
+ curseqIx++;
+
+ bool ok;
+ switch (aseq.ActionType)
+ {
+ case SA_SetGoal:
+ pc.printf( " - Move arm start\r\n");
+ ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
+ if (ok)
+ {
+ RunInMoveStep = true;
+ osSignalSet(mainTid, AS_Action);
+ }
+ else
+ {
+ // TODO: send alert
+ }
+ break;
+ case SA_Delay:
+ pc.printf( " - Delay\r\n");
+ RunInDelay = true;
+ delayTimer.start(aseq.Ms);
+ break;
+
+ case SA_Status:
+ pc.printf( " - Status\r\n");
+ robotArm.GetArmPositions(lastVals);
+
+ PushMeasurements(measureGroup, partSize, robotArm);
+
+ SendIothubMeasurements();
+
+ osSignalSet(mainTid, AS_Action);
+ break;
+ case SA_Repeat:
+ pc.printf( " - Repeat\r\n");
+ curseqIx = 0;
+ osSignalSet(mainTid, AS_Action);
+ break;
+
+ }
+ }
+
+ if (curseqIx >= curseq->size())
+ {
+ RunInSequence = false;
+ }
+ }
+
+ }
+ break;
+ }
+
+ }
+}
int main()
{
@@ -27,90 +421,21 @@
pc.foreground(Teal);
pc.background(Black);
- pc.locate( 0 , 5 );
- for( int n = 1; n <= 254; n++)
- {
- if( bus.Ping(n) & statusValid )
- {
- pc.printf("Found servo ID = %d\r\n", n);
- AX12 servo( &bus, n );
- servos.push_back( servo );
- servo.TorqueEnable(true);
- }
- }
-
- // time delay is to allow the position encoders to come online after initial power supply event
- wait(5);
-
- // get home positions of servos
- pc.foreground(Purple);
- pc.background(Black);
-
- vector<float> homePositions;
- homePositions.clear();
- pc.locate( 0 , 12 );
- for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
- {
- float homePosition = servos[servoIndex].GetPosition();
- pc.printf( "home[%d] = %f\r\n", servoIndex, homePosition);
- homePositions.push_back( homePosition );
- }
+ InitEthernet();
+
+ // start IotHub connection
+ StartIothubThread();
- while( true )
- {
- // determine positions to extend arm into middle range
- pc.foreground(Green);
- pc.background(Black);
- pc.locate( 0 , 18 );
+ // time delay is to allow the position encoders to come online after initial power supply event ~ 5 secs
+ // and to allow IoTHub SSL connection established
+ // TODO: test for connection established
+ Thread::wait(15000);
+
+ pc.printf( "Initialization done. Ready to run. \r\n");
- int steps = 100;
- vector<float> differentials;
- for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
- {
- float difference = 150.0f - homePositions[servoIndex];
- pc.printf( "diff[%d] = %f\r\n", servoIndex, difference);
- differentials.push_back( difference );
- }
-
- // slowly move arm our of home position
- for( int step = 0; step < steps; step++)
- {
- pc.foreground(Red);
- pc.background(Black);
- pc.locate( 0 , 24 );
-
- for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
- {
- float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
- pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
- servos[servoIndex].SetGoal( goal );
- }
+ RobotArmPc = &pc;
+ AX12Pc = &pc;
- wait(.05);
- }
-
- // pause for 10 seconds
- wait(10);
-
-
- // slowly move all servos back to home position
- for( int step = steps; step > 0; step--)
- {
- pc.foreground(Red);
- pc.background(Black);
- pc.locate( 0 , 24 );
+ run();
- for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
- {
- float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
- pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
- servos[servoIndex].SetGoal( goal );
- }
-
- wait(.05);
- }
-
- // pause for 10 seconds
- wait(10);
- }
-}
\ No newline at end of file
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#c825593ece39
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/proton-c-mbed.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AzureIoTClient/code/proton-c-mbed/#d39ef3474ca8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/wolfSSL.lib Wed Dec 23 18:34:06 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/wolfSSL/code/wolfSSL/#28278596c2a2