Here is my Speed Control Example using VS-RC003 SDK.
It will allow you to set the goal position and its speed for multiple servos, and detect whenever it reaches to the goal position.
It only supports up to 19 servos for speed control without synchronized frame, and supports up to 18 servos for speed control with synchronized frame.
The example is only implemented for Head Yaw servo, but you should be able to add more servos easilly.
Motion files are already set for the use of multiple servo...
Please complete or read through previous examples before start working on this example.
1. Download SDK Example
Please download latest example from this link, and extract the file into your working directory.
2. Compile the Project
Now open the solution file from VC++ or VStudio and build the project.
3. Understand the Logic
In the motion file, it will do following process.
loop(infinite){
if (next frame is signaled){
if(currentPosition + incStepSize < goalPosition){
currentPosition += incStepSize
}else if (currentPosition){
currentPosition -= incStepSize
}else{
currentPosition = goalPosition
}
}
processFrame()
}
In the example application, it will do following.
updateGoalPosition(newGoalPosition)
loop(){
signalNextFrame()
}
4. Setup your robot
Connect your robot to your PC, and change the servo output parameter, so that it can support this example.
In the example, I'm using following variables to keep the values.
//user defined variable numbers
#define GAIN 64
#define FRAME 65
#define HEAD_YAW_CURRENT_POSITION 68
#define HEAD_YAW_GOAL_POSITION 69
#define HEAD_YAW_STEP_SIZE 70
In the preference, add one more calculation as following.
+ 1400 x Va*[64]/256 x Vb*[68]/256 Always enable
Then, upload the configuration to the CPU board.
5. Assign motion file
Assign motion file to the example application.
Open "MotionController.cpp" and check the line 41 and 55.
6. Set Goal Position
Open "main.cpp" and go to line 30 and 35.
7. Run the example
Click on start debugging and start the program.
Push Up or Down allow key, it will move the servo.
Then, upload the configuration to the CPU board.
5. Assign motion file
Assign motion file to the example application.
Open "MotionController.cpp" and check the line 41 and 55.
void MotionController::initVsrc003(){
m_vsrc003 = new Vsrc003("../Sample Motion/KTX-PC/initMotion.txt");
//initialize variables
//set all zeros to the variables will be used
m_vsrc003->SetValue(GAIN, 0);
m_vsrc003->SetValue(FRAME, 0);
m_vsrc003->SetValue(HEAD_YAW_CURRENT_POSITION, 0);
m_vsrc003->SetValue(HEAD_YAW_GOAL_POSITION, 0);
m_vsrc003->SetValue(HEAD_YAW_STEP_SIZE, 0);
//set default value for the gain
SetGain(DEFAULT_GAIN_VALUE);
m_vsrc003->LoadMotion("../Sample Motion/KTX-PC/RealTimeSpeedControl.txt");
//m_vsrc003->LoadMotion("../Sample Motion/KTX/MoveHead.txt");
m_vsrc003->ServoPowerOn();
m_vsrc003->PlayMotion(-1); //-1 for infinit loop
}
Make sure that you are using the motion files corresponds to your robot.
6. Set Goal Position
Open "main.cpp" and go to line 30 and 35.
void main(){
MotionController motionController = MotionController(true, true);
motionController.Startup();
cout << "\nPlease press ESC to stop playing motion";
bool loop = true;
do{
/**************************
In the motion file, there is a pose which has a step size 001 (0.016666 sec).
To maintain the accuracy of the synchronization,
use value greater than 16 for the sleep function.
**************************/
Sleep(16);
if(GetKeyState(VK_ESCAPE) & 0x80){
cout << "\nESC is recognized. Cancel playing motion.";
//motionController.CancelMotion();
loop = false;
}else if(GetKeyState(VK_DOWN) & 0x80){
motionController.ChangeStepSize(HEAD_YAW, 500);
Sleep(16);
motionController.ChangeGoalPosition(HEAD_YAW, -9000);
}else if(GetKeyState(VK_UP) & 0x80){
motionController.ChangeStepSize(HEAD_YAW, 100);
Sleep(16);
motionController.ChangeGoalPosition(HEAD_YAW, 9000);
}else{
motionController.MoveToNextFrame();
}
}while(loop);
motionController.Shutdown();
return;
}
Change the goal position to the value within the range your robot can support.
100 steps equals to 1 degree.
7. Run the example
Click on start debugging and start the program.
Push Up or Down allow key, it will move the servo.
No comments:
Post a Comment