testMotorTorqueController.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <string>
00003 #include <stdlib.h>
00004 #include "MotorTorqueController.h"
00005 
00006 #define ABS(x) (((x) < 0) ? (-(x)) : (x))
00007 
00008 int main (int argc, char* argv[]) {
00009   double ke = 2.0, kd = 20.0, tc = 0.05, dt = 0.005;
00010   const int test_num = 2;
00011   MotorTorqueController *controller[test_num];
00012   TwoDofController::TwoDofControllerParam tdc_param;
00013   tdc_param.ke = ke; tdc_param.tc = tc; tdc_param.dt = dt;
00014   controller[0] = new MotorTorqueController("hoge", tdc_param);
00015   TwoDofControllerPDModel::TwoDofControllerPDModelParam tdc_pdmodel_param;
00016   tdc_pdmodel_param.ke = ke; tdc_pdmodel_param.kd = kd; tdc_pdmodel_param.tc = tc; tdc_pdmodel_param.dt = dt;
00017   controller[1] = new MotorTorqueController("hoge", tdc_pdmodel_param);
00018   double q[test_num], dq[test_num], q_ref[test_num], tau[test_num], dqref[test_num], ddqref[test_num];
00019   double tau_d = 10.0, limit = 90.0, pgain = 1.0, dgain = 0.01;
00020   bool activate_flag[test_num], stop_flag[test_num];
00021 
00022   // initialize
00023   for (int i = 0; i < test_num; i++){
00024     q[i] = dq[i] = q_ref[i] = tau[i] = dqref[i] = ddqref[i] = 0;
00025     activate_flag[i] = stop_flag[i] = false;
00026   }
00027 
00028   double time = 0;
00029   while ( true ) {
00030     for (int i = 0; i < test_num; i++) {
00031       controller[i]->setReferenceTorque(tau_d);
00032 
00033       if (time > 5.0 && activate_flag[i] == false) {
00034         controller[i]->activate();
00035         std::cerr << "#activated[" << i << "]" << std::endl;
00036         activate_flag[i] = true;
00037       }
00038 
00039       // execute controller
00040       tau[i] = -ke * q[i] + kd * (ddqref[i] / dt);
00041       dqref[i] = controller[i]->execute(tau[i], limit); // execute returns dqRef = sum(dq)
00042       ddqref[i] = dqref[i] - q[i];
00043 
00044       // joint simulation
00045       q[i] = dqref[i];
00046 
00047       if (activate_flag[i] == true) {
00048         if (time > 20 && stop_flag[i] != true) {
00049           controller[i]->deactivate();
00050           stop_flag[i] = true;
00051         } else if (time > 10) {
00052           tau_d = 5.0;
00053         }
00054       }
00055       if (time > 30) {
00056         return 0;
00057       }
00058     }
00059     std::cout << time << " " << q[0] << " " << dqref[0] << " " << tau[0] << " " << q[1] << " " << dqref[1] << " " << tau[1] << " " << tau_d << std::endl;
00060     time += dt;
00061   }
00062   return 0;
00063 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56