DASL Wiki

User Tools

Site Tools

Trace:
darwin_walk_backward_with_cv

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
darwin_walk_backward_with_cv [2017/11/27 08:18] – yuhanghedarwin_walk_backward_with_cv [2017/12/03 18:23] (current) – [Demonstration] yuhanghe
Line 29: Line 29:
 ===== Programming ===== ===== Programming =====
  
-Darwin OP 2's Walking class calculates motor positions for walking algorithm internally based on parameters. Most of these parameters can be edited through walking_tuner program. Here is a detailed [[http://support.robotis.com/en/product/darwin-op/development/tools/walking_tuner.htm | tutorial ]] from Robotis on how to use walking_tuner.+The BallFollower class use an algorithm that is similar to proportional controller to follow a target. Even though BallFollower class will not directly work with walking backward, the basic algorithm used in that class is applicable to backward motion. 
 \\ \\  \\ \\ 
-There are 3 parameter in walking_tuner that cannot be saved: ''Step forward/back (mm), Step right/left (mm), and Step Direction (degree)''. Instead, these variables should be updated constantly by your program during Darwin OP 2's walking process. +In the algorithm, Darwin OP 2's head pan angle is set as process variable(PV). The head pan position at 0, when Darwin OP 2 is facing directly forward, is set as setpoint(SP). The percentage difference between PV and SP is set as error. Finally, A_MOVE_AMPLITUDE is control variable(CV), output of algorithm.
 \\ \\  \\ \\ 
-Below is a sample program that demonstrates Darwin OP 2 walking backward. Place both ''main.cpp'' and ''Makefile'' inside a new folder in directory **''robotisop2/Linux/project/tutorial/''**. Then, type ''make'' command to compile  executable and type ''./walk_backward'' to execute program. +Below is a demonstration of the algorithm combining with backward motion.
  
 <code cpp | main.cpp> <code cpp | main.cpp>
-#include <stdio.h> +int m_FollowMaxRLTurn = 35.0, m_UnitRLTurn = 1.0, m_GoalRLTurn = 0, m_RLTurn = 0; 
-#include <unistd.h> +//initialize 4 variables to be used in the algorithm. m_FollowMaxRLTurn is used to set the maximum right or  
-#include <string.h> +//left turn amplitude. m_UnitRLTurn is used to set how much turn amplitude change at each iteration.  
-#include <libgen.h> +//m_GoalRLTurn is calculated based on error. m_RLTurn is the output.
-#include "LinuxDARwIn.h" +
- +
-#define INI_FILE_PATH       "../../../../Data/config.ini" +
-#define U2D_DEV_NAME        "/dev/ttyUSB0" +
- +
-using namespace Robot; +
- +
- +
-void change_current_dir() +
-{ +
-    char exepath[1024] = {0}; +
-    if(readlink("/proc/self/exe", exepath, sizeof(exepath)) != -1) +
-        chdir(dirname(exepath)); +
-} +
- +
-int main(void) +
-{ +
-    printf( "\n===== Walk Backward Tutorial for DARwIn =====\n\n"); +
- +
-    change_current_dir(); +
- +
-    minIni* ini = new minIni(INI_FILE_PATH); +
- +
- //////////////////// Framework Initialize //////////////////////////// +
- LinuxCM730 linux_cm730(U2D_DEV_NAME); +
- CM730 cm730(&linux_cm730); +
- if(MotionManager::GetInstance()->Initialize(&cm730) == false) +
- { +
- printf("Fail to initialize Motion Manager!\n"); +
- return 0; +
- } +
-        //Port initialization and opening, dynamixel power on +
-         +
-        MotionManager::GetInstance()->LoadINISettings(ini); +
-        Walking::GetInstance()->LoadINISettings(ini); +
-        //Load default settings for MotionManager and Walking module +
- +
- MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); +
- MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); +
-        LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance()); +
-        motion_timer->Start(); +
-        //Create MotionManager object and registers head and walking modules, then timers are initialized. +
- ///////////////////////////////////////////////////////////////////// +
- +
-        /////////////////////////Capture Motor Position////////////////////// +
- int n = 0; +
- int param[JointData::NUMBER_OF_JOINTS * 5]; +
- int wGoalPosition, wStartPosition, wDistance; +
- +
- for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++) +
- { +
- wStartPosition = MotionStatus::m_CurrentJoints.GetValue(id); +
- wGoalPosition = Walking::GetInstance()->m_Joint.GetValue(id); +
- if( wStartPosition > wGoalPosition ) +
- wDistance = wStartPosition - wGoalPosition; +
- else +
- wDistance = wGoalPosition - wStartPosition; +
- +
- wDistance >>= 2; +
- if( wDistance < 8 ) +
- wDistance = 8; +
- +
- param[n++] = id; +
- param[n++] = CM730::GetLowByte(wGoalPosition); +
- param[n++] = CM730::GetHighByte(wGoalPosition); +
- param[n++] = CM730::GetLowByte(wDistance); +
- param[n++] = CM730::GetHighByte(wDistance); +
- } +
- cm730.SyncWrite(MX28::P_GOAL_POSITION_L, 5, JointData::NUMBER_OF_JOINTS - 1, param); +
-        //Capture initial position of Darwin OP 2's motor position +
-         +
- printf("Press the ENTER key to begin!\n"); +
- getchar(); +
- +
-        Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); +
-        Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); +
- MotionManager::GetInstance()->SetEnable(true); +
-        //Walking and MotionManager enable motors+
  
-    while(1) +_marker_found = marker_tracker.SearchAndTracking(center); 
-    {+//The algorithm must work with BallTracker class since it depends on head's pan angle, which is used as PV.
  
-     if(Action::GetInstance()->IsRunning() == 0)+            if(Action::GetInstance()->IsRunning() == 0)
             {             {
  Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);  Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Line 130: Line 51:
                 if(Walking::GetInstance()->IsRunning() == false){                 if(Walking::GetInstance()->IsRunning() == false){
                 Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;                 Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0;
-                //X_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's walking step length,  
-                //each unit corresponds to approximately 1 mm. Set X_MOVE_AMPLITUDE to negative numbers to program  
-                //Darwin OP 2 to walk backwards. The Walking class calculates corresponding motor positions  
-                //internally 
-                 
                 Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;                 Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;
-                //A_MOVE_AMPLITUDE is a variable in Walking class that controls Darwin OP 2's yaw movement. Each  
-                //unit correspond approximately to 1 degree. Positive degrees for Darwin OP 2 to turn  
-                //counterclockwise, negative degree for clockwise.  
-                 
                 Walking::GetInstance()->Start();                 Walking::GetInstance()->Start();
-                //Start walking algorithm 
                 }                 }
- +                //Enable motors and initialize backward motion
- Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0; +
- Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0;+
                                  
-      }+                 
 +                if(_marker_found == 1) 
 +                { 
 +                    double pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN); 
 +                    //receiving pan angle of Darwin OP 2's head as process variable 
 +                     
 +                    double pan_range = Head::GetInstance()->GetLeftLimitAngle(); 
 +                    double pan_percent = pan / pan_range; 
 +                    //Calculate the error as a percentage 
  
-    }+                    m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent; 
 +                    //Convert error percentage to changes in control variable 
 +                     
 +                    if(m_RLTurn < m_GoalRLTurn) 
 +              m_RLTurn += m_UnitRLTurn; 
 +                  else if(m_RLTurn > m_GoalRLTurn) 
 +              m_RLTurn -= m_UnitRLTurn; 
 +                    //Instead of directly setting control variable as output, the algorithm use incremental changes  
 +                    //to provide smoother transition while walking 
 +                     
 +                  Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn; 
 +                    //A_MOVE_AMPLITUDE is set to the output of algorithm 
 +                     
 +                    Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0; 
 + 
 +                } 
 +                else 
 +                { 
 +                    Walking::GetInstance()->X_MOVE_AMPLITUDE = -15.0; 
 +                    Walking::GetInstance()->A_MOVE_AMPLITUDE = 0.0; 
 +                } 
 +             }
  
-    return 0; 
-} 
 </code> </code>
  
-<code make | Makefile> 
- 
-TARGET = walk_backward 
- 
-INCLUDE_DIRS = -I../../../include -I../../../../Framework/include 
- 
-CXX = g++ 
-CXXFLAGS += -O2 -DLINUX -Wall $(INCLUDE_DIRS) 
-#CXXFLAGS += -O2 -DDEBUG -DLINUX -Wall $(INCLUDE_DIRS) 
-LFLAGS += -lpthread -ljpeg -lrt 
- 
-OBJECTS =   main.o 
- 
-all: $(TARGET) 
- 
-clean: 
- rm -f *.a *.o $(TARGET) core *~ *.so *.lo 
- 
-libclean: 
- make -C ../../../build clean 
- 
-distclean: clean libclean 
- 
-darwin.a: 
- make -C ../../../build 
- 
-$(TARGET): darwin.a $(OBJECTS) 
- $(CXX) $(CFLAGS) $(OBJECTS) ../../../lib/darwin.a -o $(TARGET) $(LFLAGS) 
- chmod 755 $(TARGET) 
- 
-# useful to make a backup "make tgz" 
-tgz: clean 
- mkdir -p backups 
- tar czvf ./backups/ball_following_`date +"%Y_%m_%d_%H.%M.%S"`.tgz --exclude backups * 
-</code> 
 ===== Demonstration ===== ===== Demonstration =====
  
 In this demonstration, I successfully compile and execute the above programs In this demonstration, I successfully compile and execute the above programs
  
-{{ youtube>GQ8AyZpQ9NU?large }} +{{ youtube>v8hwcmquTac?large }}  
  
 +{{ youtube>SHYVUATcH5I?large }} 
 ===== Final Words ===== ===== Final Words =====
  
-This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk backward. +This tutorial's objective was to demonstrate how to program Darwin OP 2 to walk backward with CV. 
 \\ \\
 \\ \\
darwin_walk_backward_with_cv.1511799488.txt.gz · Last modified: 2017/11/27 08:18 by yuhanghe