summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/main.c15
-rw-r--r--digital/io/src/pwm.c101
-rw-r--r--digital/io/src/pwm.h45
-rw-r--r--digital/io/src/simu.host.c19
-rw-r--r--digital/io/tools/io/mex.py30
6 files changed, 211 insertions, 1 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 87486890..28ccd626 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -3,7 +3,7 @@ BASE = ../../avr
# Name of the program to build.
PROGS = io
# Sources to compile.
-io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \
+io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c pwm.c \
switch.avr.c chrono.c main_timer.avr.c \
simu.host.c
# Modules needed for IO.
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 9f9874a4..fd5b720c 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -47,6 +47,7 @@
#include "chrono.h" /* chrono_end_match */
#include "gutter.h" /* gutter_generate_wait_finished_event */
#include "sharp.h" /* sharp module */
+#include "pwm.h"
#include "playground.h"
#include "io.h"
@@ -149,6 +150,8 @@ main_init (void)
top_start ();
/* Sharp module */
sharp_init ();
+ /* PWM module */
+ pwm_init ();
/* io initialization done */
proto_send0 ('z');
@@ -193,6 +196,9 @@ main_loop (void)
return;
}
+ /* Update PWM */
+ pwm_update ();
+
/* Update TWI module to get new data from the asserv board */
asserv_update_status ();
@@ -427,6 +433,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
}
}
break;
+
case c ('s', 2):
/* Set servo motor to a desired position using the servo module.
* - 1b: servo id number;
@@ -472,6 +479,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v16 (args[3], args[4]));
break;
+ case c ('w', 4):
+ /* Set PWM.
+ * - 1w: PWM value id number;
+ * - 1w: pwm high time value (position).
+ */
+ pwm_set (v8_to_v16 (args[0], args[1]), v8_to_v16 (args[2], args[3]));
+ break;
+
/* EEPROM command */
case c ('e', 1):
{
diff --git a/digital/io/src/pwm.c b/digital/io/src/pwm.c
new file mode 100644
index 00000000..b6ee727a
--- /dev/null
+++ b/digital/io/src/pwm.c
@@ -0,0 +1,101 @@
+/* pwm.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2009 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * 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 of the License, 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.
+ *
+ * }}} */
+#include "common.h"
+#include "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "io.h"
+
+/** Delay for LMD18200. */
+#ifndef HOST
+# define PWM_LMD18200_DELAY_US 1
+#else
+# define PWM_LMD18200_DELAY_US 0
+#endif
+
+#ifdef HOST
+extern uint16_t OCR1A;
+extern uint8_t PORTB;
+#endif
+
+/** Stop timer. PWM is set to zero when it reaches zero. */
+uint16_t pwm_stop_timer;
+
+/** Initialise PWM. */
+void
+pwm_init (void)
+{
+#ifndef HOST
+ /* Fast PWM, TOP = 0x3ff, OCnA, OCnB & OCnC with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */
+ TCCR1A =
+ regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10,
+ 1, 0, 1, 0, 1, 0, 1, 1);
+ TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
+ 0, 0, 0, 0, 1, 0, 0, 1);
+ /* Enable pwm and direction outputs in DDRB. */
+ IO_DDR (PWM_IO) |= IO_BV (PWM_IO);
+ IO_DDR (PWM_DIR_IO) |= IO_BV (PWM_DIR_IO);
+#endif
+}
+
+/**
+ * Set a PWM for a given time.
+ * - value: PWM value (-PWM_MAX..+PWM_MAX).
+ * - timer: timer after which the PWM is stopped, or 0 for infinite.
+ */
+void
+pwm_set (int16_t value, uint16_t timer)
+{
+ if (value == 0)
+ {
+ PWM_OCR = 0;
+ }
+ else if (value > 0)
+ {
+ IO_PORT (PWM_DIR_IO) |= IO_BV (PWM_DIR_IO);
+ if (PWM_LMD18200_DELAY_US)
+ utils_delay_us (PWM_LMD18200_DELAY_US);
+ PWM_OCR = value;
+ }
+ else
+ {
+ IO_PORT (PWM_DIR_IO) &= ~IO_BV (PWM_DIR_IO);
+ if (PWM_LMD18200_DELAY_US)
+ utils_delay_us (PWM_LMD18200_DELAY_US);
+ PWM_OCR = -value;
+ }
+ pwm_stop_timer = timer;
+}
+
+/** Update PWM, should be called at regular interval. */
+void
+pwm_update (void)
+{
+ if (pwm_stop_timer && --pwm_stop_timer == 0)
+ pwm_set (0, 0);
+}
+
diff --git a/digital/io/src/pwm.h b/digital/io/src/pwm.h
new file mode 100644
index 00000000..ac5c821e
--- /dev/null
+++ b/digital/io/src/pwm.h
@@ -0,0 +1,45 @@
+#ifndef pwm_h
+#define pwm_h
+/* pwm.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2009 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * 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 of the License, 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.
+ *
+ * }}} */
+
+/** PWM maximum value. */
+#define PWM_MAX 0x3ff
+
+/** OCR register. */
+#define PWM_OCR OCR1A
+/** PWM io. */
+#define PWM_IO B, 5
+/** PWM direction io. */
+#define PWM_DIR_IO B, 4
+
+void
+pwm_init (void);
+void
+pwm_set (int16_t value, uint16_t timer);
+void
+pwm_update (void);
+
+#endif /* pwm_h */
diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c
index 99870e74..935eadec 100644
--- a/digital/io/src/simu.host.c
+++ b/digital/io/src/simu.host.c
@@ -27,12 +27,14 @@
#include "servo.h"
#include "sharp.h"
+#include "pwm.h"
#include "modules/utils/utils.h"
#include "modules/host/host.h"
#include "modules/host/mex.h"
#include "modules/adc/adc.h"
#include "modules/path/path.h"
+#include "io.h"
enum
{
@@ -41,6 +43,7 @@ enum
MSG_SIMU_IO_SERVO = 0xb2,
MSG_SIMU_IO_SHARPS = 0xb3,
MSG_SIMU_IO_PATH = 0xb4,
+ MSG_SIMU_IO_PWM = 0xb5,
};
/** Requested servo position. */
@@ -57,10 +60,15 @@ uint8_t servo_high_time_current_[SERVO_NUMBER];
uint8_t simu_servo_update = 10, simu_servo_update_cpt;
uint8_t simu_switch_update = 100, simu_switch_update_cpt;
uint8_t simu_sharps_update = 9, simu_sharps_update_cpt;
+uint8_t simu_pwm_update = 10, simu_pwm_update_cpt;
/** Sampled switches. */
uint8_t simu_switches;
+/** PWM registers. */
+uint16_t OCR1A;
+uint8_t PORTB;
+
/** Initialise simulation. */
void
simu_init (void)
@@ -68,6 +76,8 @@ simu_init (void)
mex_node_connect ();
simu_servo_update_cpt = 1;
simu_switch_update_cpt = 1;
+ simu_sharps_update_cpt = 1;
+ simu_pwm_update_cpt = 1;
}
/** Make a simulation step. */
@@ -128,6 +138,15 @@ simu_step (void)
mex_msg_pop (m, "H", &adc_values[i]);
mex_msg_delete (m);
}
+ /* Send PWM. */
+ if (simu_pwm_update && !--simu_pwm_update_cpt)
+ {
+ simu_pwm_update_cpt = simu_pwm_update;
+ m = mex_msg_new (MSG_SIMU_IO_PWM);
+ mex_msg_push (m, "h", (IO_PORT (PWM_DIR_IO) & IO_BV (PWM_DIR_IO))
+ ? PWM_OCR : -PWM_OCR);
+ mex_node_send (m);
+ }
}
void
diff --git a/digital/io/tools/io/mex.py b/digital/io/tools/io/mex.py
index 98886d1a..52069db1 100644
--- a/digital/io/tools/io/mex.py
+++ b/digital/io/tools/io/mex.py
@@ -30,12 +30,16 @@ ID_COLOR = 0xb1
ID_SERVO = 0xb2
ID_ADC = 0xb3
ID_PATH = 0xb4
+ID_PWM = 0xb5
SERVO_NB = 6
SERVO_VALUE_MAX = 255
ADC_NB = 5
+PWM_NB = 1
+PWM_VALUE_MAX = 1024
+
class Mex:
"""Handle communications with simulated io."""
@@ -129,6 +133,30 @@ class Mex:
self.path.append (msg.pop ('hh'))
self.notify ()
+ class PWM (Observable):
+ """PWM output.
+
+ - value: current PWM value (-1 ... +1).
+
+ """
+
+ def __init__ (self):
+ Observable.__init__ (self)
+ self.value = None
+
+ class Pack:
+ """Handle reception of several PWM for one message."""
+
+ def __init__ (self, node, list):
+ self.__list = list
+ node.register (ID_PWM, self.__handle)
+
+ def __handle (self, msg):
+ values = msg.pop ('%dh' % len (self.__list))
+ for pwm, value in zip (self.__list, values):
+ pwm.value = float (value) / PWM_VALUE_MAX
+ pwm.notify ()
+
def __init__ (self, node):
self.jack = self.Switch (node, ID_JACK)
self.color_switch = self.Switch (node, ID_COLOR)
@@ -137,4 +165,6 @@ class Mex:
self.adc = tuple (self.ADC () for i in range (0, ADC_NB))
self.__adc_pack = self.ADC.Pack (node, self.adc)
self.path = self.Path (node)
+ self.pwm = tuple (self.PWM () for i in range (0, PWM_NB))
+ self.__adc_pwm = self.PWM.Pack (node, self.pwm)