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

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

Committer:
henryrawas
Date:
Thu Feb 04 20:10:33 2016 +0000
Revision:
33:8b9dcbf6d8ec
Parent:
20:891b5270845a 
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 19:2f0ec9ac1238 3
henryrawas 14:570c8071f577 4 #include "mbed.h"
henryrawas 14:570c8071f577 5 #include "rtos.h"
henryrawas 14:570c8071f577 6
henryrawas 20:891b5270845a 7 #include "RobotArm.h"
henryrawas 20:891b5270845a 8 #include "MeasureBuf.h"
henryrawas 14:570c8071f577 9 #include "Alert.h"
henryrawas 20:891b5270845a 10 #include "ControllerIo.h"
henryrawas 20:891b5270845a 11 #include "Timestamp.h"
henryrawas 20:891b5270845a 12
henryrawas 14:570c8071f577 13
henryrawas 14:570c8071f577 14 // use controller timer
henryrawas 14:570c8071f577 15 extern Timer IdleTimer;
henryrawas 14:570c8071f577 16
henryrawas 20:891b5270845a 17 // use timestamp to get secs and ms
henryrawas 20:891b5270845a 18 Timestamp MessageTimer;
henryrawas 20:891b5270845a 19
henryrawas 14:570c8071f577 20
henryrawas 14:570c8071f577 21 // utility method to show state in console
henryrawas 14:570c8071f577 22 void DispMeasure(char* label, int partSize, float vals[])
henryrawas 14:570c8071f577 23 {
henryrawas 14:570c8071f577 24 // printf("%s: ", label);
henryrawas 14:570c8071f577 25 // for (int ix = 0; ix < partSize; ix++)
henryrawas 14:570c8071f577 26 // {
henryrawas 20:891b5270845a 27 // printf("%d:%f ", ix, vals[ix]);
henryrawas 14:570c8071f577 28 // }
henryrawas 14:570c8071f577 29 // printf("\r\n");
henryrawas 14:570c8071f577 30 }
henryrawas 14:570c8071f577 31
henryrawas 14:570c8071f577 32
henryrawas 14:570c8071f577 33 // send position alert
henryrawas 14:570c8071f577 34 void PushPositionAlert(RobotArm& robotArm)
henryrawas 14:570c8071f577 35 {
henryrawas 14:570c8071f577 36 // stop trying to move
henryrawas 14:570c8071f577 37 float diff = robotArm.GetLastPosDiff();
henryrawas 14:570c8071f577 38 int ix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 39 robotArm.MoveArmPositionsStop();
henryrawas 14:570c8071f577 40
henryrawas 14:570c8071f577 41 // report load error
henryrawas 14:570c8071f577 42 printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
henryrawas 14:570c8071f577 43
henryrawas 14:570c8071f577 44 Alert alert;
henryrawas 20:891b5270845a 45 MessageTimer.GetTimestamp();
henryrawas 14:570c8071f577 46
henryrawas 14:570c8071f577 47 ShowLedRed();
henryrawas 20:891b5270845a 48 alert.SetPositionAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, diff);
henryrawas 14:570c8071f577 49 AlertBuf.push(alert);
henryrawas 14:570c8071f577 50
henryrawas 14:570c8071f577 51 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 14:570c8071f577 52 }
henryrawas 14:570c8071f577 53
henryrawas 14:570c8071f577 54 // send load alert
henryrawas 14:570c8071f577 55 void PushLoadAlert(RobotArm& robotArm)
henryrawas 14:570c8071f577 56 {
henryrawas 14:570c8071f577 57 float lastVals[NUMJOINTS];
henryrawas 14:570c8071f577 58
henryrawas 14:570c8071f577 59 // stop trying to move
henryrawas 14:570c8071f577 60 robotArm.GetArmLastMeasure(NM_Load, lastVals);
henryrawas 14:570c8071f577 61 int ix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 62 robotArm.MoveArmPositionsStop();
henryrawas 14:570c8071f577 63
henryrawas 14:570c8071f577 64 // report load error
henryrawas 14:570c8071f577 65 printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 14:570c8071f577 66
henryrawas 14:570c8071f577 67 Alert alert;
henryrawas 20:891b5270845a 68 MessageTimer.GetTimestamp();
henryrawas 14:570c8071f577 69
henryrawas 14:570c8071f577 70 ShowLedRed();
henryrawas 20:891b5270845a 71 alert.SetLoadAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
henryrawas 14:570c8071f577 72 AlertBuf.push(alert);
henryrawas 14:570c8071f577 73
henryrawas 14:570c8071f577 74 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 14:570c8071f577 75 }
henryrawas 14:570c8071f577 76
henryrawas 14:570c8071f577 77 // send temperature alert
henryrawas 14:570c8071f577 78 void PushTemperatureAlert(RobotArm& robotArm)
henryrawas 14:570c8071f577 79 {
henryrawas 14:570c8071f577 80 float lastVals[NUMJOINTS];
henryrawas 14:570c8071f577 81
henryrawas 14:570c8071f577 82 // stop trying to move
henryrawas 14:570c8071f577 83 robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
henryrawas 14:570c8071f577 84 int ix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 85 robotArm.MoveArmPositionsStop();
henryrawas 14:570c8071f577 86
henryrawas 14:570c8071f577 87 // report load error
henryrawas 14:570c8071f577 88 printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 14:570c8071f577 89
henryrawas 14:570c8071f577 90 Alert alert;
henryrawas 20:891b5270845a 91 MessageTimer.GetTimestamp();
henryrawas 14:570c8071f577 92
henryrawas 14:570c8071f577 93 ShowLedRed();
henryrawas 20:891b5270845a 94 alert.SetTemperatureAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
henryrawas 14:570c8071f577 95 AlertBuf.push(alert);
henryrawas 14:570c8071f577 96
henryrawas 14:570c8071f577 97 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 14:570c8071f577 98 }
henryrawas 14:570c8071f577 99
henryrawas 14:570c8071f577 100 // send temperature alert
henryrawas 14:570c8071f577 101 void PushVoltageAlert(RobotArm& robotArm)
henryrawas 14:570c8071f577 102 {
henryrawas 14:570c8071f577 103 float lastVals[NUMJOINTS];
henryrawas 14:570c8071f577 104
henryrawas 14:570c8071f577 105 // stop trying to move
henryrawas 14:570c8071f577 106 robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
henryrawas 14:570c8071f577 107 int ix = robotArm.GetLastErrorPart();
henryrawas 14:570c8071f577 108 robotArm.MoveArmPositionsStop();
henryrawas 14:570c8071f577 109
henryrawas 14:570c8071f577 110 // report load error
henryrawas 14:570c8071f577 111 printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
henryrawas 14:570c8071f577 112
henryrawas 14:570c8071f577 113 Alert alert;
henryrawas 20:891b5270845a 114 MessageTimer.GetTimestamp();
henryrawas 14:570c8071f577 115
henryrawas 14:570c8071f577 116 ShowLedRed();
henryrawas 20:891b5270845a 117 alert.SetVoltageAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
henryrawas 14:570c8071f577 118 AlertBuf.push(alert);
henryrawas 14:570c8071f577 119
henryrawas 14:570c8071f577 120 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 14:570c8071f577 121 }
henryrawas 14:570c8071f577 122
henryrawas 14:570c8071f577 123
henryrawas 14:570c8071f577 124 // send hardware error alert
henryrawas 14:570c8071f577 125 void PushHardwareAlert(int partIx, int code)
henryrawas 14:570c8071f577 126 {
henryrawas 14:570c8071f577 127 Alert alert;
henryrawas 20:891b5270845a 128 MessageTimer.GetTimestamp();
henryrawas 14:570c8071f577 129
henryrawas 14:570c8071f577 130 ShowLedRed();
henryrawas 20:891b5270845a 131 alert.SetHardwareAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), partIx, code);
henryrawas 14:570c8071f577 132 AlertBuf.push(alert);
henryrawas 14:570c8071f577 133
henryrawas 14:570c8071f577 134 BuzzerStartMs((int)IdleTimer.read_ms(), 500);
henryrawas 14:570c8071f577 135 }
henryrawas 14:570c8071f577 136
henryrawas 14:570c8071f577 137 bool PushMeasurements(int partSize, RobotArm& robotArm)
henryrawas 14:570c8071f577 138 {
henryrawas 14:570c8071f577 139 // space for getting measurements from arm
henryrawas 17:0dbcbd8587fd 140 MeasureSnapshot measureSnap;
henryrawas 14:570c8071f577 141
henryrawas 14:570c8071f577 142 float lastVals[NUMJOINTS];
henryrawas 20:891b5270845a 143 MessageTimer.GetTimestamp();
henryrawas 20:891b5270845a 144 measureSnap.Created = MessageTimer.GetSecs();
henryrawas 20:891b5270845a 145 measureSnap.CreatedMs = MessageTimer.GetMs();
henryrawas 14:570c8071f577 146
henryrawas 14:570c8071f577 147 bool ok = true;
henryrawas 14:570c8071f577 148
henryrawas 14:570c8071f577 149 ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
henryrawas 14:570c8071f577 150 DispMeasure("Temperatures", partSize, lastVals);
henryrawas 18:224289104fc0 151 measureSnap.Temps.SetMeasure(partSize, lastVals);
henryrawas 14:570c8071f577 152
henryrawas 14:570c8071f577 153 if (ok)
henryrawas 14:570c8071f577 154 {
henryrawas 14:570c8071f577 155 ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
henryrawas 14:570c8071f577 156 DispMeasure("Voltage", partSize, lastVals);
henryrawas 18:224289104fc0 157 measureSnap.Volts.SetMeasure(partSize, lastVals);
henryrawas 14:570c8071f577 158 }
henryrawas 14:570c8071f577 159
henryrawas 14:570c8071f577 160 if (ok)
henryrawas 14:570c8071f577 161 {
henryrawas 14:570c8071f577 162 ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
henryrawas 14:570c8071f577 163 DispMeasure("Rotation", partSize, lastVals);
henryrawas 18:224289104fc0 164 measureSnap.Positions.SetMeasure(partSize, lastVals);
henryrawas 14:570c8071f577 165 }
henryrawas 14:570c8071f577 166
henryrawas 14:570c8071f577 167 if (ok)
henryrawas 14:570c8071f577 168 {
henryrawas 17:0dbcbd8587fd 169 ok = robotArm.GetArmLastMeasure(NM_Load, lastVals);
henryrawas 14:570c8071f577 170 DispMeasure("Load", partSize, lastVals);
henryrawas 18:224289104fc0 171 measureSnap.Loads.SetMeasure(partSize, lastVals);
henryrawas 17:0dbcbd8587fd 172 }
henryrawas 17:0dbcbd8587fd 173
henryrawas 17:0dbcbd8587fd 174 if (ok)
henryrawas 17:0dbcbd8587fd 175 {
henryrawas 17:0dbcbd8587fd 176 MeasureBuf.push(measureSnap);
henryrawas 14:570c8071f577 177 }
henryrawas 14:570c8071f577 178
henryrawas 14:570c8071f577 179 return ok;
henryrawas 14:570c8071f577 180 }