ControllerUtil.cpp

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 
00007 #include "RobotArm.h"
00008 #include "MeasureBuf.h"
00009 #include "Alert.h"
00010 #include "ControllerIo.h"
00011 #include "Timestamp.h"
00012 
00013 
00014 // use controller timer
00015 extern Timer IdleTimer;
00016 
00017 // use timestamp to get secs and ms
00018 Timestamp MessageTimer;
00019 
00020 
00021 // utility method to show state in console
00022 void DispMeasure(char* label, int partSize, float vals[])
00023 {
00024 //    printf("%s: ", label);
00025 //    for (int ix = 0; ix < partSize; ix++)
00026 //    {
00027 //        printf("%d:%f ", ix, vals[ix]);
00028 //    }
00029 //    printf("\r\n");
00030 }
00031 
00032 
00033 // send position alert
00034 void PushPositionAlert(RobotArm& robotArm)
00035 {
00036     // stop trying to move
00037     float diff = robotArm.GetLastPosDiff();
00038     int ix = robotArm.GetLastErrorPart();
00039     robotArm.MoveArmPositionsStop();
00040     
00041     // report load error
00042     printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
00043 
00044     Alert alert;
00045     MessageTimer.GetTimestamp();
00046     
00047     ShowLedRed();
00048     alert.SetPositionAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, diff);
00049     AlertBuf.push(alert);
00050     
00051     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00052 }
00053 
00054 // send load alert
00055 void PushLoadAlert(RobotArm& robotArm)
00056 {
00057     float lastVals[NUMJOINTS];
00058     
00059     // stop trying to move
00060     robotArm.GetArmLastMeasure(NM_Load, lastVals);
00061     int ix = robotArm.GetLastErrorPart();
00062     robotArm.MoveArmPositionsStop();
00063     
00064     // report load error
00065     printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
00066 
00067     Alert alert;
00068     MessageTimer.GetTimestamp();
00069     
00070     ShowLedRed();
00071     alert.SetLoadAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
00072     AlertBuf.push(alert);
00073     
00074     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00075 }
00076 
00077 // send temperature alert
00078 void PushTemperatureAlert(RobotArm& robotArm)
00079 {
00080     float lastVals[NUMJOINTS];
00081     
00082     // stop trying to move
00083     robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
00084     int ix = robotArm.GetLastErrorPart();
00085     robotArm.MoveArmPositionsStop();
00086     
00087     // report load error
00088     printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
00089 
00090     Alert alert;
00091     MessageTimer.GetTimestamp();
00092     
00093     ShowLedRed();
00094     alert.SetTemperatureAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
00095     AlertBuf.push(alert);
00096     
00097     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00098 }
00099 
00100 // send temperature alert
00101 void PushVoltageAlert(RobotArm& robotArm)
00102 {
00103     float lastVals[NUMJOINTS];
00104     
00105     // stop trying to move
00106     robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
00107     int ix = robotArm.GetLastErrorPart();
00108     robotArm.MoveArmPositionsStop();
00109     
00110     // report load error
00111     printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
00112 
00113     Alert alert;
00114     MessageTimer.GetTimestamp();
00115     
00116     ShowLedRed();
00117     alert.SetVoltageAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
00118     AlertBuf.push(alert);
00119     
00120     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00121 }
00122 
00123 
00124 // send hardware error alert
00125 void PushHardwareAlert(int partIx, int code)
00126 {
00127     Alert alert;
00128     MessageTimer.GetTimestamp();
00129     
00130     ShowLedRed();
00131     alert.SetHardwareAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), partIx, code);
00132     AlertBuf.push(alert);
00133        
00134     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00135 }
00136 
00137 bool PushMeasurements(int partSize, RobotArm& robotArm)
00138 {
00139     // space for getting measurements from arm
00140     MeasureSnapshot measureSnap;
00141     
00142     float lastVals[NUMJOINTS];
00143     MessageTimer.GetTimestamp();
00144     measureSnap.Created = MessageTimer.GetSecs();
00145     measureSnap.CreatedMs = MessageTimer.GetMs();
00146    
00147     bool ok = true;
00148 
00149     ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
00150     DispMeasure("Temperatures", partSize, lastVals);
00151     measureSnap.Temps.SetMeasure(partSize, lastVals);
00152     
00153     if (ok)
00154     {
00155         ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
00156         DispMeasure("Voltage", partSize, lastVals);
00157         measureSnap.Volts.SetMeasure(partSize, lastVals);
00158     }
00159     
00160     if (ok)
00161     {
00162         ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
00163         DispMeasure("Rotation", partSize, lastVals);
00164         measureSnap.Positions.SetMeasure(partSize, lastVals);
00165     }
00166     
00167     if (ok)
00168     {
00169         ok = robotArm.GetArmLastMeasure(NM_Load, lastVals);
00170         DispMeasure("Load", partSize, lastVals);
00171         measureSnap.Loads.SetMeasure(partSize, lastVals);
00172     }
00173     
00174     if (ok)
00175     {
00176         MeasureBuf.push(measureSnap);
00177     }
00178     
00179     return ok;
00180 }