Diff: Utils/ControllerUtil.cpp

Revision:

18:224289104fc0

Parent:

17:0dbcbd8587fd

Child:

19:2f0ec9ac1238

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Utils/ControllerUtil.cpp	Sat Jan 23 00:08:30 2016 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include <RobotArm.h>
+#include <MeasureBuf.h>
+#include <ControllerIo.h>
+#include "Alert.h"
+
+// use controller timer
+extern Timer IdleTimer;
+
+
+// utility method to show state in console
+void DispMeasure(char* label, int partSize, float vals[])
+{
+//    printf("%s: ", label);
+//    for (int ix = 0; ix < partSize; ix++)
+//    {
+//        printf("%d:%f ", ix, vals[ix]); 
+//    }
+//    printf("\r\n");
+}
+
+
+// send position alert
+void PushPositionAlert(RobotArm& robotArm)
+{
+    // stop trying to move
+    float diff = robotArm.GetLastPosDiff();
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
+
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetPositionAlert(seconds, ix, diff);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+// send load alert
+void PushLoadAlert(RobotArm& robotArm)
+{
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Load, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
+
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetLoadAlert(seconds, ix, lastVals[ix]);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+// send temperature alert
+void PushTemperatureAlert(RobotArm& robotArm)
+{
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
+
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetTemperatureAlert(seconds, ix, lastVals[ix]);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+// send temperature alert
+void PushVoltageAlert(RobotArm& robotArm)
+{
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
+
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetVoltageAlert(seconds, ix, lastVals[ix]);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+
+// send hardware error alert
+void PushHardwareAlert(int partIx, int code)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetHardwareAlert(seconds, partIx, code);
+    AlertBuf.push(alert);
+       
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+bool PushMeasurements(int partSize, RobotArm& robotArm)
+{
+    // space for getting measurements from arm
+    MeasureSnapshot measureSnap;
+    
+    float lastVals[NUMJOINTS];
+    measureSnap.Created = time(NULL);
+   
+    bool ok = true;
+
+    ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
+    DispMeasure("Temperatures", partSize, lastVals);
+    measureSnap.Temps.SetMeasure(partSize, lastVals);
+    
+    if (ok)
+    {
+        ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
+        DispMeasure("Voltage", partSize, lastVals);
+        measureSnap.Volts.SetMeasure(partSize, lastVals);
+    }
+    
+    if (ok)
+    {
+        ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
+        DispMeasure("Rotation", partSize, lastVals);
+        measureSnap.Positions.SetMeasure(partSize, lastVals);
+    }
+    
+    if (ok)
+    {
+        ok = robotArm.GetArmLastMeasure(NM_Load, lastVals);
+        DispMeasure("Load", partSize, lastVals);
+        measureSnap.Loads.SetMeasure(partSize, lastVals);
+    }
+    
+    if (ok)
+    {
+        MeasureBuf.push(measureSnap);
+    }
+    
+    return ok;
+}