robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

RobotNode/NodeAX12.cpp@33:8b9dcbf6d8ec, 2016-02-04 (annotated)

Committer:
henryrawas
Date:
Thu Feb 04 20:10:33 2016 +0000
Revision:
33:8b9dcbf6d8ec
Parent:
19:2f0ec9ac1238 
update libs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 19:2f0ec9ac1238 1 // Copyright (c) Microsoft. All rights reserved.
henryrawas 19:2f0ec9ac1238 2 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
henryrawas 12:ac6c9d7f8c40 3
henryrawas 12:ac6c9d7f8c40 4 #include "mbed.h"
henryrawas 12:ac6c9d7f8c40 5 #include "NodeAX12.h"
henryrawas 12:ac6c9d7f8c40 6
henryrawas 12:ac6c9d7f8c40 7
henryrawas 12:ac6c9d7f8c40 8 NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
henryrawas 12:ac6c9d7f8c40 9 _Servo(pbus, ID)
henryrawas 12:ac6c9d7f8c40 10 {
henryrawas 15:4bd10f531cdc 11
henryrawas 12:ac6c9d7f8c40 12 }
henryrawas 12:ac6c9d7f8c40 13
henryrawas 12:ac6c9d7f8c40 14
henryrawas 12:ac6c9d7f8c40 15 bool NodeAX12::HasMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 16 {
henryrawas 12:ac6c9d7f8c40 17 switch (measureId)
henryrawas 12:ac6c9d7f8c40 18 {
henryrawas 12:ac6c9d7f8c40 19 case NM_Temperature:
henryrawas 12:ac6c9d7f8c40 20 return true;
henryrawas 12:ac6c9d7f8c40 21
henryrawas 12:ac6c9d7f8c40 22 case NM_Degrees:
henryrawas 12:ac6c9d7f8c40 23 return true;
henryrawas 12:ac6c9d7f8c40 24
henryrawas 12:ac6c9d7f8c40 25 case NM_Voltage:
henryrawas 12:ac6c9d7f8c40 26 return true;
henryrawas 12:ac6c9d7f8c40 27
henryrawas 12:ac6c9d7f8c40 28 case NM_Load:
henryrawas 12:ac6c9d7f8c40 29 return true;
henryrawas 12:ac6c9d7f8c40 30
henryrawas 12:ac6c9d7f8c40 31 default:
henryrawas 12:ac6c9d7f8c40 32 return false;
henryrawas 12:ac6c9d7f8c40 33 }
henryrawas 12:ac6c9d7f8c40 34 }
henryrawas 12:ac6c9d7f8c40 35
henryrawas 12:ac6c9d7f8c40 36 float NodeAX12::GetMeasure(int measureId)
henryrawas 12:ac6c9d7f8c40 37 {
henryrawas 12:ac6c9d7f8c40 38 switch (measureId)
henryrawas 12:ac6c9d7f8c40 39 {
henryrawas 12:ac6c9d7f8c40 40 case NM_Temperature:
henryrawas 15:4bd10f531cdc 41 return (float)_Servo.GetTemperature();
henryrawas 12:ac6c9d7f8c40 42
henryrawas 12:ac6c9d7f8c40 43 case NM_Degrees:
henryrawas 15:4bd10f531cdc 44 return (float)_Servo.GetPosition();
henryrawas 12:ac6c9d7f8c40 45
henryrawas 12:ac6c9d7f8c40 46 case NM_Voltage:
henryrawas 15:4bd10f531cdc 47 return (float)_Servo.GetSupplyVoltage();
henryrawas 12:ac6c9d7f8c40 48
henryrawas 12:ac6c9d7f8c40 49 case NM_Load:
henryrawas 15:4bd10f531cdc 50 return (float)_Servo.GetLoad();
henryrawas 12:ac6c9d7f8c40 51
henryrawas 12:ac6c9d7f8c40 52 default:
henryrawas 12:ac6c9d7f8c40 53 return 0.0f;
henryrawas 12:ac6c9d7f8c40 54 }
henryrawas 12:ac6c9d7f8c40 55 }
henryrawas 12:ac6c9d7f8c40 56
henryrawas 15:4bd10f531cdc 57 void NodeAX12::ClearMeasureCache()
henryrawas 12:ac6c9d7f8c40 58 {
henryrawas 15:4bd10f531cdc 59 _Servo.ClearCache();
henryrawas 12:ac6c9d7f8c40 60 }
henryrawas 12:ac6c9d7f8c40 61
henryrawas 12:ac6c9d7f8c40 62 bool NodeAX12::HasAction(int actionId)
henryrawas 12:ac6c9d7f8c40 63 {
henryrawas 12:ac6c9d7f8c40 64 switch (actionId)
henryrawas 12:ac6c9d7f8c40 65 {
henryrawas 12:ac6c9d7f8c40 66 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 67 return true;
henryrawas 12:ac6c9d7f8c40 68
henryrawas 12:ac6c9d7f8c40 69 case NA_Init:
henryrawas 12:ac6c9d7f8c40 70 return true;
henryrawas 12:ac6c9d7f8c40 71
henryrawas 12:ac6c9d7f8c40 72 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 73 return true;
henryrawas 12:ac6c9d7f8c40 74
henryrawas 12:ac6c9d7f8c40 75 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 76 return true;
henryrawas 12:ac6c9d7f8c40 77
henryrawas 12:ac6c9d7f8c40 78 default:
henryrawas 12:ac6c9d7f8c40 79 return false;
henryrawas 12:ac6c9d7f8c40 80 }
henryrawas 12:ac6c9d7f8c40 81 }
henryrawas 12:ac6c9d7f8c40 82
henryrawas 12:ac6c9d7f8c40 83
henryrawas 12:ac6c9d7f8c40 84 bool NodeAX12::DoAction(int actionId, float actionValue)
henryrawas 12:ac6c9d7f8c40 85 {
henryrawas 12:ac6c9d7f8c40 86 StatusCode sc;
henryrawas 12:ac6c9d7f8c40 87
henryrawas 12:ac6c9d7f8c40 88 switch (actionId)
henryrawas 12:ac6c9d7f8c40 89 {
henryrawas 12:ac6c9d7f8c40 90 case NA_Ping:
henryrawas 12:ac6c9d7f8c40 91 if (_Servo.Ping() == statusValid)
henryrawas 12:ac6c9d7f8c40 92 return true;
henryrawas 12:ac6c9d7f8c40 93 else
henryrawas 12:ac6c9d7f8c40 94 return false;
henryrawas 12:ac6c9d7f8c40 95
henryrawas 12:ac6c9d7f8c40 96 case NA_Init:
henryrawas 15:4bd10f531cdc 97 sc = _Servo.SetReplyDelay(100);
henryrawas 12:ac6c9d7f8c40 98 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 99 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 100 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 101
henryrawas 12:ac6c9d7f8c40 102 case NA_Rotate:
henryrawas 12:ac6c9d7f8c40 103 sc = _Servo.SetGoal(actionValue);
henryrawas 12:ac6c9d7f8c40 104 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 105
henryrawas 12:ac6c9d7f8c40 106 case NA_ClearError:
henryrawas 12:ac6c9d7f8c40 107 sc = _Servo.TorqueEnable(false);
henryrawas 12:ac6c9d7f8c40 108 sc = _Servo.TorqueEnable(true);
henryrawas 12:ac6c9d7f8c40 109 return sc == statusValid;
henryrawas 12:ac6c9d7f8c40 110
henryrawas 12:ac6c9d7f8c40 111 default:
henryrawas 12:ac6c9d7f8c40 112 return false;
henryrawas 12:ac6c9d7f8c40 113 }
henryrawas 12:ac6c9d7f8c40 114 }
henryrawas 12:ac6c9d7f8c40 115
henryrawas 12:ac6c9d7f8c40 116 NodePartType NodeAX12::GetNodeType()
henryrawas 12:ac6c9d7f8c40 117 {
henryrawas 12:ac6c9d7f8c40 118 return NT_AX12;
henryrawas 12:ac6c9d7f8c40 119 }
henryrawas 12:ac6c9d7f8c40 120
henryrawas 12:ac6c9d7f8c40 121 int NodeAX12::GetLastError()
henryrawas 12:ac6c9d7f8c40 122 {
henryrawas 12:ac6c9d7f8c40 123 return _Servo.GetLastError();
henryrawas 12:ac6c9d7f8c40 124 }
henryrawas 12:ac6c9d7f8c40 125
henryrawas 12:ac6c9d7f8c40 126 bool NodeAX12::HasError()
henryrawas 12:ac6c9d7f8c40 127 {
henryrawas 12:ac6c9d7f8c40 128 return _Servo.HasError();
henryrawas 12:ac6c9d7f8c40 129 }