Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[82] trunk/rcssserver3d
|
From: <he...@us...> - 2009-07-31 20:40:34
|
Revision: 82
http://simspark.svn.sourceforge.net/simspark/?rev=82&view=rev
Author: hedayat
Date: 2009-07-31 20:40:14 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Added the HMDP plugin (By: N. Michael Mayer and Joschka)
Modified Paths:
--------------
trunk/rcssserver3d/ChangeLog
trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg
trunk/rcssserver3d/plugin/soccer/CMakeLists.txt
trunk/rcssserver3d/plugin/soccer/export.cpp
Added Paths:
-----------
trunk/rcssserver3d/plugin/soccer/hmdp_effector/
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.c
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/compatible.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/sine_fixed.c
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/sine_fixed.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpaction.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpeffector_c.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor.h
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpperceptor_c.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdpwrapper.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/naospecific.cpp
trunk/rcssserver3d/plugin/soccer/hmdp_effector/naospecific.h
Modified: trunk/rcssserver3d/ChangeLog
===================================================================
--- trunk/rcssserver3d/ChangeLog 2009-07-31 20:27:41 UTC (rev 81)
+++ trunk/rcssserver3d/ChangeLog 2009-07-31 20:40:14 UTC (rev 82)
@@ -1,3 +1,29 @@
+2009-07-31 Hedayat Vatankhah <he...@gr...>
+
+ * plugin/soccer/hmdp_effector/hmdp_c/sine_fixed.h:
+ * plugin/soccer/hmdp_effector/hmdp_c/sine_fixed.c:
+ * plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.h:
+ * plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c:
+ * plugin/soccer/hmdp_effector/hmdp_c/compatible.h:
+ * plugin/soccer/hmdp_effector/hmdp_c/base.h:
+ * plugin/soccer/hmdp_effector/hmdp_c/base.c:
+ * plugin/soccer/hmdp_effector/naospecific.h:
+ * plugin/soccer/hmdp_effector/naospecific.cpp:
+ * plugin/soccer/hmdp_effector/hmdpwrapper.cpp:
+ * plugin/soccer/hmdp_effector/hmdpperceptor.h:
+ * plugin/soccer/hmdp_effector/hmdpperceptor.cpp:
+ * plugin/soccer/hmdp_effector/hmdpperceptor_c.cpp:
+ * plugin/soccer/hmdp_effector/hmdpeffector.h:
+ * plugin/soccer/hmdp_effector/hmdpeffector.cpp:
+ * plugin/soccer/hmdp_effector/hmdpeffector_c.cpp:
+ * plugin/soccer/hmdp_effector/hmdpaction.h:
+ * plugin/soccer/CMakeLists.txt:
+ * plugin/soccer/export.cpp:
+ * data/rsg/agent/nao/nao.rsg:
+ - added HMDP effector/perceptor. Thanks to N. Michael Mayer and Joschka.
+ - some code cleanup and formatting
+ - replacing the use of the obsolete function boost::make_shared()
+
2009-07-30 Hedayat Vatankhah <he...@gr...>
* plugin/CMakeLists.txt:
Modified: trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg
===================================================================
--- trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg 2009-07-31 20:27:41 UTC (rev 81)
+++ trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg 2009-07-31 20:40:14 UTC (rev 82)
@@ -37,14 +37,14 @@
(switch $loadObj
(true
- (nd Transform
- (setLocalRotation -90 0 180)
- (nd StaticMesh
- (load 'models/naobody.obj')
- (setScale $TorsoLength $TorsoLength $TorsoLength)
- )
- )
+ (nd Transform
+ (setLocalRotation -90 0 180)
+ (nd StaticMesh
+ (load 'models/naobody.obj')
+ (setScale $TorsoLength $TorsoLength $TorsoLength)
+ )
)
+ )
(false
(importScene rsg/agent/nao/box_appearance.rsg $TorsoLength $TorsoWidth $TorsoHeight matGrey)
@@ -59,10 +59,10 @@
(importScene rsg/agent/nao/contactjointhandler.rsg)
)
- ;Install effectors and perceptors
+ ;Install effectors and perceptors
(nd StaticMeshInitEffector)
- (nd TimePerceptor)
+ (nd TimePerceptor)
(nd AgentState
(setName AgentState)
@@ -72,11 +72,14 @@
(nd GyroRatePerceptor (setName torso))
- (nd BeamEffector)
+ ;(nd HMDPPerceptor)
+ ;(nd HMDPEffector)
+ (nd BeamEffector)
+
(nd SayEffector)
- ;(nd VisionPerceptor
+ ;(nd VisionPerceptor
; (setSenseMyPos false)
; (setStaticSenseAxis false)
; (addNoise false))
Modified: trunk/rcssserver3d/plugin/soccer/CMakeLists.txt
===================================================================
--- trunk/rcssserver3d/plugin/soccer/CMakeLists.txt 2009-07-31 20:27:41 UTC (rev 81)
+++ trunk/rcssserver3d/plugin/soccer/CMakeLists.txt 2009-07-31 20:40:14 UTC (rev 82)
@@ -41,6 +41,10 @@
gametimeperceptor/gametimeperceptor.h
visionperceptor/visionperceptor.h
agentintegration/soccerbotbehavior.h
+ hmdp_effector/hmdpaction.h
+ hmdp_effector/hmdpeffector.h
+ hmdp_effector/hmdpperceptor.h
+ hmdp_effector/naospecific.h
)
set(soccer_LIB_SRCS
@@ -107,6 +111,12 @@
gametimeperceptor/gametimeperceptor_c.cpp
agentintegration/soccerbotbehavior.cpp
agentintegration/soccerbotbehavior_c.cpp
+ hmdp_effector/hmdpeffector_c.cpp
+ hmdp_effector/hmdpeffector.cpp
+ hmdp_effector/hmdpperceptor_c.cpp
+ hmdp_effector/hmdpperceptor.cpp
+ hmdp_effector/hmdpwrapper.cpp
+ hmdp_effector/naospecific.cpp
)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${SPARK_INCLUDE_DIR}
Modified: trunk/rcssserver3d/plugin/soccer/export.cpp
===================================================================
--- trunk/rcssserver3d/plugin/soccer/export.cpp 2009-07-31 20:27:41 UTC (rev 81)
+++ trunk/rcssserver3d/plugin/soccer/export.cpp 2009-07-31 20:40:14 UTC (rev 82)
@@ -51,6 +51,8 @@
#include "hearperceptor/hearperceptor.h"
#include "gametimeperceptor/gametimeperceptor.h"
#include "agentintegration/soccerbotbehavior.h"
+#include "hmdp_effector/hmdpeffector.h"
+#include "hmdp_effector/hmdpperceptor.h"
ZEITGEIST_EXPORT_BEGIN()
ZEITGEIST_EXPORT(SoccerControlAspect);
@@ -84,5 +86,7 @@
ZEITGEIST_EXPORT(VisionPerceptor);
ZEITGEIST_EXPORT(GameTimePerceptor);
ZEITGEIST_EXPORT(SoccerbotBehavior);
+ ZEITGEIST_EXPORT(HMDPPerceptor);
+ ZEITGEIST_EXPORT(HMDPEffector);
ZEITGEIST_EXPORT_END()
Added: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.c
===================================================================
--- trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.c (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.c 2009-07-31 20:40:14 UTC (rev 82)
@@ -0,0 +1,636 @@
+/***************************************************************************
+ * Base routines of the command parser (without the HMDP commands) *
+ * Part of the Microcontroller Basimplementation of HMDP *
+ * N. Michael Mayer, 2007 email: nm...@gm... *
+ * This part of the program is intended to be used in the micro-controller *
+ ***************************************************************************/
+
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2002,2003 Koblenz University
+ Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
+ Copyright (C) 2008 N. Michael Mayer, email: nm...@gm...
+
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+//
+
+#include "base.h"
+#include "hmdp_c.h"
+#include "compatible.h"
+
+Base_data *base_data; // pointer to current instantiation of the data
+
+
+void sendMesg(const char *str) //! uses sendByte to make messages...
+{
+ while (*str)
+ {
+ sendByte(*str);
+ str++;
+ }
+}
+
+void write_int(int in) //! write out integer to master
+{
+ char xx[] =
+ { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+ data2hex(8, in, xx);
+ sendMesg(xx);
+}
+
+void set_current_servo_pos_as_zero()
+{
+ int i;
+ init_servo_list();
+ for (i = 1; i <= base_data->servo_list_base[0]; ++i)
+
+ base_data->zero_pos_inits_feed[base_data->servo_list_base[i]]
+ = read_back_pos_set(base_data->servo_list_base[i]);
+ base_data->zero_pos_inits = &(base_data->zero_pos_inits_feed[0]);
+ sendMesg("Current zero pos is now zero\r\n");
+}
+
+void init_base()
+{
+ base_data->op_state = 0;
+ base_data->echo_on = 0;
+ base_data->bbase = 0;
+ init_servo_list(); /*! search for all servos */
+ //init_hmdl(); /*! in arm.c */
+ sendMesg("# HMDP 0.3 \n");
+}
+
+int servo_list_in_hex(char *li)
+{
+
+ init_servo_list();
+ int i;
+ for (i = 0; i <= base_data->servo_list_base[0]; ++i)
+ {
+ data2hex(2, base_data->servo_list_base[i], &li[2* i ]);
+ }
+ li[base_data->servo_list_base[0] * 2 + 2] = 0;
+ return 0;
+}
+
+void send_servo_list()
+{
+ char list[200];
+ servo_list_in_hex(list);
+
+ sendByte('!'); //!Data indikator
+
+ sendMesg(list);
+ sendByte(13);
+ sendByte(10);
+}
+
+int init_servo_list() //!gives back
+//!(size, id, id...)
+{
+
+ char i;
+ int pos = 1;
+ for (i = 0; i < 62; ++i)
+ {
+ if (read_back_id(i))
+ {
+ base_data->servo_list_base[pos] = i;
+ pos++;
+ }
+ }
+ base_data->servo_list_base[0] = pos - 1;
+ return 0;
+}
+;
+
+void eval_set_echo_on_off(const char in[])
+/*! >E command one can turn on/off the echo */
+{
+ if (in[1] == 'N')
+ base_data->echo_on = 0;
+ if (in[1] == 'F')
+ base_data->echo_on = -1;
+
+}
+;
+
+void eval_send_gen_message(const char in[])
+/*! >$ command allows direct access to the servos */
+{
+ int si = hex2data(2, in);
+ int i;
+ for (i = 0; i < si; i++)
+ {
+ char me = hex2data(2, in + 2 + 2* i );
+ sendBytetoMo(me);
+
+ };
+}
+;
+
+void eval_set_position_message(const char in[])
+/*! >P command sets servo to position */
+{
+ int servoid, value;
+ servoid = hex2data(2, in);
+ value = hex2data(4, in + 2);
+ value += base_data->zero_pos_inits[servoid]
+ - base_data->zero_pos_inits_feed[servoid];
+ send_servo_to_pos(servoid, value);
+}
+;
+
+void eval_set_min_max_message(const char in[])
+//! >M message -- for setting up min and max values
+{
+ int servoid, minvalue, maxvalue;
+ servoid = hex2data(2, in);
+ base_data->servo_min_val[servoid] = hex2data(4, in + 2);
+ base_data->servo_max_val[servoid] = hex2data(4, in + 6);
+ char eins[5];
+ eins[4] = 0;
+ sendMesg("\n");
+ data2hex(4, base_data->servo_min_val[servoid], eins);
+ sendMesg(eins);
+ data2hex(4, base_data->servo_max_val[servoid], eins);
+ sendMesg(eins);
+ sendMesg("\n");
+}
+;
+
+void eval_set_state_message(const char in[])
+//! >S message
+{
+ base_data->op_state = hex2data(2, in);
+
+ sendMesg("\r\n");
+ write_int(base_data->op_state);
+ sendMesg("\r\n");
+}
+;
+
+void eval_set_gain_message(const char in[])
+//! set gain for servo
+{
+ int servoid, value;
+ servoid = hex2data(2, in);
+ value = hex2data(2, &in[2]);
+ send_servo_to_gain(servoid, value);
+
+}
+;
+
+void eval_set_time(const char in[])
+//! set time
+{
+ long time = hex2data(8, in);
+ set_hmdl_time(time);
+}
+;
+
+void send_time()
+//! read time from HMDP and send it to the client
+{
+ char mes[12] =
+ { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+ data2hex(8, get_hmdl_time(), mes);
+ sendMesg("!");
+ sendMesg(mes);
+ sendMesg("\r\n");
+
+ data2hex(8, get_hmdl_time_inc(), mes);
+ sendMesg("inc: ");
+ sendMesg(mes);
+ sendMesg("\r\n");
+
+}
+;
+
+void eval_set_servo_on_off(const char in[])
+//! check for >ON or >OFF message both start with the the letter O
+{
+
+ int servoid;
+ sendMesg("in on off\n");
+ switch (in[0])
+ {
+ case 'N': //! >ON##
+
+ servoid = hex2data(2, &in[1]);
+ send_servo_on(servoid);
+ break;
+
+ default: //! >OFF##
+ servoid = hex2data(2, &in[2]);
+ send_servo_off(servoid);
+ break;
+ }
+
+}
+;
+
+void eval_get_jname(const char in[])
+/*! reads the name of the servo form the preprogrammed list defined in
+ compatible.h */
+{
+
+ int servoid, position, i;
+
+ sendByte('!'); //Data indikator
+
+ if (jointnames != NULL)
+ {
+
+ if (in[0] == 'v') //! all joints
+ {
+ //init_servo_list();
+ for (i = 0; i < base_data->servo_list_base[0]; ++i)
+ {
+
+ sendMesg(jointnames[i]);
+ sendByte(':');
+
+ };
+
+ }
+ else
+ {
+ servoid = hex2data(2, &in[0]);
+
+ sendMesg(jointnames[servoid]);
+
+ };
+ }
+ sendByte(13);
+ sendByte(10);
+
+}
+
+void eval_get_pos_message(const char in[])
+/* <P command read serv-pos from potis */
+{
+
+ int servoid, position, i;
+
+ sendByte('!'); //Data indikator
+
+ if (in[0] == 'v')
+ {
+ //init_servo_list();
+ for (i = 0; i < base_data->servo_list_base[0]; ++i)
+ {
+ servoid = base_data->servo_list_base[i + 1];
+ position = read_back_pos(servoid);
+ position -= base_data->zero_pos_inits[servoid]
+ - base_data->zero_pos_inits_feed[servoid]; //ACHTUNG
+
+ char xx[] =
+ { 0, 0, 0, 0, 0 };
+ data2hex(4, position, xx);
+ sendMesg(xx);
+
+ };
+ sendByte(13);
+ sendByte(10);
+
+ }
+ else
+ {
+ servoid = hex2data(2, &in[0]);
+ position = read_back_pos(servoid);
+ position -= base_data->zero_pos_inits[servoid]
+ - base_data->zero_pos_inits_feed[servoid]; //ACHTUNG
+
+ char xx[] =
+ { 0, 0, 0, 0, 13, 0 };
+ data2hex(4, position, xx);
+ sendMesg(xx);
+ //uart0SendByte('*');
+ sendByte(13);
+ sendByte(10);
+ };
+}
+;
+
+void eval_get_pos_set_message(const char in[])
+/*! <R get target value */
+{
+
+ int servoid, position, i;
+
+ sendByte('!'); //Data indikator
+
+ if (in[0] == 'v')
+ {
+ init_servo_list();
+ for (i = 0; i < base_data->servo_list_base[0]; ++i)
+ {
+ servoid = base_data->servo_list_base[i + 1];
+ position = read_back_pos_set(servoid);
+ char xx[] =
+ { 0, 0, 0, 0, 0 };
+ data2hex(4, position, xx);
+ sendMesg(xx);
+
+ };
+ //uart0SendByte('*');
+ sendByte(13);
+ sendByte(10);
+
+ }
+ else
+ {
+
+ servoid = hex2data(2, &in[0]);
+ position = read_back_pos_set(servoid);
+ char xx[] =
+ { 0, 0, 0, 0, 13, 0 };
+ data2hex(4, position, xx);
+ sendMesg(xx);
+ sendByte(13);
+ sendByte(10);
+ }
+}
+;
+
+void send_zero_pos()
+{
+
+ int i;
+ sendByte('!');
+ for (i = 0; i < base_data->servo_list_base[0]; ++i)
+ {
+ unsigned char servoid = base_data->servo_list_base[i + 1];
+ int zeropos = base_data->zero_pos_inits_feed[servoid]; //ACHTUNG DEBUG
+
+ char xx[] =
+ { 0, 0, 0, 0, 0 };
+ data2hex(4, zeropos, xx);
+ sendMesg(xx);
+
+ };
+ sendByte(13);
+ sendByte(10);
+
+}
+; // send zero pos feed
+
+
+void plastic_state()
+/*! timer call for plastic state */
+{
+ init_servo_list();
+ int i;
+ int j, val;
+ for (i = 1; i <= base_data->servo_list_base[0]; ++i)
+ {
+
+ j = base_data->servo_list_base[i];
+ val = read_back_pos(j);
+ send_servo_to_pos(j, val);
+ };
+}
+;
+
+/* first level parser for base.c commands (<..., >...., @....)*/
+int eval_seq_base(const char in[])
+{
+
+ int found_command = 0;
+ found_command = -1;
+ switch (in[0])
+ {
+ case '>':
+ disableIRQ();
+
+ switch (in[1])
+ {
+ case '$':
+ eval_send_gen_message(&in[2]); /* >$ (gen messg command */
+ case 'S':
+ eval_set_state_message(&in[2]); /* >S command */
+ break;
+ case 'P':
+ eval_set_position_message(&in[2]); /* >P command */
+ break;
+ case 'G':
+ eval_set_gain_message(&in[2]); /* >G command */
+ break;
+ case 'O':
+ eval_set_servo_on_off(&in[2]); /* >O command */
+ break;
+ case 'M':
+ eval_set_min_max_message(&in[2]); /* >M command */
+ break;
+ case 'T':
+ eval_set_time(&in[2]); /* >T command */
+ break;
+ case 'E':
+ eval_set_echo_on_off(&in[2]); /* >E command */
+ break;
+ case 'Z':
+ set_current_servo_pos_as_zero();
+ break;
+ }
+ enableIRQ();
+ break;
+ case '<':
+
+ switch (in[1])
+ {
+ case 'N':
+ disableIRQ();
+ eval_get_jname(&in[2]); /* <N command: get the names of the servos */
+ break;
+ case 'P':
+ disableIRQ();
+ eval_get_pos_message(&in[2]); /* <P command */
+ break;
+ case 'R':
+ disableIRQ();
+ eval_get_pos_set_message(&in[2]); /* <R command */
+ break;
+ case 'L':
+ disableIRQ();
+ send_servo_list(); /* <L command servo list*/
+ break;
+ case 'T':
+ disableIRQ();
+ send_time();
+ break;
+ case 'Z':
+ send_zero_pos(); // send zero pos feed
+ break;
+
+ }
+ enableIRQ();
+ default:
+ found_command = -1;
+ break;
+ }
+ return found_command;
+}
+;
+
+/* sending motion to servo ...*/
+
+void send_hmdp_motion_to_servo()
+{
+ int i;
+ for (i = 0; i < base_data->servo_list_base[0]; ++i)
+ {
+
+ int ID = base_data->servo_list_base[i + 1];
+ int pos = base_data->zero_pos_inits[(int) base_data->servo_list_base[i
+ + 1]] + get_hmdl_servo_out(i);
+
+ send_servo_to_pos((int) base_data->servo_list_base[i + 1], pos);
+ };
+
+}
+;
+
+/*! interrupt routine for plastic state */
+void inter_routine_state_2()
+{
+ plastic_state();
+}
+;
+
+/*! interrupt routine for HMDP runnung */
+void inter_routine_state_1()
+{
+ // if ((get_hmdl_time_inc()!=0))
+ // {
+ calc_next_timer_value();
+ send_hmdp_motion_to_servo();
+ motion_machine(calc_next_timer_value());
+
+}
+;
+
+/*! main interrupt routine */
+void inter_routine_base()
+{
+ if (base_data->op_state == 1)
+ inter_routine_state_1();
+ if (base_data->op_state == 2)
+ inter_routine_state_2();
+
+}
+;
+
+int main_eval(char *buff)
+{
+
+ if (eval_seq_base(buff) == -1)
+ {
+ //sendMesg ("\n");
+ eval_seq(buff); // HMDP parser in hmdp_c.c
+
+ }
+ return 0;
+}
+
+//! main parser
+int parse_one_line()
+{
+ char in[3];
+ in[0] = 0;
+ in[1] = 0;
+ int buff_c = 0;
+ int a = 0;
+
+ while (a != 13)
+ {
+ // led1_off();
+ a = readByte();
+ ;
+ if (a != -1)
+ {
+ // led1_on();
+ base_data->buff[base_data->bbase + buff_c] = a;
+ buff_c++;
+ in[0] = a;
+ if (base_data->echo_on == 0)
+ sendByte(a);
+
+ };
+ };
+
+ if (buff_c > 5) //! Checksum feature
+ {
+ if ((base_data->buff[base_data->bbase + buff_c - 4] == 'C')
+ && (base_data->buff[base_data->bbase + buff_c - 3] == 'S'))
+ {
+ int sum = 0;
+ int i;
+ for (i = 0; i < buff_c - 4; ++i)
+ sum += base_data->buff[base_data->bbase + i];
+ sum %= 15;
+ int sum2 = hex2data(1, &(base_data->buff[base_data->bbase + buff_c
+ - 2]));
+ if (sum == sum2)
+ {
+ sendMesg("\r\n*\r\n");
+ }
+ else
+ {
+ sendMesg("\r\nE\r\n");
+ };
+ buff_c -= 3;
+
+ };
+ };
+ if (buff_c > 1) //! Long message feature
+ {
+ if (base_data->buff[base_data->bbase + buff_c - 2] == '&')
+ {
+ base_data->bbase += buff_c - 2;
+ sendMesg("add line \n");
+ }
+ else
+ {
+ base_data->buff[base_data->bbase + buff_c] = 13;
+ base_data->buff[base_data->bbase + buff_c + 1] = 0;
+
+ base_data->bbase = 0;
+
+ }
+
+ }
+ else
+ {
+
+ base_data->buff[base_data->bbase + buff_c] = 13;
+ base_data->buff[base_data->bbase + buff_c + 1] = 0;
+ base_data->bbase = 0;
+ }
+ if (base_data->bbase == 0)
+ {
+ sendMesg("\r\n");
+
+ main_eval(base_data->buff); // see base.c
+ clearBuffer();
+ };
+
+}
+;
+
Property changes on: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.c
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.h (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.h 2009-07-31 20:40:14 UTC (rev 82)
@@ -0,0 +1,73 @@
+/***************************************************************************
+ * base.h for the Harmonic Motion Description Protocol (HMDP) *
+ * N. Michael Mayer, 2007 email: nm...@gm... *
+ * This part of the program is intended to be used in the micro-controller *
+ ***************************************************************************/
+
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2002,2003 Koblenz University
+ Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
+ Copyright (C) 2008 N. Michael Mayer, email: nm...@gm...
+
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __HMDP_BASE__
+#define __HMDP_BASE__
+
+#define MAX_ALLOWED_SERVOS 64
+#define MAX_PARSER_BUFFER_SIZE 200
+
+typedef struct
+{
+ /*! instantiation of the state variable
+
+ */
+ int op_state; // main state variable HMDP off=0, HMDP on =1, plastic =2
+ int servo_min_val[MAX_ALLOWED_SERVOS + 1]; // sets a min and a max value for each joint
+ int servo_max_val[MAX_ALLOWED_SERVOS + 1];
+ char servo_list_base[MAX_ALLOWED_SERVOS + 1]; // reserved memory for the joint ids
+ int *zero_pos_inits; // pointer to current zero positions
+ int zero_pos_inits_feed[MAX_ALLOWED_SERVOS + 1]; // ram list of the zero positions
+ int bbase; // pointer to the current position in the parser buffer
+ char buff[MAX_PARSER_BUFFER_SIZE]; // parser buffer
+ int echo_on; // echo flag -- for terminal communication
+
+} Base_data; // all "global" data put for base.c
+
+
+// functions in base.c
+
+
+void init_base();
+int parse_one_line();
+void inter_routine_base();
+
+int init_servo_list();
+int servo_list_in_hex(char *li);
+int eval_seq_base(const char in[]);
+void send_hmdp_motion_to_servo();
+void write_int(int in);
+
+extern char jointnames[MAX_ALLOWED_SERVOS][8];
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#endif
Property changes on: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/base.h
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/compatible.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/compatible.h (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/compatible.h 2009-07-31 20:40:14 UTC (rev 82)
@@ -0,0 +1,92 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ The functions names in this header have to be implmented for each microcontroller individually
+
+ Copyright (C) 2008 N. Michael Mayer, nm...@gm...
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+//! HMDP Compatibility Layer:
+
+#ifndef ___COMPATIBLE_RR___
+#define ___COMPATIBLE_RR___
+
+//! This is the set of functions and constants used by the hmdp_c.c and base.c functions. They have to be implemented for each robot individually
+
+//! Interrupt related functions
+
+// #1
+void disableIRQ();
+// #2
+void enableIRQ();
+
+// !Communication over with the the client
+// #3
+int sendByte(int data);
+// #4
+int readByte();
+// #5
+void clearBuffer();
+
+//! possibly useful to allow direct communication between client and servos using the >$ command
+// #6
+int sendBytetoMo(int);
+
+//! These commands have to be implemented to control the servos
+//! For all servos it is assummed that 0 corresponds to the minimal value and 4096 corresponds to the maximal value
+
+// #7
+void send_servo_off(int ID); // ! turn servo off
+// #8
+void send_servo_on(int ID); // ! turn servo on
+// #9
+void send_servo_to_pos(int ID, int pos); // ! commands
+// #10
+void send_servo_to_gain(int ID, int gain);
+
+// #11
+int read_back_pos(int ID); // ! read current servo pos from poti value
+// #12
+int read_back_pos_set(int ID); // ! read targert position from servo
+// #13
+int read_back_id(int ID); // ! check if a servo with the particular ID is present
+
+
+/*! for all things mentioned below this point:
+ HMDP internally counts the servo from lower numbers to higher numbers thus a robot with servo
+ IDs 1,3,8,15 internally has the IDs 0,1,2,3
+ the arrays: jointnames[][8]; and zer_pos_inits_feed */
+
+// #14
+extern char jointnames[][8]; // ! names of the joints
+// ! can be read out by: <Nxx and used by tools e.g. for transferring motion patterns from one robot to another
+/*! Naming Conventions: NNXYPD[D] NN: Limb name that is: HE: Head, RA: right arm RL: Right leg, etc.
+ X number of the major joint position counted from the body center: hip, shoulder, neck = 1;
+ knee, elbow = 2;
+ foot, wrist = 3;
+ Y number of the hingjoint within the joint -- differs in dependence of the design
+ P parity -- if the robot executes the same motion according to reflection symmetry, i.e. all R become L if the motion stays
+ the same with parity +
+ is inverted in the case of parity -
+ D direction in Pitch (P), Roll (R), Yaw (Y), diagonal joints may use double letters e.g. PY
+ numbering convention: IF THE ROBOT HAS GAPS BETWEEN SERVO NUMBERS:
+ */
+
+// #15
+extern int zero_pos_inits_feed[];
+/*! Zero positions i.e. the values that should be send to the servos of the robot to set it into an initial posture,
+ a posture from which the motions deviate. In some robots it is necessary */
+
+#endif // ___COMPATIBLE_RR___
Property changes on: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/compatible.h
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c
===================================================================
--- trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c 2009-07-31 20:40:14 UTC (rev 82)
@@ -0,0 +1,977 @@
+#include "hmdp_c.h"
+#include "sine_fixed.h"
+
+Hmdl *hmdl; // hook for a pointer to all data
+
+
+/*! Util routines to convert integers to hex strings and vice versa */
+
+int hex2data(short size_v, const char res[])
+{
+
+ int k;
+ int data;
+ data = 0;
+
+ for (k = 0; k < size_v; k++)
+ {
+
+ data *= 16;
+ switch (res[k])
+
+ {
+ case '0':
+ break;
+ case '1':
+ data += 1;
+ break;
+ case '2':
+ data += 2;
+ break;
+ case '3':
+ data += 3;
+ break;
+ case '4':
+ data += 4;
+ break;
+ case '5':
+ data += 5;
+ break;
+ case '6':
+ data += 6;
+ break;
+ case '7':
+ data += 7;
+ break;
+ case '8':
+ data += 8;
+ break;
+ case '9':
+ data += 9;
+ break;
+ case 'a':
+ data += 10;
+ break;
+ case 'b':
+ data += 11;
+ break;
+ case 'c':
+ data += 12;
+ break;
+ case 'd':
+ data += 13;
+ break;
+ case 'e':
+ data += 14;
+ break;
+ case 'f':
+ data += 15;
+ break;
+
+ }
+
+ };
+ return data;
+}
+;
+
+void data2hex(short size_v, const int dat, char res[])
+{
+
+ int k;
+ int data = dat;
+ for (k = size_v - 1; k >= 0; k--)
+ {
+
+ switch (data % 16)
+
+ {
+ case 0:
+ res[k] = '0';
+ break;
+ case 1:
+ res[k] = '1';
+ break;
+ case 2:
+ res[k] = '2';
+ break;
+ case 3:
+ res[k] = '3';
+ break;
+ case 4:
+ res[k] = '4';
+ break;
+ case 5:
+ res[k] = '5';
+ break;
+ case 6:
+ res[k] = '6';
+ break;
+ case 7:
+ res[k] = '7';
+ break;
+ case 8:
+ res[k] = '8';
+ break;
+ case 9:
+ res[k] = '9';
+ break;
+ case 10:
+ res[k] = 'a';
+ break;
+ case 11:
+ res[k] = 'b';
+ break;
+ case 12:
+ res[k] = 'c';
+ break;
+ case 13:
+ res[k] = 'd';
+ break;
+ case 14:
+ res[k] = 'e';
+ break;
+ case 15:
+ res[k] = 'f';
+ break;
+
+ }
+ data /= 16;
+ };
+
+}
+;
+
+void set_hmdl_time(long time)
+{
+ hmdl->time = time;
+}
+;
+
+long get_hmdl_time()
+{
+ return hmdl->time;
+}
+;
+
+long get_hmdl_time_inc()
+{
+ return hmdl->time_inc;
+}
+;
+
+short get_hmdl_servo_out(int i)
+{
+ return cfloat2int(hmdl->positions_to_set[i]);
+}
+;
+
+void write_cfloat(struct c_float *in)
+{
+
+ struct c_f_hex in_hex = c_float2hex(*in);
+ sendMesg(in_hex.stri);
+
+}
+
+#define INTERVAL 50 /* num of ms for the timer to go off */
+
+/* function prototype */
+void motion_control(int);
+
+/*
+ * global variables
+ */
+
+/* struct Hmdl hmdl;*/
+
+struct c_f_hex c_float2hex(struct c_float in)
+{
+ short sign, esign, i;
+ struct c_f_hex ret;
+ for (i = 0; i < 15; ++i)
+ ret.stri[i] = 0;
+
+ if (in.mantisse < 0)
+ {
+ ret.stri[0] = '-';
+ sign = -1;
+ }
+ else
+ {
+ sign = 1;
+ ret.stri[0] = '+';
+ };
+
+ if (in.exponent < 0)
+ {
+ ret.stri[9] = '-';
+ esign = -1;
+ }
+ else
+ {
+ esign = 1;
+ ret.stri[9] = '+';
+ };
+
+ data2hex(8, sign * in.mantisse, &ret.stri[1]);
+ /*ret.stri[9]='^';*/
+
+ data2hex(2, esign * in.exponent, &ret.stri[10]);
+
+ return ret;
+}
+;
+
+struct c_float mult_cc(struct c_float a, struct c_float b)
+{
+ struct c_float c;
+ int sign_c = 1;
+
+ if (a.mantisse < 0)
+ {
+ sign_c = -1;
+ a.mantisse = -a.mantisse;
+ }
+
+ if (b.mantisse < 0)
+ {
+ sign_c = -sign_c;
+ b.mantisse = -b.mantisse;
+ }
+
+ a.mantisse = a.mantisse >> (INT_MAXE / 2);
+ b.mantisse = b.mantisse >> (INT_MAXE / 2);
+
+ c.exponent = a.exponent + b.exponent;
+ c.mantisse = a.mantisse * b.mantisse * sign_c;
+
+ if (c_abs(c.mantisse) < INT_MAXX)
+ {
+ c.exponent--;
+ c.mantisse = c.mantisse << 1;
+ };
+
+ return c;
+}
+;
+
+int cfloat2int(struct c_float a)
+{
+ int r;
+ if (a.mantisse > 0)
+ r = a.mantisse >> (INT_MAXE - a.exponent);
+ else
+ {
+ r = -1* ( (-a.mantisse)>>(INT_MAXE-a.exponent));
+ };
+ return r;
+};
+
+struct c_float hex2c_float(const char in[])
+{
+ struct c_float ret;
+ short sign, esign;
+ if (in[0] == '-')
+ sign = -1;
+ else if (in[0] == '+')
+ sign = +1;
+
+ if (in[9] == '-')
+ esign = -1;
+ else if (in[9] == '+')
+ esign = +1;
+
+ ret.mantisse = sign * hex2data(8, &in[1]);
+ ret.exponent = esign * hex2data(2, &in[10]);
+ return ret;
+}
+
+int lo2(int in)
+//! returns rough integer logarithm of in
+{
+ if (in >= 2048)
+ return 11;
+ if (in >= 1024)
+ return 10;
+ if (in >= 512)
+ return 9;
+ if (in >= 256)
+ return 8;
+ if (in >= 128)
+ return 7;
+ if (in >= 64)
+ return 6;
+ if (in >= 32)
+ return 5;
+ if (in >= 16)
+ return 4;
+ if (in >= 8)
+ return 3;
+ if (in >= 4)
+ return 2;
+ if (in >= 2)
+ return 1;
+ if (in < 2)
+ return 0;
+}
+;
+
+struct c_float c_f_sum(struct c_float a[], int size)
+//! sum of size c_floats
+{
+ int i, max, lsize;
+ lsize = lo2(size);
+ struct c_float result;
+ max = 0;
+ result.mantisse = 0;
+
+ for (i = 0; i < size; ++i)
+ if (a[i].exponent > max)
+ max = a[i].exponent;
+
+ for (i = 0; i < size; ++i)
+ {
+ result.mantisse += (a[i].mantisse >> (max - a[i].exponent + lsize));
+ };
+
+ result.exponent = max + lsize;
+ return result;
+}
+;
+
+int c_abs(int in)
+//! absolut value of integer
+{
+ if (in > 0)
+ return in;
+ else
+ return -in;
+}
+;
+
+struct c_float c_f_add(struct c_float s1, struct c_float s2)
+//! add 2 c_floats
+{
+ int i, max, lsize;
+ struct c_float result;
+ max = 0;
+ result.mantisse = 0;
+
+ max = s1.exponent;
+ if (s2.exponent > max)
+ max = s2.exponent;
+
+ if (s1.mantisse < 0)
+ result.mantisse -= ((-s1.mantisse) >> (max - s1.exponent + 1));
+ else
+ result.mantisse += ((s1.mantisse) >> (max - s1.exponent + 1));
+
+ if (s2.mantisse < 0)
+ result.mantisse -= (-s2.mantisse >> (max - s2.exponent + 1));
+ else
+ result.mantisse += (s2.mantisse >> (max - s2.exponent + 1));
+
+ result.exponent = max + 1;
+ if (c_abs(result.mantisse) < INT_MAXX)
+ {
+ result.exponent--;
+ if (result.mantisse > 0)
+ result.mantisse = result.mantisse << 1;
+ else
+ result.mantisse = -1* ( (-result.mantisse) <<1);
+ };
+
+ return result;
+ };
+
+struct c_float mult_c_sinus(struct c_float a, int d)
+//! multiply routine for sinus input c_float * sinus
+{
+ struct c_float c;
+
+ int sign_c = 1;
+
+ if (a.mantisse < 0)
+ {
+ sign_c = -1;
+ a.mantisse = -a.mantisse;
+ }
+
+ if (d < 0)
+ {
+ sign_c = -sign_c;
+ d = -d;
+ }
+
+ a.mantisse = a.mantisse >> (INT_MAXE / 2);
+ d = d >> (INT_MAXE / 2);
+
+ c.exponent = a.exponent;
+ c.mantisse = a.mantisse * d * sign_c;
+
+ return c;
+}
+;
+
+struct c_float mult_cc_sinus(struct c_float a, struct c_float b, int d)
+//! multiply 2 c_floats with one sinus
+{
+ struct c_float c;
+ int sign_c = 1;
+
+ if (a.mantisse < 0)
+ {
+ sign_c = -1;
+ a.mantisse = -a.mantisse;
+ }
+
+ if (d < 0)
+ {
+ sign_c = -sign_c;
+ d = -d;
+ }
+
+ if (b.mantisse < 0)
+ {
+ sign_c = -sign_c;
+ b.mantisse = -b.mantisse;
+ }
+
+ a.mantisse = a.mantisse >> (2*INT_MAXE / 3);
+ b.mantisse = b.mantisse >> (2*INT_MAXE / 3);
+ d = d >> (2*INT_MAXE / 3);
+
+ c.exponent = a.exponent + b.exponent;
+ c.mantisse = a.mantisse * b.mantisse * d * sign_c;
+
+ if (c_abs(c.mantisse) < INT_MAXX)
+ {
+ c.exponent--;
+ if (c.mantisse > 0)
+ c.mantisse = c.mantisse << 1;
+ else
+ c.mantisse = -1* ( (-c.mantisse) <<1);
+ };
+
+ return c;
+ };
+
+struct c_float set_c_float_zero()
+// returns a c_float = 0
+{
+ struct c_float a;
+ a.mantisse = 0;
+ a.exponent = 0;
+ return a;
+}
+
+struct c_float fade_in(struct c_float a, struct c_float b, long t_a, long t_b,
+ long t)
+//! provides fading in for motion patterns (in a linear -- ramp like manner)
+{
+
+ int max_man;
+ struct c_float c;
+ max_man = a.exponent;
+ int sign_a, sign_b;
+ if (a.mantisse >= 0)
+ {
+ sign_a = 1;
+ }
+ else
+ {
+ sign_a = -1;
+ a.mantisse = -a.mantisse;
+ };
+
+ if (b.mantisse >= 0)
+ {
+ sign_b = 1;
+ }
+ else
+ {
+ sign_b = -1;
+ b.mantisse = -b.mantisse;
+ };
+
+ if (max_man < b.exponent)
+ max_man = b.exponent;
+
+ long long diff = sign_b * (b.mantisse >> (max_man - b.exponent)) - sign_a
+ * (a.mantisse >> (max_man - a.exponent));
+ diff *= (t - t_a);
+ diff /= (t_b - t_a);
+
+ c.exponent = max_man;
+ c.mantisse = sign_a * a.mantisse + diff;
+
+ return c;
+}
+;
+
+void motion_machine(unsigned int time)
+/*! interpolation using direct access to the hmdl structure */
+{
+
+ int i, k, l;
+ struct c_float y, sine_amp, cos_amp;
+ struct c_float amp;
+ struct c_float *coeffs;
+
+ /*! iterate over the servos and sum all values for the active motion patterns
+ -> superposition */
+ for (i = 0; i < MAX_SERVOS; ++i)
+ {
+ hmdl->positions_to_set[i] = set_c_float_zero(); // init servo_list
+
+ /*! loop over all motion patterns, see which ones should be active */
+ for (k = 0; k < MAX_MPS; ++k)
+ {
+ /*! skip pattern if amplitude is 0 or not active */
+ if (!(hmdl->mp[k].active))
+
+ {
+ continue;
+ }
+
+ coeffs = hmdl->mp[k].pp->sv[i].s_position;
+
+ if (time < hmdl->mp[k].start_0)
+ amp = hmdl->mp[k].A_old;
+ else if (time >= hmdl->mp[k].start_1)
+ amp = hmdl->mp[k].A_new;
+ else
+ amp = fade_in(hmdl->mp[k].A_old, hmdl->mp[k].A_new,
+ hmdl->mp[k].start_0, hmdl->mp[k].start_1, time);
+
+ /*if (hmdl->mp[k].end_0!=0)
+ {
+ if (time > hmdl->mp[k].start_1)
+ {
+ hmdl->mp[k].start_0=hmdl->mp[k].end_0;
+ hmdl->mp[k].start_1=hmdl->mp[k].end_1;
+
+ hmdl->mp[k].end_0=0;
+ hmdl->mp[k].end_1=0;
+ hmdl->mp[k].A_old=hmdl->mp[k].A_new;
+ hmdl->mp[k].A_new=set_c_float_zero();
+ };
+ };*/
+
+ if (hmdl->mp[k].end_0 != 0)
+ {
+ if (time > hmdl->mp[k].end_0)
+ {
+ if (time > hmdl->mp[k].end_1)
+ {
+ //
+ amp = set_c_float_zero();
+ // hmdl->mp[k].active=0;
+ }
+
+ else
+ {
+ amp = fade_in(hmdl->mp[k].A_new, set_c_float_zero(),
+ hmdl->mp[k].end_0, hmdl->mp[k].end_1, time);
+
+ };
+
+ };
+ };
+
+ //amp = hmdl->mp[k].A_curr;
+ //if (i==20)
+ // {write_int(cfloat2int(amp)); DMESG(" : ");}
+
+ y = mult_cc(amp, coeffs[0]);
+ //y = coeffs[0];
+ //if (i==20) {
+ // write_int(cfloat2int(y)); DMESG(" : ");}
+
+
+ for (l = 0; l < (MAX_COEFFS - 1) / 2; ++l)
+ {
+ y = c_f_add(y, mult_cc_sinus(amp, coeffs[2* l + 1], sin_fixed(
+ ((time - hmdl->mp[k].offset) * hmdl->mp[k].pp->wlqs_top[l])
+ / hmdl->mp[k].pp->wlqs_bot[l])));
+
+ y = c_f_add(y, mult_cc_sinus(amp, coeffs[2* l + 2], cos_fixed(
+ ((time - hmdl->mp[k].offset) * hmdl->mp[k].pp->wlqs_top[l])
+ / hmdl->mp[k].pp->wlqs_bot[l])));
+ };
+
+ hmdl->positions_to_set[i] = c_f_add(y, hmdl->positions_to_set[i]);
+
+ };
+ };
+}
+;
+
+int calc_next_timer_value()
+//! wrapper for time
+{
+ hmdl->time += hmdl->time_inc;
+ return hmdl->time;
+}
+
+void eval_set_time_message(const char in[])
+// TS HMDP command similar to >T command but also increment can be set
+{
+ hmdl->time = hex2data(MAX_TIME_DIGITS, in);
+ hmdl->time_max = hex2data(MAX_TIME_DIGITS, &in[MAX_TIME_DIGITS]);
+ hmdl->time_inc = hex2data(MAX_TIME_DIGITS, &in[2*MAX_TIME_DIGITS]);
+ //timer_running=0;
+ write_int(hmdl->time);
+ sendMesg("\r\n");
+
+}
+;
+
+void eval_set_alarm_point(const char in[])
+// not implelemented so far
+{
+
+}
+;
+
+void eval_use_pattern_message(const char in[])
+//! PU command
+{
+ int patternid, curr;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+
+ hmdl->mp[patternid].start_0 = hex2data(MAX_TIME_DIGITS, &in[MAX_ID_DIGITS]);
+ hmdl->mp[patternid].start_1 = hex2data(MAX_TIME_DIGITS, &in[MAX_ID_DIGITS
+ + MAX_TIME_DIGITS]);
+ hmdl->mp[patternid].offset = hex2data(MAX_TIME_DIGITS,
+ &in[MAX_ID_DIGITS + 2*MAX_TIME_DIGITS]);
+
+ hmdl->mp[patternid].end_0 = 0;
+ hmdl->mp[patternid].end_1 = 0;
+
+ curr = MAX_ID_DIGITS + 3*MAX_TIME_DIGITS;
+
+ hmdl->mp[patternid].A_old = hmdl->mp[patternid].A_new;
+ hmdl->mp[patternid].A_new = hex2c_float(&in[curr]);
+
+ hmdl->mp[patternid].active = 1;
+
+}
+;
+
+void eval_use_short_pattern_message(const char in[])
+//! PP command - shorter PU message
+//
+{
+ int patternid, curr;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+
+ hmdl->mp[patternid].start_0 = 0;
+ hmdl->mp[patternid].start_1 = 0;
+ hmdl->mp[patternid].end_0 = 0;
+ hmdl->mp[patternid].end_1 = 0;
+ hmdl->mp[patternid].offset = 0;
+
+ hmdl->mp[patternid].A_old = hmdl->mp[patternid].A_new;
+ hmdl->mp[patternid].A_new.mantisse = 0x40000000;
+ hmdl->mp[patternid].A_new.exponent = 0;
+ hmdl->mp[patternid].active = 1;
+
+}
+;
+
+void eval_use_pattern_message_with_end(const char in[])
+//! PY sending a pattern with the ramp end
+{
+ int patternid, curr;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+
+ hmdl->mp[patternid].A_old = set_c_float_zero();
+ hmdl->mp[patternid].A_new = set_c_float_zero();
+
+ hmdl->mp[patternid].start_0 = hex2data(MAX_TIME_DIGITS, &in[MAX_ID_DIGITS]);
+ hmdl->mp[patternid].start_1 = hex2data(MAX_TIME_DIGITS, &in[MAX_ID_DIGITS
+ + MAX_TIME_DIGITS]);
+ hmdl->mp[patternid].end_0 = hex2data(MAX_TIME_DIGITS,
+ &in[MAX_ID_DIGITS + 2*MAX_TIME_DIGITS]);
+ hmdl->mp[patternid].end_1 = hex2data(MAX_TIME_DIGITS,
+ &in[MAX_ID_DIGITS + 3*MAX_TIME_DIGITS]);
+ hmdl->mp[patternid].offset = hex2data(MAX_TIME_DIGITS,
+ &in[MAX_ID_DIGITS + 4*MAX_TIME_DIGITS]);
+ /*printf (" on sh 2 :%d %d\n", hmdl->mp[patternid].start_0,
+ hmdl->mp[patternid].start_1);*/
+
+ curr = MAX_ID_DIGITS + 5*MAX_TIME_DIGITS;
+
+ hmdl->mp[patternid].A_old = hmdl->mp[patternid].A_new;
+ hmdl->mp[patternid].A_new = hex2c_float(&in[curr]);
+
+ hmdl->mp[patternid].active = 1;
+
+}
+;
+
+void eval_set_servo_coeff_message(const char in[])
+{
+ int patternid, servoid, curr;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+ servoid = hex2data(MAX_ID_DIGITS, &in[MAX_ID_DIGITS]);
+ curr = 2*MAX_ID_DIGITS;
+ int i = 0;
+ while ((in[curr] != 'X') && (in[curr] != 0) && (i < MAX_COEFFS))
+ {
+ hmdl->mp[patternid].pp->sv[servoid].s_position[i] = hex2c_float(
+ &in[curr]);
+ write_cfloat(&hmdl->mp[patternid].pp->sv[servoid].s_position[i]);
+ sendMesg("\r\n");
+ write_int(i);
+ sendMesg("\r\n");
+
+ i++;
+ curr += C_FLOAT_LENGTH;
+ };
+
+}
+;
+
+void eval_set_servo_small_coeff_message(const char in[])
+{
+ int patternid, servoid, coeffid, curr;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+ servoid = hex2data(MAX_ID_DIGITS, &in[MAX_ID_DIGITS]);
+ coeffid = hex2data(MAX_ID_DIGITS, &in[2*MAX_ID_DIGITS]);
+ hmdl->mp[patternid].pp->sv[servoid].s_position[coeffid]
+ = hex2c_float(&in[3*MAX_ID_DIGITS]);
+}
+;
+
+//long long i;
+
+void eval_new_pattern_message(const char in[])
+{
+ int id, wp_number, counter, i, j;
+
+ /* get number */
+ id = hex2data(MAX_ID_DIGITS, in);
+ wp_number = hex2data(MAX_COE_DIGITS, &in[MAX_ID_DIGITS]);
+
+ counter = MAX_ID_DIGITS + MAX_COE_DIGITS;
+ for (i = 0; i < MAX_COEFFS; ++i)
+ for (j = 0; j < MAX_SERVOS; ++j)
+ {
+ hmdl->mp[id].pp->sv[j].s_position[i].mantisse = 0;
+ hmdl->mp[id].pp->sv[j].s_position[i].exponent = 0;
+ };
+
+ for (i = 0; i < (MAX_COEFFS - 1) / 2; ++i)
+ {
+ hmdl->mp[id].pp->wlqs_top[i] = 0;
+ hmdl->mp[id].pp->wlqs_bot[i] = 1;
+
+ };
+
+ for (i = 0; i < (wp_number - 1) / 2; ++i)
+ {
+ hmdl->mp[id].pp->wlqs_top[i] = hex2data(MAX_WLQ_DIGITS, &in[counter]);
+ counter += MAX_WLQ_DIGITS;
+ hmdl->mp[id].pp->wlqs_bot[i] = hex2data(MAX_WLQ_DIGITS, &in[counter]);
+ counter += MAX_WLQ_DIGITS;
+
+ /* DEBUG */
+ // printf ("MC: coeffs top %d bot %d \n", hmdl->mp[id].pp->wlqs_top[i],
+ // hmdl->mp[id].pp->wlqs_bot[i]);
+
+ };
+
+ /* DEBUG */
+ //intf("MC: Setting new pattern for number %d\n", id);
+}
+;
+
+void init_hmdl()
+{
+ int i, j, k, l, p;
+
+ /*
+ * inititialize HDML structure
+ */
+
+ /* motion patterns */
+
+ for (i = 0; i < MAX_MPS_IN_RAM; ++i)
+ {
+ hmdl->mp[i].pp = &(hmdl->mpp_in_ram[i]);
+ hmdl->mp[i].pp->ID_prop = 8888 + i;
+
+ for (j = 0; j < (MAX_COEFFS - 1) / 2; ++j)
+ {
+ hmdl->mp[i].pp->wlqs_top[j] = 0;
+ hmdl->mp[i].pp->wlqs_bot[j] = 1;
+ };
+
+ for (j = 0; j < MAX_SERVOS; j++)
+ {
+ for (k = 0; k < MAX_COEFFS; k++)
+ {
+ hmdl->mp[i].pp->sv[j].s_position[k].mantisse = 0;
+ hmdl->mp[i].pp->sv[j].s_position[k].exponent = 0;
+ };
+
+ };
+
+ };
+
+ /*for (i=MAX_MPS_IN_RAM;i<MAX_MPS;i++) // meant for motion patterns in flash
+ hmdl->mp[i].pp = 0x00058000 + 0x00000800*(i-MAX_MPS_IN_RAM);
+
+ for (i=0;i<MAX_MPS;++i)
+ {
+
+ hmdl->mp[i].A_old.mantisse=0;
+ hmdl->mp[i].A_old.exponent=0;
+ hmdl->mp[i].A_new.mantisse=0;
+ hmdl->mp[i].A_new.exponent=0;
+
+ hmdl->mp[i].start_0 = 0;
+ hmdl->mp[i].start_1 = 0;
+ hmdl->mp[i].updated = 0;
+ hmdl->mp[i].active = 0;
+ };*/// until here
+
+
+ hmdl->time = 0;
+ hmdl->time_max = 0;
+ hmdl->time_inc = 1;
+
+ /* positions to set */
+ for (p = 0; p < MAX_SERVOS; ++p)
+ {
+ hmdl->positions_to_set[p].exponent = 0;
+ hmdl->positions_to_set[p].mantisse = 0;
+ }
+
+}
+
+void eval_get_current_sine_val(const char in[])
+{
+ int patternid, freqid;
+ unsigned int retval;
+ patternid = hex2data(MAX_ID_DIGITS, in);
+ freqid = hex2data(MAX_ID_DIGITS, in);
+
+ retval = sin_fixed((hmdl->time) * hmdl->mp[patternid].pp->wlqs_top[freqid]
+ / hmdl->mp[patternid].pp->wlqs_bot[freqid]);
+ retval /= 100000;
+
+ sendMesg("current value:\r\n");
+ write_int(retval);
+ sendMesg("\r\n");
+ sendMesg("wlqs_top:\r\n");
+ write_int(hmdl->mp[patternid].pp->wlqs_top[freqid]);
+ sendMesg("\r\n");
+
+}
+;
+
+void eval_seq(const char in[])
+{
+
+ switch (in[0])
+ {
+ case 'P':
+ switch (in[1])
+ {
+ case 'N':
+ eval_new_pattern_message(&in[2]); // PN command
+ break;
+ case 'S':
+ eval_set_servo_coeff_message(&in[2]); // PS command
+ break;
+ case 'I':
+ eval_set_servo_small_coeff_message(&in[2]);
+ // PS command
+ break;
+ case 'U':
+ eval_use_pattern_message(&in[2]); // PU command
+ break;
+ case 'P':
+ eval_use_short_pattern_message(&in[2]); // PU command
+ break;
+ case 'Y':
+ eval_use_pattern_message_with_end(&in[2]); // PY command
+
+ break;
+ }
+ break;
+ case 'T':
+ switch (in[1])
+ {
+ case 'S':
+ eval_set_time_message(&in[2]); // TS command
+ break;
+ }
+
+ break;
+ case '?':
+ switch (in[1])
+ {
+ case 'S':
+ eval_get_current_sine_val(&in[2]);
+ break;
+
+ }
+ break;
+
+ }
+}
+;
+
+struct c_float interpolate_rational_c_float( /*Real amplitude interpolation*/
+int size_c, struct c_float amp, int time, struct c_float coeffs[],
+ unsigned int frequencies_up[],
+ unsigned int frequencies_bottom[])
+{
+ int l;
+ struct c_float y;
+ y = mult_cc(amp, coeffs[0]);
+
+ for (l = 0; l < (size_c - 1) / 2; ++l)
+ {
+ y = c_f_add(y, mult_cc_sinus(amp, coeffs[2* l + 1], sin_fixed((time
+ * frequencies_up[l]) / frequencies_bottom[l])));
+
+ y = c_f_add(y, mult_cc_sinus(amp, coeffs[2* l + 2], cos_fixed((time
+ * frequencies_up[l]) / frequencies_bottom[l])));
+ };
+
+ return y;
+
+}
+;
+
+struct c_float interpolate_c_float_phase(
+/*Amplitude is rotational matrix (complex) */
+int size_c, struct c_float amp[], int time, struct c_float coeffs[],
+ unsigned int frequencies[])
+{
+ int i;
+ struct c_float y, sine_amp, cos_amp;
+ y = mult_cc(amp[0], coeffs[0]);
+ for (i = 0; i < size_c / 2; i++)
+ {
+ sine_amp = c_f_add(mult_cc(amp[2], coeffs[2* i + 2]), mult_cc(amp[3],
+ coeffs[2* i + 1]));
+ cos_amp = c_f_add(mult_cc(amp[0], coeffs[2* i + 2]), mult_cc(amp[1],
+ coeffs[2* i + 1]));
+
+ y
+ = c_f_add(y, mult_c_sinus(sine_amp, sin_fixed(time
+ * frequencies[i])));
+ y = c_f_add(y, mult_c_sinus(cos_amp, cos_fixed(time * frequencies[i])));
+ };
+
+ return y;
+}
+;
+
Property changes on: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.c
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.h
===================================================================
--- trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.h (rev 0)
+++ trunk/rcssserver3d/plugin/soccer/hmdp_effector/hmdp_c/hmdp_c.h 2009-07-31 20:40:14 UTC (rev 82)
@@ -0,0 +1,196 @@
+/***************************************************************************
+ * Header for the Harmonic Motion Description Protocol (HMDP) *
+ * N. Michael Mayer, 2007 email: nm...@gm... *
+ * This part of the program is intended to be used in the micro-controller *
+ ***************************************************************************/
+
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2002,2003 Koblenz University
+ Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
+ Copyright (C) 2008 N. Michael Mayer, email: nm...@gm...
+
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; version 2 of the License.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef ___HMDP_C___
+#define ___HMDP_C___
+
+struct c_float /* Float Wrapper */
+{
+ int mantisse;
+ short exponent;
+};
+
+struct c_f_hex
+{
+ char stri[15];
+};
+
+#define C_FLOAT_LENGTH (1+8+1+2) // SMMMMMMMMSEEY Sign, Exponent, Mantisse, Y=Y
+#define HMDL_MESSAGE_START "";
+/*! Header for all HMDL Messages when they are sent over the serial bus */
+
+#define MAX_WLQ_DIGITS 6
+/*! Number of hex digits reserved for the Frequencies = wavelengthquarters: WLQs */
+
+#define MAX_TIME_DIGITS 8
+
+#define MAX_COE_DIGITS 2
+/*! Hex Digits reverved for the number of coeffieicnets */
+
+#define MAX_ID_DIGITS 2
+/*! Maximal Number of Digits of the ID; Since the ID can be up to 127 this
+ means it is 2 */
+
+#define MAX_MPS 67
+#define MAX_MPS_IN_RAM 67
+/*! Maximal numbers of Motion patterns that can be present
+ at one time in the SH2*/
+#define MAX_SERVOS 22
+/*! Maximal number of servos */
+#define MAX_COEFFS 11
+/*!maximal number of coefficients*/
+#define MAX_ALARM_POINTS 1
+/*! maximal number of points were the status can be readout */
+
+#define TM_BUFFER_SIZE 1024
+/*! Size of the List of timer tasks, see @... commands */
+
+struct motion_pattern_servo
+/* Structure that contains data about the motion of one servo */
+{
+ struct c_float s_position[MAX_COEFFS];
+ /*struct c_float s_gain [MAX_COEFFS];*/
+};
+
+struct motion_pattern_props
+{
+ int wlqs_top[((MAX_COEFFS - 1) / 2)];
+ int wlqs_bot[((MAX_COEFFS - 1) / 2)];
+ struct motion_pattern_servo sv[MAX_SERVOS];
+ int ID_prop;
+};
+
+struct motion_pattern
+/* Structure that contains all data necessary for a specific motion pattern */
+{
+ struct motion_pattern_props *pp; //[1];
+ struct c_float A_new;
+ struct c_float A_old;
+ // struct c_float A_curr;
+ unsigned long start_0, start_1, end_0, end_1,// interpolate amplitude from start_0 to start_1
+ offset, // used to offset the pattern when started
+ offsetoverflow; // used to save the offset after the time wraps around
+ unsigned short updated, // pattern was changed
+ active; // pattern is active
+
+};
+
+struct message_alarm_clock
+{
+ unsigned long alarm_time;
+ void (*callf)();
+};
+
+typedef struct
+/* all data necessary for the complete harmonic motion description
+ language */
+{
+ unsigned long time, time_max, time_inc;
+ struct motion_pattern mp[MAX_MPS]; /* -> sv -> s_position */
+ struct message_alarm_clock alarm[MAX_ALARM_POINTS];
+ struct c_float positions_to_set[MAX_SERVOS];
+ struct motion_pattern_props mpp_in_ram[MAX_MPS_IN_RAM];
+
+} Hmdl;
+
+struct c_f_hex c_float2hex(struct c_float in);
+struct c_float float2cfloat(float a);
+struct c_float mult_cc(struct c_float a, struct c_float b);
+// multiply 2 c_floats
+struct c_float mult_ccc(struct c_float a, struct c_float b, struct c_float d);
+// multiply 3 c-fs
+struct c_float mult_cc_sinus(struct c_float a, struct c_float b, int d);
+// multiply 2 c-fs and the output of the sine_fixed routine (d parameter)
+struct c_float mult_c_sinus(struct c_float a, int d);
+// multiply c-f and the output of the sine_fixed routine (d parameter)
+
+
+struct c_float c_f_sum(struct c_float a[], int size);
+// sum a list of c_floats
+struct c_float hex2c_float(const char in[]);
+// read a cfloat from ascii with special format
+int cfloat2int(struct c_float a);
+// make a cloat to int (cut behind the point = floor)
+/*struct c_float cfloat2int (int a);*/
+// interpolation function
+/* 1. parameter size, 2. Amplitude over all, 3. time, 4. Coeffs array of c_float,
+ 5. frequencies (frequency * time -> parameter to sine_fixed routines*/
+
+struct c_float interpolate_rational_c_float( /*Real amplitude interpolation*/
+int size_c, struct c_float amp, int time, struct c_float coeffs[],
+ unsigned int frequencies_up[],
+ unsigned int frequencies_bottom[]);
+
+struct c_float interpolate_c_float(int, struct c_float, int,
+ struct c_float coeffs[],
+ unsigned int frequencies[]);
+
+struct c_float interpolate_c_float_phase(int, struct c_float amps[], int,
+ struct c_float coeffs[],
+ unsigned int frequencies[]);
+
+void eval_seq(const char in[]);
+/*! input interpreter of HMDP*/
+
+/* Inititalize variables, start timer, etc */
+void init_hmdl();
+
+/* Clean up */
+void end_hmdl();
+
+/*! Handle situtations when the time needs to wrap around since time_max has
+ been reached. Offset values have to be saved for the motion patterns, and
+ the update flag needs to be set */
+void handle_overflow(void);
+
+void motion_machine(unsigned int time);
+
+/*! motion control - time management+*
+
+ This function is triggered by an interrupt and call the motion m...
[truncated message content] |
