[tuxdroid-svn] r841 - daemon/trunk daemon/trunk/libs firmware/tuxdefs
Status: Beta
Brought to you by:
ks156
Menu
▾
▴
[tuxdroid-svn] r841 - daemon/trunk daemon/trunk/libs firmware/tuxdefs
|
From: Paul_R <c2m...@c2...> - 2008-01-11 10:30:33
|
Author: Paul_R
Date: 2008-01-11 11:30:31 +0100 (Fri, 11 Jan 2008)
New Revision: 841
Added:
daemon/trunk/movements.c
daemon/trunk/movements.h
Modified:
daemon/trunk/Makefile
daemon/trunk/libs/USBDaemon_command_tux.c
daemon/trunk/libs/USBDaemon_status_table.c
daemon/trunk/libs/USBDaemon_status_table.h
daemon/trunk/status.h
firmware/tuxdefs/commands.h
firmware/tuxdefs/defines.h
Log:
* Added the movements module :
- Added the module in the makefile
- Added the doxygen comments
- Modified some definitions on defines.h and commands.h
* Minor change on the status to store the wings positions.
Modified: daemon/trunk/Makefile
===================================================================
--- daemon/trunk/Makefile 2008-01-11 09:08:02 UTC (rev 840)
+++ daemon/trunk/Makefile 2008-01-11 10:30:31 UTC (rev 841)
@@ -47,6 +47,7 @@
$(OBJ_DIR)/supervise.o \
$(OBJ_DIR)/versioning.o \
$(OBJ_DIR)/led.o \
+ $(OBJ_DIR)/movements.o \
$(OBJ_DIR)/log.o
define build_target
@@ -107,6 +108,9 @@
$(OBJ_DIR)/led.o: led.c led.h
$(compile_source)
+$(OBJ_DIR)/movements.o: movements.c movements.h
+ $(compile_source)
+
$(OBJ_DIR)/status.o: status.c status.h
$(compile_source)
Modified: daemon/trunk/libs/USBDaemon_command_tux.c
===================================================================
--- daemon/trunk/libs/USBDaemon_command_tux.c 2008-01-11 09:08:02 UTC (rev 840)
+++ daemon/trunk/libs/USBDaemon_command_tux.c 2008-01-11 10:30:31 UTC (rev 841)
@@ -31,6 +31,7 @@
#include "../tuxdefs/commands.h"
#include "USBDaemon_pidfile.h"
#include "../led.h"
+#include "../movements.h"
#include "../status.h"
/*_____________________ D E F I N I T I O N S ______________________________*/
@@ -361,6 +362,14 @@
/* Command type */
switch (data[0])
{
+ case TUX_CMD_STRUCT_MOTORS:
+ {
+ int timeout;
+ timeout = (data[3] << 8) + data[4];
+ ACK = movements_control(data[1], data[2], (float)timeout / 100,
+ data[5], data[6], data[7]);
+ break;
+ }
case TUX_CMD_STRUCT_EYES:
switch (data[1])
{
Modified: daemon/trunk/libs/USBDaemon_status_table.c
===================================================================
--- daemon/trunk/libs/USBDaemon_status_table.c 2008-01-11 09:08:02 UTC (rev 840)
+++ daemon/trunk/libs/USBDaemon_status_table.c 2008-01-11 10:30:31 UTC (rev 841)
@@ -691,6 +691,7 @@
case STATUS_POSITION2_CMD:
if (position2.Byte != new_status[1])
position2_changed(new_status[1]);
+ tux_status.io.wings_low = (bool)new_status[2];
break;
case STATUS_LED_CMD:
Modified: daemon/trunk/libs/USBDaemon_status_table.h
===================================================================
--- daemon/trunk/libs/USBDaemon_status_table.h 2008-01-11 09:08:02 UTC (rev 840)
+++ daemon/trunk/libs/USBDaemon_status_table.h 2008-01-11 10:30:31 UTC (rev 841)
@@ -123,6 +123,7 @@
#define TUX_CMD_STRUCT_PING 0x0B
#define TUX_CMD_STRUCT_SLEEP 0x0C
#define TUX_CMD_STRUCT_AUDIO_CHANNEL 0x0D
+#define TUX_CMD_STRUCT_MOTORS 0x0E
/* Tux structured sub commands */
#define TUX_CMD_STRUCT_SUB_ON 0x01
@@ -143,6 +144,7 @@
#define TUX_CMD_STRUCT_SUB_ERASE 0x0E
#define TUX_CMD_STRUCT_SUB_SET 0x0F
#define TUX_CMD_STRUCT_SUB_PULSE 0x10
+#define TUX_CMD_STRUCT_SUB_CONFIG 0x11
/* Tux request information */
#define TUX_REQ_INFO_VERSION 0x01
Added: daemon/trunk/movements.c
===================================================================
--- daemon/trunk/movements.c (rev 0)
+++ daemon/trunk/movements.c 2008-01-11 10:30:31 UTC (rev 841)
@@ -0,0 +1,283 @@
+/*
+ * Tux Droid - USB Daemon
+ * Copyright (C) 2007 KYSOH S.A. <in...@ky...>
+ *
+ * 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; either version 2, or (at your option)
+ * any later version.
+ *
+ * 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., 59 Temple Place - Suite 330, Boston, MA
+ * 02111-1307, USA.
+ */
+
+/* $Id: led.c 830 2008-01-04 16:59:01Z jaguarondi $ */
+
+
+
+#include <assert.h>
+#include <math.h>
+
+#include "movements.h"
+#include "status.h"
+#include "libs/USBDaemon_command_tux.h"
+#include "libs/USBDaemon_status_table.h"
+#include "tuxdefs/api.h"
+#include "tuxdefs/commands.h"
+
+/*
+ * Prototypes
+ */
+int static movement_status_control(char final_state, int value, char condition);
+int static control_final_state(char motor, char final_state, int value);
+int static compute_value(char motor, float timeout);
+void static single_movement(char motor, char final_state);
+
+
+/**
+ * \brief Convert the timeout on a number of movements.
+ * \param motor Which motor should be configured.
+ * \param timeout The time (sec.) to convert.
+ * \return The number of movement to execute.
+ *
+ * The firmware use a timebase at 4ms, and is configured with a 8bit
+ * variable.
+ *
+ * The maximum timeout for the firmware is 4ms * 256. When the timeout
+ * exceed 1s, the time is converted on a number of movements.
+ *
+ * The time of each movement has been measured, and defined in movements.h.
+ */
+int static compute_value(char motor, float timeout)
+{
+ int value;
+ switch (motor)
+ {
+ case (MOT_EYES):
+ value = (int)(timeout / EYES_MOVE_TIME);
+ break;
+ case (MOT_MOUTH):
+ value = (int)(timeout / MOUTH_MOVE_TIME);
+ break;
+ case (MOT_FLIPPERS):
+ value = (int)(timeout / FLIPPERS_MOVE_TIME);
+ break;
+ case (MOT_SPIN_L):
+ value = (int)(timeout / SPIN_MOVE_TIME);
+ break;
+ case (MOT_SPIN_R):
+ value = (int)(timeout / SPIN_MOVE_TIME);
+ break;
+ }
+
+ if (value > 255)
+ value = 255;
+ return value;
+}
+
+/**
+ * \brief Control the final state.
+ * \param motor Which motor should be configured.
+ * \param final_state The wanted final state.
+ * \param value The actual number of movements value.
+ * \return The number of movements with the final_state.
+ *
+ * This function verify if the number of mouvements is OK to reach the desired
+ * final state.
+ *
+ * To do this, the actual motor state is analyzed to determine if the 'value'
+ * variable must be incremented or not.
+ *
+ * If the value is null and a final_state has been determined, a single movement
+ * is executed.
+ */
+int static control_final_state(char motor, char final_state, int value)
+{
+ int condition;
+ switch (motor)
+ {
+ case (MOT_EYES):
+ {
+ condition = !portd.bits.PB7;
+ if (value)
+ value = movement_status_control(final_state, value, condition);
+ else if (final_state != FP_UNDEFINED)
+ single_movement(motor, final_state);
+ break;
+ }
+ case (MOT_MOUTH):
+ {
+ condition = !portb.bits.PB4;
+ if (value)
+ value = movement_status_control(final_state, value, condition);
+ else if (final_state != FP_UNDEFINED)
+ single_movement(motor, final_state);
+ break;
+ }
+ case (MOT_FLIPPERS):
+ {
+ condition = tux_status.io.wings_low;
+ if (value)
+ value = movement_status_control(final_state, value, condition);
+ else if (final_state != FP_UNDEFINED)
+ single_movement(motor, final_state);
+ break;
+ }
+ }
+ return value;
+}
+
+/**
+ * \brief Control the number of mouvements.
+ * \param final_state The wanted final state.
+ * \param value The actual number of movements.
+ * \param condition Status to determine the actual state.
+ *
+ * This function determines if the number of movements must be incremented or
+ * not.
+ *
+ * The 'condition' parameter must be equal to 1 when the position is
+ * closed or lower.
+ */
+int static movement_status_control(char final_state, int value, char condition)
+{
+ if (condition == 1 && final_state == FP_OPEN && !(value % 2))
+ value ++;
+ else if (condition == 1 && final_state == FP_CLOSE && value % 2)
+ value ++;
+ else if (condition == 0 && final_state == FP_CLOSE && !(value % 2))
+ value ++;
+ else if (condition == 0 && final_state == FP_OPEN && value % 2)
+ value ++;
+ return value;
+}
+
+/**
+ * \brief Determine the command to send.
+ * \param motor Which motor should be configured.
+ * \param final_state The wanted final state.
+ *
+ * Each single movements has its own command.
+ *
+ * This function determine which command must to be sent to Tux.
+ */
+void single_movement(char motor, char final_state)
+{
+ switch (motor)
+ {
+ case (MOT_EYES):
+ if (final_state == FP_OPEN)
+ send_usb_tux_cmd(OPEN_EYES_CMD, 0, 0, 0);
+ else if (final_state == FP_CLOSE)
+ send_usb_tux_cmd(CLOSE_EYES_CMD, 0, 0, 0);
+ break;
+
+ case (MOT_MOUTH):
+ if (final_state == FP_OPEN)
+ send_usb_tux_cmd(OPEN_MOUTH_CMD, 0, 0, 0);
+ else if (final_state == FP_CLOSE)
+ send_usb_tux_cmd(CLOSE_MOUTH_CMD, 0, 0, 0);
+ break;
+
+ case (MOT_FLIPPERS):
+ if (final_state == FP_UP)
+ send_usb_tux_cmd(RAISE_WINGS_CMD, 0, 0, 0);
+ else if (final_state == FP_DOWN)
+ send_usb_tux_cmd(LOWER_WINGS_CMD, 0, 0, 0);
+ break;
+ }
+}
+
+/**
+ * \brief Control et configure the movements.
+ * \param motor Which motor should be configured
+ * \param counter The number of movements to execute
+ * \param timeout The duration of the movement (if counter = 0)
+ * \param speed The PWM value. Only applicable for the wings and the spinning.
+ * \param final_state The final state to reach
+ * \param refresh Flag indicate if the entire command must be sent, or just the
+ * new PWM value.
+ * \return ack of tux command.
+ *
+ * This function analyzes the command received from the API.
+ * - 'Refresh' has the maximum priority. If this flag is set, only the command
+ * to change the PWM value is sent.
+ * - Timeout has priority on counter.
+ * - If timeout's value is defined, the movement will be executed for the
+ * specified time.
+ * - If timeout is lower than 300ms, the timeout will be controlled by the
+ * Tux's firmwares. In this case, the final state will be ignored.
+ * - Else, if timeout's value is greater than 300ms, the value is converted on
+ * a number of movements, and the final state is considered.
+ * - If 'counter' has been specified, a command is sent to Tux with this value.
+ * - To do an infinite movement, counter's and timeout's values must be null,
+ * and final_state must be undefined (0).
+ * - If 'counter' is equal to 0 and a final state has been specified, a single
+ * movement is executed if needed.*
+ */
+int movements_control(char motor, char counter, float timeout,
+ char speed, char final_state, char refresh)
+{
+ int ack, value;
+ char type = 0;
+
+ /*
+ * Refresh has the maximum priority. If it's set, only the command to
+ * refresh the PWM value is sent to Tux
+ */
+ if (refresh)
+ {
+ ack = send_usb_tux_cmd(MOTORS_CONFIG_CMD, motor, speed, 0);
+ }
+ else
+ {
+ /*
+ * Timeout has priority on counter.
+ */
+ if (timeout == 0)
+ {
+ value = counter;
+ value = control_final_state(motor, final_state, value);
+ }
+ else
+ {
+ /*
+ * If timeout is lower than 300ms, the timeout value is sent to Tux
+ */
+ if (timeout < 0.3)
+ {
+ type = 1;
+ value = timeout / 0.004;
+ }
+ /*
+ * Else, the timeout's value is converted on a number of movements.
+ * The final state is considered
+ */
+ else
+ {
+ value = compute_value(motor, timeout);
+ value = control_final_state(motor, final_state, value);
+ }
+ }
+ /*
+ * If a normal movement must be sent, the value is not null, or if value
+ * is null, the final state must be also null (infinite movement).
+ *
+ * If the value is null and a final state has been specified, this means
+ * that a single move has already been sent. No command must be sent.
+ */
+ if (value || !(final_state))
+ {
+ ack = send_usb_tux_cmd(MOTORS_CONFIG_CMD, motor, speed, 0);
+ ack = send_usb_tux_cmd(MOTORS_SET_CMD, motor, value, type);
+ }
+ }
+ return ack;
+}
Added: daemon/trunk/movements.h
===================================================================
--- daemon/trunk/movements.h (rev 0)
+++ daemon/trunk/movements.h 2008-01-11 10:30:31 UTC (rev 841)
@@ -0,0 +1,51 @@
+/*
+ * Tux Droid - USB Daemon
+ * Copyright (C) 2007 KYSOH S.A. <in...@ky...>
+ *
+ * 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; either version 2, or (at your option)
+ * any later version.
+ *
+ * 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., 59 Temple Place - Suite 330, Boston, MA
+ * 02111-1307, USA.
+ */
+
+/* $Id: led.h 830 2008-01-04 16:59:01Z jaguarondi $ */
+
+/** \file movements.h
+ \ingroup movements
+*/
+
+/** \file movements.c
+ \ingroup movements
+*/
+
+/** \defgroup movements Movements
+ This module provides all the functions needed to make move Tux.
+*/
+/*! @{ */
+
+#ifndef _MOVEMENTS_H_
+#define _MOVEMENTS_H_
+
+#include "tuxdefs/defines.h"
+
+#define EYES_MOVE_TIME 0.2
+#define MOUTH_MOVE_TIME 0.25
+#define FLIPPERS_MOVE_TIME 0.3
+#define SPIN_MOVE_TIME 0.2
+/*
+ * Prototypes
+ */
+extern int movements_control(char motor, char counter, float timeout,
+ char speed, char final_state, char refresh);
+/*! @{ */
+#endif /* _MOVEMENTS_H_ */
Modified: daemon/trunk/status.h
===================================================================
--- daemon/trunk/status.h 2008-01-11 09:08:02 UTC (rev 840)
+++ daemon/trunk/status.h 2008-01-11 10:30:31 UTC (rev 841)
@@ -206,6 +206,7 @@
go crazy when PWM is applied so they
can't be used to determine the status of
the LEDs. */
+ bool wings_low;
} io;
};
Modified: firmware/tuxdefs/commands.h
===================================================================
--- firmware/tuxdefs/commands.h 2008-01-11 09:08:02 UTC (rev 840)
+++ firmware/tuxdefs/commands.h 2008-01-11 10:30:31 UTC (rev 841)
@@ -88,6 +88,7 @@
#define LED_L_OFF_CMD 0x1D /* Turn left LED off */
#define LED_R_ON_CMD 0x1E /* Turn right LED on */
#define LED_R_OFF_CMD 0x1F /* Turn right LED off */
+#define LED_BLINK_CMD 0xD2
/* 1st parameter: number of LED toggles */
/* 2nd parameter: delay between each LED toggle, in multiple of 4ms */
Modified: firmware/tuxdefs/defines.h
===================================================================
--- firmware/tuxdefs/defines.h 2008-01-11 09:08:02 UTC (rev 840)
+++ firmware/tuxdefs/defines.h 2008-01-11 10:30:31 UTC (rev 841)
@@ -212,18 +212,18 @@
MOT_SPIN_L,
MOT_SPIN_R,
} MOTOR_TYPE_t;
-
-
/**
* Defines indicating the final position
*/
-#define FP_EYES_CLOSE 0x80
-#define FP_EYES_OPEN 0x40
-#define FP_MOUTH_CLOSE 0x10
-#define FP_MOUTH_OPEN 0x08
-#define FP_FLIPPERS_UP 0x04
-#define FP_FLIPPERS_DOWN 0x02
+typedef enum
+{
+ FP_UNDEFINED = 0,
+ FP_OPEN = 1,
+ FP_UP = 1,
+ FP_CLOSE = 2,
+ FP_DOWN = 2,
+} MOT_FINAL_POS_t;
/**
* Type indicating the status
|
