From c7cfba37ed1294880a30f616c14491aaa3d734e9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Fri, 21 Aug 2015 17:33:48 +0000
Subject: [PATCH] Added the action module.
---
src/bioloid_robot.cpp | 13 +++--
src/bioloid_robot.h | 6 +--
src/examples/bioloid_robot_test.cpp | 84 ++++++++++++++++++++++++++++-
3 files changed, 92 insertions(+), 11 deletions(-)
diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index 5e64507..06da9f3 100644
--- a/src/bioloid_robot.cpp
+++ b/src/bioloid_robot.cpp
@@ -399,7 +399,7 @@ void CBioloidRobot::mm_set_period(double period_ms)
if(this->robot_device!=NULL)
{
// internal time in 0|16 fixed point float format in seconds
- period=(period_ms/1000.0)*65536;
+ period=(period_ms/1000.0)*65536;
this->robot_device->write_word_register(BIOLOID_MM_PERIOD_L,period);
}
else
@@ -524,7 +524,7 @@ void CBioloidRobot::mm_disable_power(void)
{
if(this->robot_device!=NULL)
{
- this->manager_status|=0xFB;
+ this->manager_status&=0xFB;
this->robot_device->write_byte_register(BIOLOID_MM_CNTRL,this->manager_status);
}
else
@@ -711,7 +711,7 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
}
// motion action interface
-/*void CBioloidRobot::action_load_page(unsigned char page_id)
+void CBioloidRobot::action_load_page(unsigned char page_id)
{
this->robot_device->write_byte_register(BIOLOID_ACTION_PAGE,page_id);
}
@@ -727,12 +727,12 @@ unsigned char CBioloidRobot::action_get_current_page(void)
void CBioloidRobot::action_start(void)
{
- this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x01);
+ this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,0x01);
}
void CBioloidRobot::action_stop(void)
{
- this->robot_device->write_byte_register(BIOLOID_ACTION_CONTROL,0x02);
+ this->robot_device->write_byte_register(BIOLOID_ACTION_CNTRL,0x02);
}
bool CBioloidRobot::action_is_page_running(void)
@@ -740,12 +740,11 @@ bool CBioloidRobot::action_is_page_running(void)
unsigned char status;
this->robot_device->read_byte_register(BIOLOID_ACTION_STATUS,&status);
- std::cout << std::hex << (int)status << std::endl;
if(status&0x01)
return true;
else
return false;
-}*/
+}
CBioloidRobot::~CBioloidRobot()
{
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index b10fbb9..d90094a 100644
--- a/src/bioloid_robot.h
+++ b/src/bioloid_robot.h
@@ -13,7 +13,7 @@
#define MAX_NUM_SERVOS 31
/* available motion modules */
-typedef enum {BIOLOD_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
+typedef enum {BIOLOID_MM_NONE=0x00,BIOLOID_MM_ACTION=0x01,BIOLOID_MM_WALKING=0x02,BIOLOID_MM_JOINTS=0x03} mm_mode_t;
/* available leds */
typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
/* available pushbuttons */
@@ -158,11 +158,11 @@ class CBioloidRobot
std::vector<double> mm_get_servo_angles(void);
double mm_get_servo_angle(unsigned char servo_id);
// motion action interface
-/* void action_load_page(unsigned char page_id);
+ void action_load_page(unsigned char page_id);
unsigned char action_get_current_page(void);
void action_start(void);
void action_stop(void);
- bool action_is_page_running(void);*/
+ bool action_is_page_running(void);
~CBioloidRobot();
};
diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp
index 6ab6d36..dbc692c 100644
--- a/src/examples/bioloid_robot_test.cpp
+++ b/src/examples/bioloid_robot_test.cpp
@@ -30,7 +30,8 @@ class CCallbacks
int main(int argc, char *argv[])
{
- unsigned int i=0;
+ std::vector<double> angles;
+ unsigned int i=0,j=0;
CCallbacks callbacks;
try{
@@ -78,7 +79,88 @@ int main(int argc, char *argv[])
/* tina.zigbee_enable_power();
sleep(3);
tina.zigbee_disable_power();*/
+/* std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
+ tina.mm_set_period(7.8);
+ std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
+ // check the state of all servos
+ for(i=0;i<=MAX_NUM_SERVOS;i++)
+ {
+ if(tina.mm_is_servo_enabled(i))
+ std::cout << "Servo " << i << " is enabled";
+ else
+ std::cout << "Servo " << i << " is disabled";
+ std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl;
+ }
+ // enable all servos and assign them to the ACTION MODULE
+ for(i=0;i<=MAX_NUM_SERVOS;i++)
+ {
+ tina.mm_disable_servo(i);
+ tina.mm_assign_module(i,BIOLOID_MM_NONE);
+ if(tina.mm_is_servo_enabled(i))
+ std::cout << "Servo " << i << " is enabled";
+ else
+ std::cout << "Servo " << i << " is disabled";
+ std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl;
+ }
+ tina.mm_start();
+ tina.mm_enable_power();
+ for(i=0;i<1000;i++)
+ {
+ // get the servos positions
+ angles=tina.mm_get_servo_angles();
+ for(j=0;j<=MAX_NUM_SERVOS;j++)
+ std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+ usleep(100000);
+ }
+ tina.mm_disable_power();
+ tina.mm_stop();*/
std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
+ for(i=0;i<=MAX_NUM_SERVOS;i++)
+ {
+ tina.mm_enable_servo(i);
+ tina.mm_assign_module(i,BIOLOID_MM_ACTION);
+ if(tina.mm_is_servo_enabled(i))
+ std::cout << "Servo " << i << " is enabled";
+ else
+ std::cout << "Servo " << i << " is disabled";
+ std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl;
+ }
+ tina.mm_start();
+ tina.mm_enable_power();
+ tina.action_load_page(31);
+ tina.action_start();
+ while(tina.action_is_page_running())
+ {
+ // get the servos positions
+ angles=tina.mm_get_servo_angles();
+ for(j=0;j<=MAX_NUM_SERVOS;j++)
+ std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+ usleep(100000);
+ }
+ sleep(1);
+ tina.action_load_page(25);
+ tina.action_start();
+ while(tina.action_is_page_running())
+ {
+ // get the servos positions
+ angles=tina.mm_get_servo_angles();
+ for(j=0;j<=MAX_NUM_SERVOS;j++)
+ std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+ usleep(100000);
+ }
+ sleep(1);
+ tina.action_load_page(26);
+ tina.action_start();
+ while(tina.action_is_page_running())
+ {
+ // get the servos positions
+ angles=tina.mm_get_servo_angles();
+ for(j=0;j<=MAX_NUM_SERVOS;j++)
+ std::cout << "Servo " << j << " angle: " << angles[j] << std::endl;
+ usleep(100000);
+ }
+ tina.mm_disable_power();
+ tina.mm_stop();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
--
GitLab