Skip to content
Snippets Groups Projects
Commit bb8b068b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Updated the example of the walk motion module.

parent 7a0afb92
No related branches found
No related tags found
No related merge requests found
......@@ -24,9 +24,9 @@ ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
# create an example application
#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
# link necessary libraries
#TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
# create an example application
#ADD_EXECUTABLE(darwin_stairs_test darwin_stairs_test.cpp)
......
#include "darwin_robot.h"
#include "darwin_mmanager.h"
#include "darwin_action.h"
#include "darwin_walk.h"
#include "darwin_balance.h"
#include "darwin_imu.h"
#include "darwin_robot_exceptions.h"
#include "action_id.h"
#include <iostream>
std::string robot_device="/dev/pts/20";
//std::string robot_device="A603LOBS";
//std::string robot_device="A4008atn";
std::string robot_device="/tmp/darwin_driver";
int main(int argc, char *argv[])
{
int i=0,num_servos;
std::vector<double> angles;
unsigned int present_servos;
try{
CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
num_servos=darwin.mm_get_num_servos();
present_servos=darwin.mm_get_present_servos();
std::cout << "Found " << num_servos << " servos" << std::endl;
std::cout << "Present servos: " << std::hex << "0x" << present_servos << std::dec << std::endl;
// enable all servos and assign them to the action module
darwin.mm_enable_power();
sleep(1);
for(i=0;i<MAX_NUM_SERVOS;i++)
CDarwinMManager darwin("Darwin",robot_device,1000000,0x01,true);
CDarwinAction action("Darwin",robot_device,1000000,0x01,true);
CDarwinIMU imu("Darwin",robot_device,1000000,0x01,true);
CDarwinBalance balance("Darwin",robot_device,1000000,0x01,true);
CDarwinWalk walk("Darwin",robot_device,1000000,0x01,true);
std::cout << "Manager period: " << darwin.get_base_period() << std::endl;
std::cout << "Number of modules: " << darwin.get_num_modules() << std::endl;
std::cout << "Number of masters: " << darwin.get_num_masters() << std::endl;
std::cout << "Motion manager period: " << darwin.get_period() << std::endl;
darwin.start_scan();
while(darwin.is_scanning())
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_ACTION);
std::cout << "scanning ..." << std::endl;
usleep(100000);
}
num_servos=darwin.get_num_devices();
std::cout << "num. devices: " << num_servos << std::endl;
present_servos=darwin.get_present_devices();
std::cout << "present devices: " << std::hex << present_servos << std::endl;
present_servos=darwin.get_present_servos();
std::cout << "present servos: " << std::hex << present_servos << std::endl;
i=0;
while(i<num_servos)
{
if(present_servos&(0x00000001<<i))
{
darwin.enable_servo(i);
darwin.assign_module(i,DARWIN_MM_ACTION);
}
i++;
}
darwin.mm_start();
// execute an action
darwin.action_load_page(WALK_READY);
darwin.action_start();
while(darwin.action_is_page_running())
darwin.start();
action.load_page(7);
action.start();
while(action.is_page_running())
{
std::cout << "action running ... " << std::endl;
usleep(100000);
std::cout << "starting calibration" << std::endl;
darwin.imu_start_gyro_cal();
while(darwin.imu_is_gyro_cal_done())
}
imu.start();
std::cout << "Gyroscope detected: " << imu.is_gyro_detected() << std::endl;
std::cout << "Accelerometer detected: " << imu.is_accel_detected() << std::endl;
imu.start_gyro_cal();
while(!imu.is_gyro_cal_done())
{
std::cout << "calibrating gyro ..." << std::endl;
usleep(100000);
}
std::cout << "calibration done" << std::endl;
darwin.mm_enable_balance();
for(i=0;i<MAX_NUM_SERVOS;i++)
balance.enable();
i=0;
while(i<num_servos)
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_WALKING);
if(present_servos&(0x00000001<<i))
{
darwin.enable_servo(i);
darwin.assign_module(i,DARWIN_MM_WALKING);
}
i++;
}
std::cout << "Start walking ..." << std::endl;
darwin.walk_set_x_step(0.0);
darwin.walk_set_y_step(0.0);
darwin.walk_set_turn_step(0.04);
darwin.walk_start();
walk.set_x_step(0.04);
walk.set_y_step(0.0);
walk.set_turn_step(0.0);
walk.start();
sleep(10);
std::cout << "Stop walking ..." << std::endl;
darwin.walk_stop();
while(darwin.is_walking())
{
std::cout << "Walking ..." << std::endl;
usleep(100000);
}
darwin.walk_set_x_step(0.0);
darwin.walk_set_y_step(0.0);
darwin.walk_set_turn_step(0.01);
darwin.walk_start();
sleep(2);
std::cout << "Stop walking ..." << std::endl;
darwin.walk_stop();
while(darwin.is_walking())
walk.stop();
while(walk.is_walking())
{
std::cout << "Walking ..." << std::endl;
usleep(100000);
}
darwin.mm_stop();
darwin.mm_disable_power();
balance.disable();
imu.stop();
darwin.stop();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment