4. Teach Mode Type Switching and Trajectory Playback

Before enabling drag mode via the host computer, drag identification must first be configured on the teach pendant.

Note: After switching to drag mode, you need to call the set_servo_poweron interface to enable the robot before dragging.

1. Switch Teach Mode Type Function

/*
 * Encapsulated teach mode type switching function
 */
void Switching_teach_type(int fd)
{
  int type;
  get_teach_type(fd, type);
  switch (type)
  {
  case 0:   // Current teach mode type is jog mode
    set_teach_type(fd, 1);   // Switch current teach mode type to drag mode
    break;
  
  case 1:   // Current teach mode type is drag mode
    set_teach_type(fd, 0);   // Switch current teach mode type to jog mode
    break;
  }


}

2. Encapsulate the Wait-for-Motion-End Function

/*
 * Loop-blocking motion end function
 */
void wait_for_running_over(int fd)
{
    // Wait for motion to complete
    int running_state = 0;
    get_robot_running_state(fd, running_state);  // Query whether the robot is moving, 2 = moving
    while (running_state == 2)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));  // Block for 500ms
        get_robot_running_state(fd, running_state);   // Query again
    }
}

3. Encapsulate the Trajectory Playback Function

/*
 * Encapsulated trajectory playback function
 */
void record_playback(int fd)
{
  int vel = 30;                                                     // Trajectory playback speed
  set_teach_type(fd, 0); 
  track_record_playback(fd, vel);                                   // Play back the recorded trajectory
  wait_for_running_over(fd);
}

4. Encapsulate the Drag Trajectory Recording Function

/*
 * Encapsulated drag trajectory recording function
 */
void track_record(int fd)
{
  set_teach_type(fd, 1);   // Switch current teach mode type to drag mode
  double maxSamplingNum = 200;     // Trajectory sampling interval
  double samplingInterval = 0.05;  // Maximum number of sampling points
  int vel = 30;                    // Trajectory playback speed
  track_record_delete(fd);         // Clear old trajectory before recording a new one
  track_record_start(fd, maxSamplingNum, samplingInterval);         // Start trajectory recording
  for(int i = 0; i < 50; i++)
  {
    sleep(0.2);     // .....Wait 10s for dragging to finish
  }
  track_record_stop(fd);                                            // Stop trajectory recording
}

5. Drag and Playback Functionality

#include <chrono>
#include <iostream>
#include <string>
#include <unistd.h>
#include <thread>


#include "cpp_interface/nrc_api.h"


using namespace std;


int main()
{
    SOCKETFD fd = connect_robot("192.168.1.15", "6001");  // Connect to the controller
    if (fd <= 0)
    {
        std::cout << "Connection failed" << std::endl;
        return 0;
    }
    std::cout << "Connection succeeded: "<< fd << std::endl;
    clear_error(fd);     // Clear errors
    track_record(fd);
    // record_playback(fd);   // Call this function if you need to play back the just-recorded trajectory


}