Commit 804b46a1 authored by Svenja Kenneweg's avatar Svenja Kenneweg
Browse files

Removed uncommited code and added the motorControl app to the AMiRoDefault configuration

parent 5908e533
......@@ -187,9 +187,6 @@ urt_osEventMask_t _dmcSetup(urt_node_t* node, void* dmc)
// kind of a hack to save memory
urt_topic_t* const topic = ((dmc_t*)dmc)->subscriber_motion.topic;
((dmc_t*)dmc)->subscriber_motion.topic = NULL;
// urtHrtSubscriberSubscribe(&((dmc_t*)dmc)->subscriber_motion, topic, MOTIONDATAEVENT, NULL,
// MICROSECONDS_PER_MILLISECOND, MICROSECONDS_PER_MILLISECOND, 0);
urtNrtSubscriberSubscribe(&((dmc_t*)dmc)->subscriber_motion, topic, MOTIONDATAEVENT, NULL);
}
......@@ -225,9 +222,6 @@ urt_osEventMask_t _dmcLoop(urt_node_t* node, urt_osEventMask_t event, void* dmc)
// fetch latest motion message
{
dmc_motionpayload_t m;
// if (urtHrtSubscriberFetchLatestMessage(&((dmc_t*)dmc)->subscriber_motion, &m, sizeof(m), &motion_time, NULL) != URT_STATUS_OK) {
// break;
// }
if (urtNrtSubscriberFetchLatestMessage(&((dmc_t*)dmc)->subscriber_motion,
&m,
sizeof(m),
......@@ -377,7 +371,6 @@ void _dmcShutdown(urt_node_t* node, urt_status_t reason, void* dmc)
(void)reason;
// unsubscribe from motion topic
// urtHrtSubscriberUnsubscribe(&((dmc_t*)dmc)->subscriber_motion);
urtNrtSubscriberUnsubscribe(&((dmc_t*)dmc)->subscriber_motion);
......@@ -445,7 +438,6 @@ void _dmcShutdown(urt_node_t* node, urt_status_t reason, void* dmc)
_dmcShutdown, dmc);
// initialize subscriber
// urtHrtSubscriberInit(&dmc->subscriber_motion);
urtNrtSubscriberInit(&dmc->subscriber_motion);
dmc->subscriber_motion.topic = urtCoreGetTopic(motiontopic); // kind of a hack to save memory
......
......@@ -231,7 +231,6 @@ typedef struct dmc {
/**
* @brief Subscriber to receive motion information.
*/
// urt_hrtsubscriber_t subscriber_motion;
urt_nrtsubscriber_t subscriber_motion;
dmc_motionpayload_t motion;
......
......@@ -26,6 +26,8 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <amiroos.h>
#include <differentialmotionestimator.h>
#include <differentialmotorcontrol.h>
#include <stdlib.h>
#include <string.h>
/******************************************************************************/
/* LOCAL DEFINITIONS */
......@@ -47,20 +49,25 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#define WHEEL_OFFSET (34000 + (9000/2))
/**
* @brief Frequency of the DME.
*/
#define DME_FREQUENCY 100
/**
* @brief Topic ID for DME motion information.
*/
#define DME_TOPIC_ID 1
#define DME_TOPIC_ID 5
/**
* @brief Service ID for setting DMC target information.
*/
#define DMC_SERVICE_ID 1
#define DMC_TARGETSERVICE_ID 7
/**
* @brief Frequency of the DME.
* @brief Service ID for executing DMC auto calibration.
*/
#define DME_FREQUENCY 100
#define DMC_CALIBSERVICE_ID 8
/******************************************************************************/
/* EXPORTED VARIABLES */
......@@ -74,6 +81,18 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
/* LOCAL VARIABLES */
/******************************************************************************/
/*
* forward declarations
*/
static int _appsDmcShellCmdCb(BaseSequentialStream* stream, int argc, char* argv[]);
/**
* @brief DMC shell command.
*/
static AOS_SHELL_COMMAND(_appsDmcSehhCmd, "DMC:setVelocity", _appsDmcShellCmdCb);
static urt_topic_t _dme_motion_topic;
static dme_motionpayload_t _dme_motion_topic_payload;
/**
* @brief DME instance.
*/
......@@ -102,16 +121,6 @@ static const dme_config_t _dme_config = {
.interval = MICROSECONDS_PER_SECOND / DME_FREQUENCY,
};
/**
* @brief DME motion topic.
*/
static urt_topic_t _dme_motion_topic;
/**
* @brief Payload for the DME motion topic mandatory message.
*/
static dme_motionpayload_t _dme_motion_topic_payload;
/**
* @brief DMC instance.
*/
......@@ -144,18 +153,21 @@ static const dmc_config_t _dmc_config = {
},
},
.lpf = {
.factor = 1e-6,
.factor = 10.0f,
.max = {
.translation = 0.0f,
.rotation = 0.0f,
.steering = 0.0f,
.left = 0.0f,
.right = 0.0f,
},
},
};
/******************************************************************************/
/* LOCAL FUNCTIONS */
/******************************************************************************/
#if (AMIROOS_CFG_SHELL_ENABLE == true) || defined(__DOXYGEN__)
/**
......@@ -169,13 +181,46 @@ static const dmc_config_t _dmc_config = {
*/
static int _appsDmcShellCmdCb(BaseSequentialStream* stream, int argc, char* argv[])
{
return dmcShellCallback(stream, argc, argv, urtCoreGetService(DMC_SERVICE_ID));
return dmcShellCallback_setVelocity(stream, argc, argv, urtCoreGetService(DMC_TARGETSERVICE_ID));
}
/**
* @brief DMC shell command.
* @brief DMC get gains shell command callback.
*
* @param[in] stream Shell I/O stream.
* @param[in] argc Number of arguments.
* @param[in] argv Argument list.
*
* @return The operation result.
*/
static AOS_SHELL_COMMAND(_appsDmcSehhCmd, "DMC:setVelocity", _appsDmcShellCmdCb);
static int _appsDmcShellCmdCb_getGains(BaseSequentialStream* stream, int argc, char* argv[])
{
return dmcShellCallback_getGains(stream, argc, argv, &_dmc);
}
/**
* @brief DMC set gains shell command callback.
*
* @param[in] stream Shell I/O stream.
* @param[in] argc Number of arguments.
* @param[in] argv Argument list.
*
* @return The operation result.
*/
static int _appsDmcShellCmdCb_setGains(BaseSequentialStream* stream, int argc, char* argv[])
{
return dmcShellCallback_setGains(stream, argc, argv, &_dmc);
}
/**
* @brief DMC set gains shell command.
*/
static AOS_SHELL_COMMAND(_appsDmcShellCmd_setGains, "DMC:setGains", _appsDmcShellCmdCb_setGains);
/**
* @brief DMC get gains shell command.
*/
static AOS_SHELL_COMMAND(_appsDmcShellCmd_getGains, "DMC:getGains", _appsDmcShellCmdCb_getGains);
#endif /* (AMIROOS_CFG_SHELL_ENABLE == true) || defined(__DOXYGEN__) */
......@@ -184,21 +229,27 @@ static AOS_SHELL_COMMAND(_appsDmcSehhCmd, "DMC:setVelocity", _appsDmcShellCmdCb)
/******************************************************************************/
/**
* @brief Initializes all data applications for the AMiRo default configuration.
* @brief Initializes all Apps from the PowerManagement.
*/
void appsInit(void)
{
// initialize DME motion topic and DME
urtTopicInit(&_dme_motion_topic, DME_TOPIC_ID, &_dme_motion_topic_payload);
dmeInit(&_dme, &_dme_config, DME_TOPIC_ID, URT_THREAD_PRIO_RT_MAX);
// initialize DME
dmeInit(&_dme, &_dme_config, DME_TOPIC_ID, URT_THREAD_PRIO_RT_MAX);
// initialize DMC
dmcInit(&_dmc, &_dmc_config, DME_TOPIC_ID, DMC_SERVICE_ID, URT_THREAD_PRIO_RT_MAX);
#if (DMC_CALIBRATION_ENABLE == true)
dmcInit(&_dmc, &_dmc_config, DME_TOPIC_ID, DMC_TARGETSERVICE_ID, DMC_CALIBSERVICE_ID, URT_THREAD_PRIO_RT_MAX);
#else
dmcInit(&_dmc, &_dmc_config, DME_TOPIC_ID, DMC_TARGETSERVICE_ID, URT_THREAD_PRIO_RT_MAX);
#endif
#if (AMIROOS_CFG_SHELL_ENABLE == true) || defined(__DOXYGEN__)
// add DMC shell command
// add shell commands
aosShellAddCommand(&_appsDmcSehhCmd);
aosShellAddCommand(&_appsDmcShellCmd_getGains);
aosShellAddCommand(&_appsDmcShellCmd_setGains);
#endif /* (AMIROOS_CFG_SHELL_ENABLE == true) || defined(__DOXYGEN__) */
return;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment