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

ControllerUtil.cpp@17:0dbcbd8587fd, 2016-01-22 (annotated)

Committer:
henryrawas
Date:
Fri Jan 22 01:35:07 2016 +0000
Revision:
17:0dbcbd8587fd
Parent:
14:570c8071f577 
add all measures to circular buffer at once. Handle alert command

Who changed what in which revision?

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