File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

#include "mbed.h"
#include "rtos.h"

#include "RobotArm.h"
#include "MeasureBuf.h"
#include "Alert.h"
#include "ControllerIo.h"
#include "Timestamp.h"


// use controller timer
extern Timer IdleTimer;

// use timestamp to get secs and ms
Timestamp MessageTimer;


// 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;
    MessageTimer.GetTimestamp();
    
    ShowLedRed();
    alert.SetPositionAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), 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;
    MessageTimer.GetTimestamp();
    
    ShowLedRed();
    alert.SetLoadAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), 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;
    MessageTimer.GetTimestamp();
    
    ShowLedRed();
    alert.SetTemperatureAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), 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;
    MessageTimer.GetTimestamp();
    
    ShowLedRed();
    alert.SetVoltageAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
    AlertBuf.push(alert);
    
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}


// send hardware error alert
void PushHardwareAlert(int partIx, int code)
{
    Alert alert;
    MessageTimer.GetTimestamp();
    
    ShowLedRed();
    alert.SetHardwareAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), 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];
    MessageTimer.GetTimestamp();
    measureSnap.Created = MessageTimer.GetSecs();
    measureSnap.CreatedMs = MessageTimer.GetMs();
   
    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;
}