Skip to content
This repository was archived by the owner on Apr 24, 2023. It is now read-only.

Motor output from MBED #583

Open
oswinso wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Motor output from MBED #583

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c8bd1d4
Updated igvc.pb.c and igvc.pb.h with motor_output
oswinso Nov 6, 2019
db726ad
Merge branch 'master' into feat/motor_values_returned
oswinso Nov 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions igvc_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ add_message_files(
igvc_path.msg
trajectory_point.msg
trajectory.msg
motor_control.msg
)

# Generate added messages and services with any dependencies listed here
Expand Down
4 changes: 4 additions & 0 deletions igvc_msgs/msg/motor_control.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
Header header
uint8 left
uint8 right
duration duration
6 changes: 4 additions & 2 deletions igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/* Automatically generated nanopb constant definitions */
/* Generated by nanopb-0.4.0-dev */
/* Generated by nanopb-0.3.9.3 at Tue Nov 5 17:37:54 2019. */

#include "igvc.pb.h"

Expand All @@ -12,7 +12,7 @@ const float RequestMessage_speed_l_default = 0;
const float RequestMessage_speed_r_default = 0;


const pb_field_t ResponseMessage_fields[14] = {
const pb_field_t ResponseMessage_fields[16] = {
PB_FIELD( 1, FLOAT , OPTIONAL, STATIC , FIRST, ResponseMessage, p_l, p_l, 0),
PB_FIELD( 2, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, p_r, p_l, 0),
PB_FIELD( 3, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, i_l, p_r, 0),
Expand All @@ -26,6 +26,8 @@ const pb_field_t ResponseMessage_fields[14] = {
PB_FIELD( 11, BOOL , OPTIONAL, STATIC , OTHER, ResponseMessage, estop, voltage, 0),
PB_FIELD( 12, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, kv_l, estop, 0),
PB_FIELD( 13, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, kv_r, kv_l, 0),
PB_FIELD( 14, UINT32 , OPTIONAL, STATIC , OTHER, ResponseMessage, left_output, kv_r, 0),
PB_FIELD( 15, UINT32 , OPTIONAL, STATIC , OTHER, ResponseMessage, right_output, left_output, 0),
PB_LAST_FIELD
};

Expand Down
16 changes: 11 additions & 5 deletions igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/* Automatically generated nanopb header */
/* Generated by nanopb-0.4.0-dev */
/* Generated by nanopb-0.3.9.3 at Tue Nov 5 17:37:54 2019. */

#ifndef PB_IGVC_PB_H_INCLUDED
#define PB_IGVC_PB_H_INCLUDED
Expand Down Expand Up @@ -69,6 +69,10 @@ extern "C"
float kv_l;
bool has_kv_r;
float kv_r;
bool has_left_output;
uint32_t left_output;
bool has_right_output;
uint32_t right_output;
/* @@protoc_insertion_point(struct:ResponseMessage) */
} ResponseMessage;

Expand All @@ -80,7 +84,7 @@ extern "C"
#define ResponseMessage_init_default \
{ \
false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, \
false, 0, false, 0 \
false, 0, false, 0, false, 0, false, 0 \
}
#define RequestMessage_init_default \
{ \
Expand All @@ -89,7 +93,7 @@ extern "C"
#define ResponseMessage_init_zero \
{ \
false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, \
false, 0, false, 0 \
false, 0, false, 0, false, 0, false, 0 \
}
#define RequestMessage_init_zero \
{ \
Expand Down Expand Up @@ -120,13 +124,15 @@ extern "C"
#define ResponseMessage_estop_tag 11
#define ResponseMessage_kv_l_tag 12
#define ResponseMessage_kv_r_tag 13
#define ResponseMessage_left_output_tag 14
#define ResponseMessage_right_output_tag 15

/* Struct field encoding specification for nanopb */
extern const pb_field_t ResponseMessage_fields[14];
extern const pb_field_t ResponseMessage_fields[16];
extern const pb_field_t RequestMessage_fields[11];

/* Maximum encoded size of messages (where known) */
#define ResponseMessage_size 62
#define ResponseMessage_size 74
#define RequestMessage_size 50

/* Message IDs (where set with "msgid" option) */
Expand Down
10 changes: 10 additions & 0 deletions igvc_platform/src/motor_controller/motor_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <igvc_msgs/motor_control.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>

Expand All @@ -23,6 +24,7 @@ MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle)
enc_pub_ = nh_.advertise<igvc_msgs::velocity_pair>("/encoders", 1000);
enabled_pub_ = nh_.advertise<std_msgs::Bool>("/robot_enabled", 1);
battery_pub_ = nh_.advertise<std_msgs::Float64>("/battery", 1);
motor_output_pub_ = nh_.advertise<igvc_msgs::motor_control>("/motor_output", 10);

// get server ip address and port number from the launch file
assertions::getParam(pNh, std::string("ip_addr"), ip_addr_);
Expand Down Expand Up @@ -277,6 +279,14 @@ void MotorController::publishResponse(const ResponseMessage& response)
enc_msg.duration = response.dt_sec;
enc_msg.header.stamp = ros::Time::now() - ros::Duration(response.dt_sec);
enc_pub_.publish(enc_msg);

// Publish motor output
igvc_msgs::motor_control motor_control;
motor_control.left = static_cast<uint8_t>(response.left_output);
motor_control.right = static_cast<uint8_t>(response.right_output);
motor_control.duration = ros::Duration(response.dt_sec);
motor_control.header.stamp = ros::Time::now() - motor_control.duration;
Copy link
Contributor

@Dallas-D Dallas-D Jan 8, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure why you subtracted duration from the current time. What do you want the header time to represent?

motor_output_pub_.publish(motor_control);
}

int main(int argc, char** argv)
Expand Down
1 change: 1 addition & 0 deletions igvc_platform/src/motor_controller/motor_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ class MotorController
ros::Publisher enc_pub_;
ros::Publisher enabled_pub_;
ros::Publisher battery_pub_;
ros::Publisher motor_output_pub_;

/**
get current motor command from the /motors topic
Expand Down