void mtsRobotIO1394QtWidget::Init(void) { DummyValueWhenNotConnected = 0; LastEnableState.SetSize(NumberOfActuators); LastEnableState.SetAll(false); UnitFactor.SetSize(NumberOfActuators); JointPosition.SetSize(NumberOfActuators); ActuatorPosition.SetSize(NumberOfActuators); ActuatorPositionGet.Position().SetSize(NumberOfActuators); ActuatorVelocity.SetSize(NumberOfActuators); PotentiometersVolts.SetSize(NumberOfActuators); PotentiometersPosition.SetSize(NumberOfActuators); ActuatorFeedbackCurrent.SetSize(NumberOfActuators); ActuatorFeedbackCurrent.Zeros(); ActuatorRequestedCurrent.SetSize(NumberOfActuators); ActuatorRequestedCurrent.Zeros(); ActuatorAmpTemperature.SetSize(NumberOfActuators); StartTime = osaGetTime(); SetupCisstInterface(); setupUi(); startTimer(TimerPeriodInMilliseconds); // ms }
int main( int argc, char** argv ){ mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "GroupTest", 99, 0 ); cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " rtcan[0-1]" << std::endl; return -1; } osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); if( can.Open() != osaCANBus::ESUCCESS ){ std::cerr << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } osaBH8_280 BH( &can ); if( BH.Initialize() != osaBH8_280::ESUCCESS ){ std::cerr << "Failed to initialize WAM" << std::endl; return -1; } std::cout << "\n\n\n\n"; double t1 = osaGetTime(); size_t cnt=0; while( 1 ){ Eigen::VectorXd q( 4, 0.1 ); BH.GetPositions( q ); //BH.SetPositions( q ); } cmnGetChar(); return 0; }
//#define _COMMUNICATION_TEST_ void mtsManagerProxyClient::ManagerClientI::Run() { #ifdef _COMMUNICATION_TEST_ int count = 0; while (IsActiveProxy()) { osaSleep(1 * cmn_s); std::cout << "\tClient [" << ManagerProxyClient->GetProxyName() << "] running (" << ++count << ")" << std::endl; std::stringstream ss; ss << "Msg " << count << " from Client " << ManagerProxyClient->GetProxyName(); ManagerProxyClient->SendTestMessageFromClientToServer(ss.str()); } #else double lastTickChecked = 0.0, now; while (IsActiveProxy()) { now = osaGetTime(); if (now < lastTickChecked + mtsProxyConfig::RefreshPeriodForManagers) { osaSleep(10 * cmn_ms); continue; } lastTickChecked = now; try { Server->Refresh(); } catch (const ::Ice::Exception & ex) { LogError(mtsManagerProxyClient, "refresh failed (" << Server->ice_toString() << ")" << std::endl << ex); if (ManagerProxyClient) { ManagerProxyClient->OnServerDisconnect(ex); } } } #endif #ifdef ENABLE_DETAILED_MESSAGE_EXCHANGE_LOG LogPrint(mtsManagerProxyClient, "mtsManagerProxyClient::ManagerClientI - terminated"); #endif }
int main( int argc, char** argv ){ cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl; return -1; } osaSocketCAN can( argv[1], osaCANBus::RATE_1000 ); if( can.Open() != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } osaBH8_280 BH( &can ); if( BH.Initialize() != osaBH8_280::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl; return -1; } std::cout << "\n\n\n\n"; double t1 = osaGetTime(); size_t cnt=0; while( 1 ){ vctDynamicVector<double> q( 4, 0.1 ); BH.GetPositions( q ); //BH.SetPositions( q ); } cmnGetChar(); return 0; }
void mtsMicroScribeDigitizer::Run(void) { // timestamp of last reconnect trial in case of disconnection static double lastReconnectTryTime = 0; ProcessQueuedCommands(); // If not connected, try reconnect at every 1 sec if (!DeviceConnected) { if (lastReconnectTryTime == 0) { lastReconnectTryTime = osaGetTime(); } else if (lastReconnectTryTime + 1.0 * cmn_s < osaGetTime()) { CMN_LOG_CLASS_INIT_VERBOSE << "Run: Try reconnecting to digitizer..." << std::endl; if (ARM_SUCCESS == ArmReconnect()) { OnDeviceConnection(); } else { CMN_LOG_CLASS_INIT_VERBOSE << "Run: Failed to reconnect to digitizer." << std::endl; } } return; } // read digitizer tip position if (ARM_NOT_CONNECTED == ArmGetTipPosition(&TipPositionVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer tip orientation if (ARM_NOT_CONNECTED == ArmGetTipOrientation(&TipOrientationVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer tip orientation unit vector if (ARM_NOT_CONNECTED == ArmGetTipOrientationUnitVector(&TipOrientationUnitVectorVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer button states if (ARM_NOT_CONNECTED == ArmGetButtonsState(&ButtonStateVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // read digitizer joint readings if (ARM_NOT_CONNECTED == ArmGetJointAngles(ARM_DEGREES, // or ARM_RADIANS JointReadingsVendor)) { // Disconnection detected OnDeviceDisconnection(); return; } // Convert vendor-defined container to cisst container // position TipPosition(X) = TipPositionVendor.x; TipPosition(Y) = TipPositionVendor.y; TipPosition(Z) = TipPositionVendor.z; // orientation TipOrientation(ROLL) = TipOrientationVendor.x; TipOrientation(PITCH) = TipOrientationVendor.y; TipOrientation(YAW) = TipOrientationVendor.z; // orientation unit vector TipOrientationUnitVector(ROLL) = TipOrientationUnitVectorVendor.x; TipOrientationUnitVector(PITCH) = TipOrientationUnitVectorVendor.y; TipOrientationUnitVector(YAW) = TipOrientationUnitVectorVendor.z; // button state static DWORD lastButtonStateVendor = 0; bool buttonStateChange = false; if (!(lastButtonStateVendor & ARM_BUTTON_1) && (ButtonStateVendor & ARM_BUTTON_1)) { EventButton1Down(); ButtonState(BUTTON_1) = DOWN; buttonStateChange = true; } else if ((lastButtonStateVendor & ARM_BUTTON_1) && !(ButtonStateVendor & ARM_BUTTON_1)) { EventButton1Up(); ButtonState(BUTTON_1) = UP; buttonStateChange = true; } if (!(lastButtonStateVendor & ARM_BUTTON_2) && (ButtonStateVendor & ARM_BUTTON_2)) { EventButton2Down(); ButtonState(BUTTON_2) = DOWN; buttonStateChange = true; } else if ((lastButtonStateVendor & ARM_BUTTON_2) && !(ButtonStateVendor & ARM_BUTTON_2)) { EventButton2Up(); ButtonState(BUTTON_2) = UP; buttonStateChange = true; } if (buttonStateChange) { lastButtonStateVendor = ButtonStateVendor; } // Joint readings for (size_t i = 0; i < NUM_DOF; ++i) { JointReadings(i) = JointReadingsVendor[i]; } }
int main( int argc, char** argv ){ #if (CISST_OS == CISST_LINUX_XENOMAI) mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "GroupTest", 99, 0 ); #endif cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl; return -1; } #if (CISST_OS == CISST_LINUX_XENOMAI) osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #else osaSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #endif if( can.Open() != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } osaWAM WAM( &can ); if( WAM.Initialize() != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl; return -1; } vctDynamicVector<double> qinit( 7, 0.0 ); qinit[1] = -cmnPI_2; qinit[3] = cmnPI; if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl; return -1; } cmnPath path; path.AddRelativeToCisstShare("/models/WAM"); std::string fname = path.Find("wam7.rob", cmnPath::READ); // Rotate the base vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0 ); vctFixedSizeVector<double,3> tw0(0.0); vctFrame4x4<double> Rtw0( Rw0, tw0 ); // Gain matrices vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0); Kp[0][0] = 250; Kd[0][0] = 3.0; Kp[1][1] = 250; Kd[1][1] = 3.0; Kp[2][2] = 250; Kd[2][2] = 3.0; Kp[3][3] = 200; Kd[3][3] = 3; Kp[4][4] = 50; Kd[4][4] = 0.8; Kp[5][5] = 50; Kd[5][5] = 0.8; Kp[6][6] = 10; Kd[6][6] = .1; osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit ); std::cout << "Activate the WAM" << std::endl; bool activated = false; double t1 = osaGetTime(); while( 1 ){ // Get the positions vctDynamicVector<double> q; if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl; return -1; } // Check if the pucks are activated if( !activated ) { osaWAM::Mode mode; if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl; return -1; } if( mode == osaWAM::MODE_ACTIVATED ) { activated = true; } } // if pucks are activated, run the controller vctDynamicVector<double> tau( q.size(), 0.0 ); double t2 = osaGetTime(); if( activated ){ if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl; return -1; } } t1 = t2; // apply torques if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl; return -1; } std::cout << "q: " << q << std::endl; std::cout << "tau: " << tau << std::endl; } return 0; }