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

Implemented the access to the expansion board analog ports and compass.

Verified the operation of the fall detection and the state machine to get up.
parent e67a920d
No related branches found
No related tags found
No related merge requests found
Pipeline #
......@@ -9,7 +9,8 @@ bioloid:
adc1_frame: IR1_link
adc3_frame: gyro_x
adc4_frame: gyro_y
adc6_frame: IR2_link
exp_adc1_frame: IR2_link
exp_board_id: 192
joints:
- j_shoulder_r
- j_shoulder_l
......
......@@ -86,9 +86,9 @@ catkin_package(
## Build ##
###########
SET(BIOLOID_FW_PATH /home/shernand/humanoids/bioloid_stm32_fw)
SET(STM32_HAL_PATH /home/shernand/humanoids/stm32_hal)
SET(STM32_LIBRARIES_PATH /home/shernand/humanoids/stm32_libraries)
SET(BIOLOID_FW_PATH ~/humanoids/bioloid_stm32_fw)
SET(STM32_HAL_PATH ~/humanoids/stm32_hal)
SET(STM32_LIBRARIES_PATH ~/humanoids/stm32_libraries)
## Specify additional locations of header files
## Your package locations should be listed before other locations
......
......@@ -4,7 +4,7 @@ project(bioloid_controller_cm510)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro sensor_msgs )
find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro sensor_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -81,7 +81,7 @@ FIND_PACKAGE(robotis_mtn_parser REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro sensor_msgs
CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro sensor_msgs tf
# DEPENDS system_lib
)
......@@ -89,7 +89,7 @@ catkin_package(
## Build ##
###########
SET(BIOLOID_FW_PATH /home/shernand/humanoids/cm510_controller_fw)
SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
## Specify additional locations of header files
## Your package locations should be listed before other locations
......@@ -124,7 +124,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
${BIOLOID_FW_PATH}/communications/src/dynamixel.c
${BIOLOID_FW_PATH}/communications/src/serial_console.c
# main application module
${BIOLOID_FW_PATH}/motion/src/examples/main.c
${BIOLOID_FW_PATH}/motion/src/examples/get_up.c
# AVR simulation modules
src/sim/avr_delay.c
src/sim/avr_registers.c
......@@ -132,6 +132,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
src/sim/motion_pages.c
src/sim/adc.c
src/sim/user_time.c
src/sim/exp_board_sim.c
include/bioloid_controller_cm510.h
include/bioloid_controller_cm510_impl.h)
......
......@@ -15,12 +15,15 @@
#include "buzzer.h"
#include "gpio.h"
#include "adc.h"
#include "exp_board_sim.h"
/* motion file parser files */
#include "mtn_file_parser.hpp"
#include "robotis_bin_parser.h"
#include "mtn_exceptions.h"
#include <tf/transform_datatypes.h>
/* external user functions */
extern "C" {
extern void user_init(void);
......@@ -52,6 +55,8 @@ extern "C" {
// expansion board external variables
extern int exp_board_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
namespace bioloid_controller_cm510
{
......@@ -289,19 +294,6 @@ namespace bioloid_controller_cm510
}catch(...){
ROS_WARN_STREAM("Unknown exception");
}
// initialize controller
init_leds();
init_buttons();
init_adc();
init_buzzer();
init_user_time();
/* call the user initialization function */
user_init();
manager_init(n_joints);
action_set_page(31);
action_start_page();
if(exp_board_id!=-1)
exp_board_init(exp_board_id);
return true;
}
......@@ -309,17 +301,25 @@ namespace bioloid_controller_cm510
template <class HardwareInterface>
void BioloidControllerCM510<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
{
static bool gyro_calibrated=false;
static bool first=true;
ros::Time current_time=ros::Time::now();
std::vector<double> target_angles(joints_.size());
if(this->found_gyro_x && this->found_gyro_y && !gyro_calibrated)
if(first)
{
gyro_calibrated=true;
ROS_INFO("Calibrating gyro ...");
balance_calibrate_gyro();
ROS_INFO("Gyro calibrated");
balance_enable_gyro();
// initialize controller
init_leds();
init_buttons();
init_adc();
exp_adc_init();
init_buzzer();
init_user_time();
/* call the user initialization function */
user_init();
manager_init(num_servos);
if(exp_board_id!=-1)
exp_board_init(exp_board_id);
first=false;
}
/* get the actual simulation angles */
......@@ -327,6 +327,7 @@ namespace bioloid_controller_cm510
real_angles[i]=joints_[i].getPosition();
pushbuttons_loop();
adc_loop();
exp_adc_loop();
buzzer_loop();
TIFR4|=(1<<OCF4A);
manager_loop();
......@@ -365,35 +366,65 @@ namespace bioloid_controller_cm510
adc_values[BALANCE_GYRO_X_CHANNEL]=512;
adc_values[BALANCE_GYRO_Y_CHANNEL]=512;
}
// process the compass information from the expansion board
exp_compass_value=(tf::getYaw(msg->orientation)*180.0/3.14159)*10.0;
}
template <class HardwareInterface>
void BioloidControllerCM510<HardwareInterface>::range_callback(const sensor_msgs::Range::ConstPtr& msg)
{
const double Vmax=3.3,Vmin=0.5;
unsigned int i=0;
unsigned short int value;
double B,C,voltage;
// compute the range value in volts
B=(100.0-(Vmax*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
for(i=0;i<NUM_ADC;i++)
{
const double Vmax=3.3,Vmin=0.5;
// compute the range value in volts
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
if(msg->header.frame_id==this->adc1_frame)
adc_values[0]=value;
adc_values[ADC_PORT_1]=value;
else if(msg->header.frame_id==this->adc2_frame)
adc_values[1]=value;
adc_values[ADC_PORT_2]=value;
else if(msg->header.frame_id==this->adc3_frame)
adc_values[2]=value;
adc_values[ADC_PORT_3]=value;
else if(msg->header.frame_id==this->adc4_frame)
adc_values[3]=value;
adc_values[ADC_PORT_4]=value;
else if(msg->header.frame_id==this->adc5_frame)
adc_values[4]=value;
adc_values[ADC_PORT_5]=value;
else if(msg->header.frame_id==this->adc6_frame)
adc_values[5]=value;
adc_values[ADC_PORT_6]=value;
}
if(exp_board_id!=-1)
{
const double Vmax=5.0,Vmin=0.5;
// compute the range value in volts
B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=(100.0/(B*msg->range+C));// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
for(i=0;i<EXP_NUM_ADC;i++)
{
if(msg->header.frame_id==this->exp_adc1_frame)
exp_adc_values[0]=value;
else if(msg->header.frame_id==this->exp_adc2_frame)
exp_adc_values[1]=value;
else if(msg->header.frame_id==this->exp_adc3_frame)
exp_adc_values[2]=value;
else if(msg->header.frame_id==this->exp_adc4_frame)
exp_adc_values[3]=value;
else if(msg->header.frame_id==this->exp_adc5_frame)
exp_adc_values[4]=value;
else if(msg->header.frame_id==this->exp_adc6_frame)
exp_adc_values[5]=value;
else if(msg->header.frame_id==this->exp_adc7_frame)
exp_adc_values[6]=value;
else if(msg->header.frame_id==this->exp_adc8_frame)
exp_adc_values[7]=value;
}
}
}
......@@ -457,8 +488,14 @@ namespace bioloid_controller_cm510
{
CRobotisStep step=robotis_page.get_step(i);
pages[page_id].steps[i].pause=step.get_pause_time()/0.0078;
pages[page_id].steps[i].time=step.get_step_time()/0.0078;
if((step.get_pause_time()/0.0078)>255)
pages[page_id].steps[i].pause=255;
else
pages[page_id].steps[i].pause=step.get_pause_time()/0.0078;
if((step.get_step_time()/0.0078)>255)
pages[page_id].steps[i].time=255;
else
pages[page_id].steps[i].time=step.get_step_time()/0.0078;
for(unsigned int j=0;j<this->motions.get_num_servos();j++)
pages[page_id].steps[i].position[j]=step.get_angle(j);
}
......@@ -523,6 +560,7 @@ namespace bioloid_controller_cm510
this->exp_adc1_frame="none";
nh.getParam("exp_adc1_frame", this->exp_adc1_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC1 associated to sensor: " << this->exp_adc1_frame);
std::cout << this->exp_adc1_frame << std::endl;
this->exp_adc2_frame="none";
nh.getParam("exp_adc2_frame", this->exp_adc2_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC2 associated to sensor: " << this->exp_adc2_frame);
......
#ifndef _EXP_BOARD_SIM_H
#define _EXP_BOARD_SIM_H
#ifdef __cplusplus
extern "C" {
#endif
#include <avr/io.h>
#define EXP_NUM_ADC 8
void exp_adc_init(void);
void exp_adc_loop(void);
#ifdef __cplusplus
}
#endif
#endif
......@@ -50,6 +50,7 @@
<build_depend>controller_manager</build_depend> <!--Tests-only!-->
<build_depend>xacro</build_depend> <!--Tests-only!-->
<build_depend>tf</build_depend> <!--Tests-only!-->
<run_depend>roscpp</run_depend>
<run_depend>urdf</run_depend>
......@@ -60,6 +61,7 @@
<run_depend>controller_manager</run_depend> <!--Tests-only!-->
<run_depend>xacro</run_depend> <!--Tests-only!-->
<run_depend>tf</run_depend> <!--Tests-only!-->
<export>
<controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
......
......@@ -58,10 +58,10 @@ void init_adc(void)
for(i=0;i<NUM_ADC;i++)
{
adc_values[i]=0x000;
adc_avg_values[i]=0x0000;
adc_values[i]=512;
adc_avg_values[i]=512;
for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
adc_ch_data[i][j]=0x0000;
adc_ch_data[i][j]=512;
}
adc_current_sample=0;
}
......
......@@ -4,11 +4,16 @@
#include "dyn_servos_reg.h"
#include "dyn_common.h"
#include "exp_board_reg.h"
#include "exp_board_sim.h"
/* external variables for the gazebo controller */
int num_servos;
double real_angles[PAGE_MAX_NUM_SERVOS];
extern int exp_board_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_adc_avg_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
extern uint16_t exp_compass_avg_value;
/* private variables */
uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
......@@ -152,14 +157,42 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
}
else if(id==exp_board_id)
{
/* if(address==ADC_CHANNEL0_L)
if(address==ADC_CHANNEL0_L)
*data=exp_adc_values[0];
else if(address==ADC_CHANNEL1_L)
*data=exp_adc_values[1];
else if(address==ADC_CHANNEL2_L)
*data=exp_adc_values[2];
else if(address==ADC_CHANNEL3_L)
*data=exp_adc_values[3];
else if(address==ADC_CHANNEL4_L)
*data=exp_adc_values[4];
else if(address==ADC_CHANNEL6_L)
*data=exp_adc_values[5];
else if(address==ADC_CHANNEL6_L)
else if(address==ADC_CHANNEL7_L)*/
*data=exp_adc_values[6];
else if(address==ADC_CHANNEL7_L)
*data=exp_adc_values[7];
if(address==ADC_AVG_CHANNEL0_L)
*data=exp_adc_avg_values[0];
else if(address==ADC_AVG_CHANNEL1_L)
*data=exp_adc_avg_values[1];
else if(address==ADC_AVG_CHANNEL2_L)
*data=exp_adc_avg_values[2];
else if(address==ADC_AVG_CHANNEL3_L)
*data=exp_adc_avg_values[3];
else if(address==ADC_AVG_CHANNEL4_L)
*data=exp_adc_avg_values[4];
else if(address==ADC_AVG_CHANNEL6_L)
*data=exp_adc_avg_values[5];
else if(address==ADC_AVG_CHANNEL6_L)
*data=exp_adc_avg_values[6];
else if(address==ADC_AVG_CHANNEL7_L)
*data=exp_adc_avg_values[7];
else if(address==COMPASS_HEADING_VAL_L)
*data=exp_compass_value;
else if(address==COMPASS_AVG_HEADING_VAL_L)
*data=exp_compass_avg_value;
}
return DYN_SUCCESS;
......
#include "cm510_cfg.h"
#include "exp_board_sim.h"
#include "adc.h"
#define EXP_BOARD_SIM_MAX_NUM_SAMPLES 16
uint16_t exp_adc_ch_data[EXP_NUM_ADC][EXP_BOARD_SIM_MAX_NUM_SAMPLES];
uint16_t exp_adc_values[EXP_NUM_ADC];
uint16_t exp_adc_avg_values[EXP_NUM_ADC];
uint8_t exp_adc_current_sample;
uint16_t exp_compass_value;
uint16_t exp_compass_avg_value;
uint8_t exp_compass_current_sample;
void exp_board_compass_avrg(uint16_t heading)
{
static unsigned long int new_average,first_value;
unsigned long int diff=0;
if(exp_compass_current_sample==0)
{
exp_compass_current_sample++;
first_value=(long int)heading;
}
else
{
diff=heading+3600-first_value;
if(diff<1800)
diff=diff+3600;
else if(diff>5400)
diff=diff-3600;
new_average=new_average+diff;
exp_compass_current_sample++;
if(exp_compass_current_sample==EXP_BOARD_SIM_MAX_NUM_SAMPLES)
{
new_average=new_average/(EXP_BOARD_SIM_MAX_NUM_SAMPLES-1);
new_average=new_average+first_value;
if(new_average>3600)
new_average-=3600;
exp_compass_avg_value=new_average;
new_average=0;
exp_compass_current_sample=0;
}
}
}
void exp_board_sim_init(void)
{
uint8_t i,j;
/* initialize the expansion board ADC data */
for(i=0;i<EXP_NUM_ADC;i++)
{
exp_adc_values[i]=512;
exp_adc_avg_values[i]=512;
for(j=0;j<EXP_BOARD_SIM_MAX_NUM_SAMPLES;j++)
exp_adc_ch_data[i][j]=512;
}
exp_adc_current_sample=0;
/* initialize the expansion board compass data */
exp_compass_value=0;
exp_compass_avg_value=0;
exp_compass_current_sample=0;
}
void exp_board_sim_loop(void)
{
uint8_t i,j;
uint32_t data;
for(i=0;i<EXP_NUM_ADC;i++)
exp_adc_ch_data[i][exp_adc_current_sample]=exp_adc_values[i];
// compute the average for the current channel
for(i=0;i<EXP_NUM_ADC;i++)
{
data=0;
for(j=0;j<EXP_BOARD_SIM_MAX_NUM_SAMPLES;j++)
data+=exp_adc_ch_data[i][j];
data=data/EXP_BOARD_SIM_MAX_NUM_SAMPLES;
exp_adc_avg_values[i]=data;
}
exp_adc_current_sample=(exp_adc_current_sample+1)%EXP_BOARD_SIM_MAX_NUM_SAMPLES;
exp_board_compass_avrg(exp_compass_value);
}
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