File content as of revision 18:224289104fc0:

#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;
}