- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Rtapi/ME/ME/meTest.cpp

    00001
    00002 //
    00003 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Organization:        Model-based Embedded and Robotic Systems Group
    00010 //                                      Massachusetts Institute of Technology
    00011 //
    00013
    00014 #include <cstdio>
    00015
    00016 //#include "../rtapi/livapi_rt.h"
    00017 #include <Mers/Executives/DeductiveController/Rtapi/ME/ME_rtapi.h>
    00018 #include <Mers/Executives/DeductiveController/Rtapi/ME/Stub/ME_callback_stub.h>
    00019 #include <Mers/Models/CCA/CCAModel.h>
    00020 #include <Mers/Estimation/Discrete/MEInterface.h>
    00021 #include <Mers/Estimation/Discrete/ModeEstimation/ModeEstimator.h>
    00022 #include <Mers/Models/CCA/Io/Mof/MOFReader.h>
    00023 #include <Mers/Utils/Lang/Sleep.h>
    00024
    00025 //#include "../rtapi/userfcns_debug.h"
    00026
    00028
    00029 //using Mers::Executives::DeductiveController::Rtapi::ME::ME;
    00030 using Mers::Executives::DeductiveController::Rtapi::ME::ME_rtapi;
    00031 using Mers::Models::CCA::Io::Mof::MOFReader;
    00032 using Mers::Models::CCA::CCAModel;
    00033 using Mers::Estimation::Discrete::ModeEstimation::ModeEstimator;
    00034 using Mers::Estimation::Discrete::MEInterface;
    00035 using Mers::Executives::DeductiveController::Rtapi::ME::Stub::ME_callback_stub;
    00036 using Mers::Utils::Lang::sleep;
    00037 using std::cout;
    00038 using std::cerr;
    00039 using std::endl;
    00040
    00042
    00043
    00044 /*void test_titanPropulsion_interface(ME_rtapi &me_rtapi)
    00045 {
    00046   printf("Testing the Titan Propulsion Module\n");
    00047
    00048   //mr_rtapi.queue_state_update(1, 1);
    00049   //mr_rtapi.queue_goal(2,1);
    00050   //mr_rtapi.queue_request_MR();
    00051   mr_rtapi.queue_state_update(0, 4);
    00052   mr_rtapi.queue_state_update(5, 4);
    00053   mr_rtapi.queue_state_update(10, 3);
    00054   mr_rtapi.queue_state_update(16, 2);
    00055
    00056   // give it the final goal
    00057   // the goal is that the driver is off and the valves are open and
    00058   // and the thruster is on
    00059   mr_rtapi.queue_goal(0, 3);
    00060   mr_rtapi.queue_goal(5, 3);
    00061   mr_rtapi.queue_goal(10, 2);
    00062   mr_rtapi.queue_goal(16, 2);
    00063
    00064   mr_rtapi.queue_request_MR();
    00065   //command (31, 0)
    00066
    00067   mr_rtapi.queue_state_update(16, 1);
    00068   mr_rtapi.queue_request_MR();
    00069   //command (29, 0)
    00070
    00071   mr_rtapi.queue_state_update(0, 3);
    00072   mr_rtapi.queue_request_MR();
    00073   // command should be (30, 0) indicating that valve1 should be
    00074
    00075   mr_rtapi.queue_state_update(5, 3);
    00076   mr_rtapi.queue_request_MR();
    00077   // command should be (28, 0) indicating that thruster should be
    00078
    00079   mr_rtapi.queue_state_update(10, 2);
    00080   mr_rtapi.queue_request_MR();
    00081   // command should be (31, 1) indicating that DCIU should be
    00082
    00083   mr_rtapi.queue_state_update(16, 2);
    00084   mr_rtapi.queue_request_MR();
    00085   // No command is required
    00086
    00087   // our new states indicate the driver is unknown and the thruster is off
    00088   //
    00089   // our desired goal of keeping the driver off and the valve closed can
    00090   // no longer be reached
    00091   // so RequestMR will return an error that the goal cannot be reached
    00092   // reversably
    00093   mr_rtapi.queue_state_update(10, 3);
    00094   mr_rtapi.queue_state_update(16, 0);
    00095   mr_rtapi.queue_request_MR();
    00096   // Failed
    00097
    00098   return;
    00099 }*/
    00100
    00101 void test_titanPropulsion_interface(ME_rtapi &me_rtapi)
    00102 {
    00103         me_rtapi.queue_observation(0, 4);
    00104         cout << "observe (0 = 4)" << endl;
    00105         me_rtapi.queue_observation(16,1);
    00106         cout << "observe (16 = 1)" << endl;
    00107         me_rtapi.queue_command(29,0);
    00108         cout << "observe (29 = 0)" << endl;
    00109         me_rtapi.queue_find_mode_estimate();
    00110 }
    00111
    00112 /*void test_MRP_interface(MR_rtapi &mr_rtapi)
    00113 {
    00114         printf("***Testing MR interface***\n");
    00115
    00116         mr_rtapi.queue_state_update(1, 1);
    00117         mr_rtapi.queue_state_update(2, 0);
    00118         //mr_rtapi.queue_state_update(3, 1);
    00119         mr_rtapi.queue_goal(1,1);
    00120         mr_rtapi.queue_goal(2,1);
    00121         mr_rtapi.queue_request_MR();
    00122         mr_rtapi.queue_state_update(1, 0);
    00123         mr_rtapi.queue_state_update(2, 0);
    00124         //mr_rtapi.queue_state_update(3, 1);
    00125         //mr_rtapi.queue_goal(1,1);
    00126         //mr_rtapi.queue_goal(2,1);
    00127         mr_rtapi.queue_request_MR();
    00128         mr_rtapi.queue_state_update(1, 0);
    00129         mr_rtapi.queue_state_update(2, 1);
    00130         //mr_rtapi.queue_state_update(3, 0);
    00131         //mr_rtapi.queue_goal(1,1);
    00132         //mr_rtapi.queue_goal(2,1);
    00133         mr_rtapi.queue_request_MR();
    00134         mr_rtapi.queue_state_update(1, 1);
    00135         mr_rtapi.queue_state_update(2, 1);
    00136         //mr_rtapi.queue_state_update(3, 0);
    00137         //mr_rtapi.queue_goal(1,1);
    00138         //mr_rtapi.queue_goal(2,1);
    00139         mr_rtapi.queue_request_MR();
    00140         mr_rtapi.queue_state_update(1,3);
    00141         mr_rtapi.queue_state_update(2,1);
    00142         //mr_rtapi.queue_state_update(3, 0);
    00143         //mr_rtapi.queue_goal(1,1);
    00144         //mr_rtapi.queue_goal(2,1);
    00145         mr_rtapi.queue_request_MR();
    00146
    00147         mr_rtapi.queue_state_update(1, 1);
    00148         mr_rtapi.queue_state_update(2, 0);
    00149         mr_rtapi.queue_goal(3, 1);
    00150         mr_rtapi.queue_request_MR();
    00151         return;
    00152 }*/
    00153
    00154 /*void test_RP_interface(MR_rtapi &mr_rtapi)
    00155 {
    00156         printf("***Testing MR interface***\n");
    00157
    00158         mr_rtapi.queue_state_update(1,1);
    00159         mr_rtapi.queue_state_update(2,0);
    00160         mr_rtapi.queue_goal(1,1);
    00161         mr_rtapi.queue_goal(2,1);
    00162         mr_rtapi.queue_request_MR();
    00163         mr_rtapi.queue_state_update(1,0);
    00164         mr_rtapi.queue_state_update(2,0);
    00165         mr_rtapi.queue_goal(1,1);
    00166         mr_rtapi.queue_goal(2,1);
    00167         mr_rtapi.queue_request_MR();
    00168         mr_rtapi.queue_state_update(1,0);
    00169         mr_rtapi.queue_state_update(2,1);
    00170         mr_rtapi.queue_goal(1,1);
    00171         mr_rtapi.queue_goal(2,1);
    00172         mr_rtapi.queue_request_MR();
    00173         mr_rtapi.queue_state_update(1,1);
    00174         mr_rtapi.queue_state_update(2,1);
    00175         mr_rtapi.queue_goal(1,1);
    00176         mr_rtapi.queue_goal(2,1);
    00177         mr_rtapi.queue_request_MR();
    00178         mr_rtapi.queue_state_update(1,3);
    00179         mr_rtapi.queue_state_update(2,1);
    00180         mr_rtapi.queue_goal(1,1);
    00181         mr_rtapi.queue_goal(2,1);
    00182         mr_rtapi.queue_request_MR();
    00183         return;
    00184 }*/
    00185
    00192 int main()
    00193 {
    00194
    00195   //-------------------------
    00196   // Creating Model
    00197   //-------------------------
    00198   //const char filename[] = "../Model/MOFReader/propulsion.mof";
    00199         const char filename[] = "../Model/MOFReader/propulsionMod.mof";
    00200
    00201   CCAModel my_model("Test Model");
    00202
    00203   //createValveDriverModel(my_model);
    00204   if(MOFReader::loadMOF(filename, my_model))
    00205   {
    00206     cerr << "Failed to load file " << filename << endl
    00207          << "Aborting" << endl;
    00208     return 1;
    00209   }
    00210   cout << "Loaded Model: " << filename << endl;
    00211
    00212   //cout << my_model << endl;
    00213
    00214   //-------------------
    00215   // Create rpCCAutomata for Reactive Planning
    00216   //--------------------
    00217   ModeEstimator * my_ME = new ModeEstimator(my_model);
    00218   //    cout << my_rpCCAutomata << endl;
    00219
    00220   //cout << "Creating mrInterface" << endl;
    00221
    00222   //-----------------------
    00223   // Create mrInterface
    00224   //-------------------------
    00225   MEInterface my_MEI(my_ME);
    00226
    00227   //cout << "Printing the Model" << endl;
    00228   cout << my_MEI << endl;
    00229   //cout << "Done" << endl;
    00230
    00231   ME_callback_stub *me_cbs;
    00232   ME_rtapi *me_rtapi;
    00233
    00234   //    mr = new MR();
    00235   me_cbs = new ME_callback_stub();
    00236   me_rtapi = new ME_rtapi(&my_MEI, me_cbs);
    00237
    00238   //test_MRP_interface(*mr_rtapi);
    00239   test_titanPropulsion_interface(*me_rtapi);
    00240
    00241   delete me_rtapi; // this will wait for the dispatcher thread to exit
    00242   delete me_cbs;
    00243
    00244
    00245 /*
    00246   Tracker * syst;
    00247   ReportTransitionsDebug * rtd;
    00248   L2_rtapi * rtapi;
    00249
    00250   syst = new Tracker();
    00251
    00252   if(!syst)
    00253   {
    00254     perror("failed to create a Tracker object\n");
    00255     return 1;
    00256   }
    00257
    00258   rtd = new ReportTransitionsDebug(syst);
    00259   if(!rtd)
    00260   {
    00261     delete syst;
    00262     perror("failed to create a reporter object\n");
    00263     return 1;
    00264   }
    00265   rtapi = new L2_rtapi(mr, syst, *rtd);
    00266   if(!rtapi)
    00267   {
    00268     delete syst;
    00269     delete rtd;
    00270     perror("failed to create a real time interface\n");
    00271     return 1;
    00272   }
    00273   testAPI(*rtapi);
    00274   delete rtapi;
    00275   delete rtd;
    00276   delete syst;
    00277 */
    00278
    00279
    00280   return 0;
    00281 }
    00282