diff --git a/controllers/Makefile b/controllers/Makefile
index dec2c3919e3440b19cb33ab108c1770f43dda770..3afb4f27ee39fbe6e3ab24d166bf02aaa26da68e 100755
--- a/controllers/Makefile
+++ b/controllers/Makefile
@@ -21,7 +21,7 @@ ARFLAGS=rsc
.PHONY: all show_banner clean
-all: show_banner communications $(PROJECT).a examples
+all: show_banner communications dyn_devices $(PROJECT).a examples
show_banner:
@echo "------------------------------------------------------";
@@ -44,6 +44,9 @@ $(PROJECT).a: ${OBJS}
communications:
$(MAKE) -C ../communications
+dyn_devices:
+ $(MAKE) -C ../dyn_devices
+
examples:
$(MAKE) -C src/examples
@@ -55,3 +58,4 @@ clean:
rm -f $(OBJS)
$(MAKE) -C src/examples clean
$(MAKE) -C ../communications clean
+ $(MAKE) -C ../dyn_devices clean
diff --git a/controllers/include/buzzer.h b/controllers/include/buzzer.h
index f3337ed84673056c3ffc3274fa742aa3de2b8da6..93c96b8ac81afee2d095a0ca58ff3727bcb06c92 100644
--- a/controllers/include/buzzer.h
+++ b/controllers/include/buzzer.h
@@ -7,8 +7,6 @@
// buzzer interface
typedef enum {NOTE_DO=0,NOTE_RE=1,NOTE_MI=2,NOTE_FA=3,NOTE_SOL=4,NOTE_LA=5,NOTE_SI=6} note_t;
#define NUM_NOTES 7
-#define BUZZER_ALARM_PERIOD_MS 3000
-#define BUZZER_ALARM_NOTE NOTE_LA
/**
* \brief Function to initialize the buzzed module
@@ -35,7 +33,7 @@ void init_buzzer(void);
*
* \return 0 if the function completes successfully and 0xFF otherwise.
*/
-void buzzer_start(note_t note);
+void buzzer_start(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
/**
* \brief Function to change the note is being played
*
@@ -48,6 +46,8 @@ void buzzer_start(note_t note);
* \return 0 if the function completes successfully and 0xFF otherwise.
*/
void buzzer_change_note(note_t note);
+void buzzer_change_on_time(uint16_t time_100ms);
+void buzzer_change_off_time(uint16_t time_100ms);
/**
* \brief Function to stop the buzzer module
*
diff --git a/controllers/src/cm510/adc.c b/controllers/src/cm510/adc.c
index d7e3af71120637ca55f7edfc1f99133be508db04..f5899c044773fe0b5acf9c8c5e0c9a4c3a3a8fc1 100644
--- a/controllers/src/cm510/adc.c
+++ b/controllers/src/cm510/adc.c
@@ -1,9 +1,10 @@
#include "adc.h"
#include "gpio.h"
+#include "buzzer.h"
#include <util/delay.h>
/* external functions */
-void buzzer_play_alarm(void);
+void buzzer_start_alarm(note_t note, uint16_t on_time_100ms, uint16_t off_time_100ms);
void buzzer_stop_alarm(void);
uint8_t buzzer_is_playing_alarm(void);
@@ -95,17 +96,13 @@ void adc_loop(void)
// compare with hysteresis
if(buzzer_is_playing_alarm())
{
- if(data<(ADC_VOLTAGE_ALARM_TH+20))// voltage under 11 V
- buzzer_play_alarm();
- else
+ if(data>=(ADC_VOLTAGE_ALARM_TH+20))// voltage under 11 V
buzzer_stop_alarm();
}
else
{
if(data<(ADC_VOLTAGE_ALARM_TH-20))// voltage under 11 V
- buzzer_play_alarm();
- else
- buzzer_stop_alarm();
+ buzzer_start_alarm(NOTE_LA,30,30);
}
}
adc_current_ch=(adc_current_ch+1)%NUM_ADC;
diff --git a/controllers/src/cm510/buzzer.c b/controllers/src/cm510/buzzer.c
index 7dda00be2a64002476ca3cf4310fd418eb14a9e9..0222db063d5f7ac3b3d83c9ed4baec120118e8d9 100644
--- a/controllers/src/cm510/buzzer.c
+++ b/controllers/src/cm510/buzzer.c
@@ -6,49 +6,91 @@
uint8_t buzzer_note_freq[NUM_NOTES];
uint8_t buzzer_playing;
uint8_t buzzer_playing_alarm;
-uint16_t buzzer_alarm_period;
+uint16_t buzzer_time_on_100ms;
+uint16_t buzzer_time_off_100ms;
+uint16_t buzzer_note;
/* private functions */
-void buzzer_play_alarm(void)
+void buzzer_loop(void)
{
- static uint8_t playing_alarm=0x00;
+ static uint8_t playing=0x00;
+ static uint16_t current_time_on=0,current_time_off=0;
- if(buzzer_playing_alarm==0x00)
- {
- if(buzzer_playing)
- buzzer_stop();
- buzzer_start(NOTE_LA);
- buzzer_playing_alarm=0x01;
- }
- buzzer_alarm_period+=ADC_SAMPLE_PERIOD_MS;
- if(buzzer_alarm_period>1000)// 1 second
+ if(TIFR3&(1<<OCF3A))
{
- buzzer_alarm_period=0;
- if(playing_alarm)
+ TIFR3|=(1<<OCF3A);// clear any interrupt
+ TCNT3H=0x00;
+ TCNT3L=0x00;
+ if(playing==0x00)// currently not playing
{
- playing_alarm=0x00;
- // disable the output
- TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
- }
- else
+ if(buzzer_playing==0x01 || buzzer_playing_alarm==0x01)
+ {
+ playing=0x01;
+ current_time_on=buzzer_time_on_100ms-1;
+ current_time_off=buzzer_time_off_100ms;
+ if(current_time_on==0)
+ {
+ // disable PWM output
+ TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
+ }
+ }
+ }
+ else// currently playing
{
- playing_alarm=0x01;
- // enable the output
- TCCR1A&=(~(1<<COM1A0));
- TCCR1A|=(1<<COM1A1);
+ if(buzzer_playing==0x01 || buzzer_playing_alarm==0x01)
+ {
+ if(current_time_on>0)
+ {
+ current_time_on--;
+ if(current_time_on==0)
+ {
+ // disable PWM output
+ TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
+ }
+ }
+ else
+ {
+ if(current_time_off>0)
+ current_time_off--;
+ else
+ {
+ // enable the output
+ TCCR1A&=(~(1<<COM1A0));
+ TCCR1A|=(1<<COM1A1);
+ current_time_on=buzzer_time_on_100ms;
+ current_time_off=buzzer_time_off_100ms;
+ }
+ }
+ }
+ else
+ playing=0x00;
}
}
}
+void buzzer_start_alarm(note_t note, uint16_t on_time_100ms,uint16_t off_time_100ms)
+{
+ if(buzzer_playing_alarm==0x00)// the alram is not playing
+ {
+ if(buzzer_playing)// if the normal buzzer is playing, stop it.
+ buzzer_stop();
+ buzzer_playing_alarm=0x01;
+ buzzer_time_on_100ms=on_time_100ms;
+ buzzer_time_off_100ms=off_time_100ms;
+ buzzer_note=note;
+ // enable PWM output
+ TCCR1A&=(~(1<<COM1A0));
+ TCCR1A|=(1<<COM1A1);
+ }
+}
+
void buzzer_stop_alarm(void)
{
- if(buzzer_playing_alarm)
+ if(buzzer_playing_alarm==0x01)
{
- buzzer_playing_alarm=0x00;
- TCCR1B=(TCCR1B & ~0x07);// set the prescaler to 0
- // turn off channel A (OC1A) PWM output
- // set OC1A (OutputCompare action) to none
+ // disable PWM output
TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
+ buzzer_playing_alarm=0x00;
}
}
@@ -60,17 +102,40 @@ void init_buzzer(void)
PORTB &= 0xDF;// clear the ouput
// initialize timer 1 to work as PWM (Fast PWM mode)
- TCCR1B=((TCCR1B & ~0x07) | 0x00);// set the prescaler to 1024
+ TIMSK1=0x00;// disable all interrupts
+ TIFR1=0x2F;// clear any pending interrupt
TCNT1H=0x00;
TCNT1L=0x00;
// set PWM mode with ICR top-count
+ TCCR1A=0x00;
TCCR1A&=~(1<<WGM10);
TCCR1A|=(1<<WGM11);
+ TCCR1B=0x00;
TCCR1B|=(1<<WGM12);
TCCR1B|=(1<<WGM13);
// clear output compare value A
OCR1AH=0x00;
OCR1AL=0x00;
+ TCCR1B&=0xF8;// set the prescaler to 1024
+ TCCR1B|=0x05;// set the prescaler to 1024
+
+ // initialize timer3 for buzzer time control (100ms period)
+ TIMSK3=0x00;// disable all interrupts
+ TIFR3=0x2F;// clear any pending interrupt
+ TCNT3H=0x00;
+ TCNT3L=0x00;
+ // set PWM mode with ICR top-count (CTC mode)
+ TCCR3A=0x00;
+ TCCR3A&=~(1<<WGM30);
+ TCCR3A&=~(1<<WGM31);
+ TCCR3B=0x00;
+ TCCR3B|=(1<<WGM32);
+ TCCR3B&=~(1<<WGM33);
+ // clear output compare value A
+ OCR3AH=0x18;
+ OCR3AL=0x6A;
+ TCCR3B&=0xF8;// set the prescaler to 256
+ TCCR3B|=0x04;
// initialize the note frequencies
buzzer_note_freq[0]=29;
@@ -82,24 +147,27 @@ void init_buzzer(void)
buzzer_note_freq[6]=16;
buzzer_playing=0x00;
buzzer_playing_alarm=0x00;
- buzzer_alarm_period=0x00;
+ buzzer_time_on_100ms=0x01;
+ buzzer_time_off_100ms=0x01;
+ buzzer_note=NOTE_DO;
}
-void buzzer_start(note_t note)
+void buzzer_start(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms)
{
if(buzzer_playing_alarm==0x00)
{
if(buzzer_playing==0x00)
{
- TCCR1B=((TCCR1B & ~0x07) | 0x05);// set the prescaler to 1024
// set top count value
ICR1H = 0x00;
ICR1L = buzzer_note_freq[note];
// Set the compare value to half the period
OCR1AH = 0x00;
OCR1AL = buzzer_note_freq[note]>>1;
- // turn on channel A (OC1A) PWM output
- // set OC1A as non-inverted PWM
+ buzzer_note=note;
+ buzzer_time_on_100ms=on_time_100ms;
+ buzzer_time_off_100ms=off_time_100ms;
+ // enable PWM output
TCCR1A&=(~(1<<COM1A0));
TCCR1A|=(1<<COM1A1);
buzzer_playing=0x01;
@@ -111,29 +179,44 @@ void buzzer_change_note(note_t note)
{
if(buzzer_playing_alarm==0x00)
{
- if(buzzer_playing)
+ if(buzzer_playing==0x01)
{
- cli();
// set top count value
ICR1H = 0x00;
ICR1L = buzzer_note_freq[note];
// Set the compare value to half the period
OCR1AH = 0x00;
OCR1AL = buzzer_note_freq[note]>>1;
- sei();
+ buzzer_note=note;
}
}
}
+void buzzer_change_on_time(uint16_t time_100ms)
+{
+ if(buzzer_playing_alarm==0x00)
+ {
+ if(buzzer_playing==0x01)
+ buzzer_time_on_100ms=time_100ms;
+ }
+}
+
+void buzzer_change_off_time(uint16_t time_100ms)
+{
+ if(buzzer_playing_alarm==0x00)
+ {
+ if(buzzer_playing==0x01)
+ buzzer_time_off_100ms=time_100ms;
+ }
+}
+
void buzzer_stop(void)
{
if(buzzer_playing_alarm==0x00)
{
- if(buzzer_playing)
+ if(buzzer_playing==0x01)
{
- TCCR1B=(TCCR1B & ~0x07);// set the prescaler to 0
- // turn off channel A (OC1A) PWM output
- // set OC1A (OutputCompare action) to none
+ // disable PWM output
TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0)));
buzzer_playing=0x00;
}
diff --git a/controllers/src/cm510/cm510.c b/controllers/src/cm510/cm510.c
index b21a89a382e7421310df4b7d7cd562b356b69c3c..0b25730059316d7fff64f3498df39de48bba5edf 100755
--- a/controllers/src/cm510/cm510.c
+++ b/controllers/src/cm510/cm510.c
@@ -31,6 +31,7 @@ extern void user_loop(void);
/* internal loop functions */
extern void pushbuttons_loop(void);
extern void adc_loop(void);
+extern void buzzer_loop(void);
// general interface
void init_cm510(void)
@@ -52,6 +53,7 @@ int16_t main(void)
{
pushbuttons_loop();
adc_loop();
+ buzzer_loop();
user_loop();
}
}
diff --git a/controllers/src/examples/main.c b/controllers/src/examples/main.c
index e7fe9e1e22c4466703c190af0c7512aef2836cbf..a3a9ce57bc59b46b152cf0bff56b32bcd989ecf1 100755
--- a/controllers/src/examples/main.c
+++ b/controllers/src/examples/main.c
@@ -34,13 +34,16 @@ void user_loop(void)
turn_led_on(LED_AUX);
else
turn_led_off(LED_AUX);
- if(get_adc_avg_channel(ADC_VCC)<512)
- turn_led_on(LED_PLAY);
+// if(get_adc_avg_channel(ADC_VCC)<512)
+// turn_led_on(LED_PLAY);
+// else
+// turn_led_off(LED_PLAY);
+ if(is_button_falling_edge(BTN_UP))
+ buzzer_start(NOTE_SOL,30,30);
else
- turn_led_off(LED_PLAY);
- if(is_button_pressed(BTN_UP))
- buzzer_start(NOTE_SOL);
- else
- buzzer_stop();
+ {
+ if(is_button_falling_edge(BTN_DOWN))
+ buzzer_stop();
+ }
printf("loop\n");
}
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index 811638bbbede01e4381d57ee6c10b3d1bab7071c..b7af2ae6f9ff4695a5df146b5336387cc0a1bb0f 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -1,5 +1,5 @@
PROJECT=libdyn_devices
-SOURCES=src/dyn_servos/dyn_servos.c src/exp_board/exp_board.c
+SOURCES=src/dyn_servos/dyn_servos.c# src/exp_board/exp_board.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
BIN_DIR=./build/
@@ -19,7 +19,7 @@ ARFLAGS=rsc
.PHONY: all show_banner clean
-all: show_banner $(PROJECT).a
+all: show_banner $(PROJECT).a examples
show_banner:
@echo "------------------------------------------------------";
@@ -34,11 +34,15 @@ show_banner:
@echo "------------------------------------------------------";
$(PROJECT).a: ${OBJS}
- $(OBJCOPY) $(ARFLAGS) ${LDIR}$(PROJECT).a $(LIBS) $(OBJS)
+ $(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(LIBS) $(OBJS)
%.o: %.c
$(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $<
+examples:
+ $(MAKE) -C src/examples
+
clean:
- rm -f ${LDIR}$(PROJECT).a
+ rm -f ${LIB_DIR}$(PROJECT).a
rm -f $(OBJS)
+ $(MAKE) -C src/examples clean
diff --git a/dyn_devices/include/dyn_servos.h b/dyn_devices/include/dyn_servos.h
index e5fdef77eb6ecb92261a33ab75035421eb3b4a90..f7b0ba738b0413ce43fe7890c203a9a701dc299a 100755
--- a/dyn_devices/include/dyn_servos.h
+++ b/dyn_devices/include/dyn_servos.h
@@ -18,6 +18,9 @@
#ifndef _DYN_SERVOS_H
#define _DYN_SERVOS_H
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
// error definitions
#define INSTRUCTION_ERROR 0x40
#define OVERLOAD_ERROR 0x20
diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..f5be84ed36e5399b332a24ba6b8abab7ce157c44
--- /dev/null
+++ b/dyn_devices/src/examples/Makefile
@@ -0,0 +1,41 @@
+PROJECT=dyn_servos_ex
+########################################################
+# afegir tots els fitxers que s'han de compilar aquÃ
+########################################################
+SOURCES=main.c
+
+OBJS=$(SOURCES:.c=.o)
+SRC_DIR=./src/
+DEV_DIR=../../
+COMM_DIR=../../../communications/
+CC=avr-gcc
+OBJCOPY=avr-objcopy
+MMCU=atmega2561
+
+LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+
+INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include
+
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+
+LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
+
+HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
+
+.PHONY: all
+
+all: $(PROJECT).hex
+
+$(PROJECT).hex: $(PROJECT).elf
+ $(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS) $< $@
+$(PROJECT).elf: $(OBJS)
+ $(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf
+%.o:%.c
+ $(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $<
+
+download: $(MAIN_OUT_HEX)
+ fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new
+
+clean:
+ -rm $(PROJECT).*
+ -rm $(OBJS)
diff --git a/dyn_devices/src/examples/main.c b/dyn_devices/src/examples/main.c
index 71c5f61d8d6e5c3b53c01e5e1ca8edaccad61ef3..f9ad49f2833c93b6444ef382640f07026af36b93 100755
--- a/dyn_devices/src/examples/main.c
+++ b/dyn_devices/src/examples/main.c
@@ -15,22 +15,42 @@
* along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>.
*/
-#include "dyn_servos.h"
-#include "dynamixel.h"
-#include "serial.h"
-#include <stdio.h>
-
-int main(void)
-{
- unsigned short int model;
-
- serial_initialize(57600);
-
- dxl_initialize(0,1);
-
- model=get_model_number(1);
-
- printf("%d\n",model);
-
- while(1);
-}
+#include "dyn_servos.h"
+#include "dynamixel_master.h"
+#include "serial_console.h"
+#include <stdio.h>
+#include <util/delay.h>
+
+int main(void)
+{
+ unsigned char num;
+ unsigned char ids[32];
+ unsigned short int model,position;
+
+ serial_console_init(57600);
+ dyn_master_init();
+ sei();
+
+ dyn_master_scan(&num,ids);
+ if(num>0)
+ {
+ model=get_model_number(ids[0]);
+ printf("%d\n",model);
+ set_target_speed(ids[0],100);
+ for(;;)
+ {
+ set_target_position(ids[0],0);
+ do{
+ position=get_current_position(ids[0]);
+ }while(position>24);
+ set_target_position(ids[0],1023);
+ do{
+ position=get_current_position(ids[0]);
+ }while(position<1000);
+ }
+ }
+ else
+ printf("No device found");
+
+ while(1);
+}
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index d11525e49666758c5ec893b10c389c9c2d1d3d6f..af73baf40bd2f1baf60df456187de07af573fbb6 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -19,8 +19,16 @@
#include "exp_board.h"
#include "dynamixel.h"
+/* private variables */
unsigned char exp_board_id;
+/* private functions */
+void exp_board_loop(void)
+{
+
+}
+
+// public functions
// expansion board initialize
void exp_board_init(unsigned char exp_board)
{
diff --git a/motion/Makefile b/motion/Makefile
index 4c00b6013a77586b15ea1ca82a2c30a649e04f59..c89f25848cd2d3d9898e1843e3b967c9f73ba641 100755
--- a/motion/Makefile
+++ b/motion/Makefile
@@ -2,16 +2,17 @@ PROJECT=libmotion_manager
SOURCES=src/motion_manager.c src/action.c src/motion_pages.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
-INC_DIR=./include/
BIN_DIR=./build/
LIB_DIR=./lib/
+COMM_DIR=../communications/
+DEV_DIR=../dyn_devices/
CC=avr-gcc
OBJCOPY=avr-ar
MMCU=atmega2561
-SERVOS_INCLUDE_DIR=../dyn_devices/include/
+INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/
-CONTROLLER_INCLUDE_DIR=../controllers/include/
+LIBS=$(DEV_DIR)lib/libdyn_devices.a $(COMM_DIR)lib/libcomm.a
CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
@@ -35,10 +36,10 @@ show_banner:
$(PROJECT).a: ${OBJS}
- $(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS)
+ $(OBJCOPY) $(ARFLAGS) ${LDIR}$(PROJECT).a $(LIBS) $(OBJS)
%.o: %.c
- $(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $<
+ $(CC) -c $(CFLAGS) ${INC_DIRS} -o $@ $<
clean:
rm -f ${LDIR}$(PROJECT).a
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 6990c585e1117017ff3f6b47c2ca8120db425703..3a1ea88bdb7c748a4bb66fc45b51b313b1a4f192 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -26,7 +26,7 @@ typedef struct{
int16_t servo_offsets[MANAGER_MAX_NUM_SERVOS];
// public functions
-void manager_init(unsigned char num_servos);
+void manager_init(uint8_t num_servos);
void manager_set_index_value(uint8_t servo_id, uint16_t value);
void manager_set_servo_value(uint8_t servo_id, uint16_t value);
uint16_t manager_get_index_value(uint8_t servo_id);
@@ -43,10 +43,10 @@ void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope);
* After reading the value, the internal state is automatically set
* back to 0x00
*/
-unsigned char manager_check_robot_fallen(void);
+uint8_t manager_check_robot_fallen(void);
// gyro interface
-unsigned char manager_calibrate_gyro(void);
+uint8_t manager_calibrate_gyro(void);
/**
* \brief Function to enable the internal gyros
*
@@ -75,22 +75,5 @@ void manager_disable_gyro(void);
*
* \return 0x01 if the gyros are enabled and 0x00 otherwise.
*/
-unsigned char manager_is_gyro_enabled(void);
-/**
- * \brief Function to assign a used defined callback function
- *
- * By default, a function is used to balance the robot motion given the values of
- * gyroscopes. This balance function can be chnaged by the user by means of this
- * function call.
- *
- * \param function the new function to be used to balance the robot motion. The
- * prototype of this function must be as follows:
- *
- * void <func_name>(int16_t x_gyro,int16_t y_gyro, int16_t *offsets)
- *
- * where x_gyro and y_gyro are the offset values of the gyroscopes
- * and the output offsets to be applied to all the servos. This
- * last vector must have the same size as the number of servos
- * controlled by the motion module.
- */
+uint8_t manager_is_gyro_enabled(void);
#endif
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 2da0394ac36df7d3bf2046695d89eaf8a3e0a63e..5c435f671797a38464a9d84730c9812b714a06e0 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -2,60 +2,79 @@
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
-#include "dynamixel.h"
+#include "dynamixel_master.h"
#include "dyn_servos.h"
#include "dyn_servos_reg.h"
#include "action.h"
+#include "buzzer.h"
+
+// external functions
+extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
+extern void buzzer_stop_alarm(void);
// private variables
uint8_t manager_num_servos;
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
-volatile unsigned long timer0_overflow_count;
// gyroscope private variables
-unsigned short int x_gyro_center;
-unsigned short int y_gyro_center;
-unsigned char gyro_enabled;
-unsigned char robot_fallen_state;
+uint16_t x_gyro_center;
+uint16_t y_gyro_center;
+uint8_t gyro_enabled;
+uint8_t robot_fallen_state;
typedef struct
{
uint8_t cc_slope;
uint8_t ccw_slope;
uint16_t position;
-}dxl_send_struct;
+}dyn_send_data;
+
+uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+dyn_send_data data[MANAGER_MAX_NUM_SERVOS];
+TWriteData packets[MANAGER_MAX_NUM_SERVOS];
// private functions
+void manager_loop(void)
+{
+// turn_led_on(LED_PROGRAM); //debug
+
+ // call the action process
+ action_process(); //action_ready
+ // balance the robot
+ manager_balance();
+ // send the motion commands to the servos
+ manager_send_motion_command();
+
+// turn_led_off(LED_PROGRAM); //debug
+}
+
void manager_send_motion_command(void)
{
- static uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
- static dxl_send_struct packet[MANAGER_MAX_NUM_SERVOS];
- uint8_t * ppacket;
+ uint8_t *pdata;
uint8_t i;
uint16_t target_pos;
- uint8_t ccslope, ccwslope;
- ppacket = (uint8_t * )packet;
+ ppacket = (uint8_t *)data;
- for(i=0;i<manager_num_servos;i++) {
- servo_ids[i]=manager_servos[i].id;
+ for(i=0;i<manager_num_servos;i++)
+ {
target_pos = manager_servos[i].current_value + servo_offsets[i];
+ pdata[i*4+2] = target_pos&0xFF;
+ pdata[i*4+3] = (target_pos>>8)&0xFF;
if(manager_servos[i].cc_slope == 0)
- ccslope = 32;
- else ccslope = 1<<(0x0F&manager_servos[i].cc_slope);
+ pdata[i*4] = 32;
+ else
+ pdata[i*4] = 1<<(0x0F&manager_servos[i].cc_slope);
if(manager_servos[i].ccw_slope == 0)
- ccwslope = 32;
- else ccslope = 1<<(0x0F&manager_servos[i].ccw_slope);
+ pdata[i*4+1] = 32;
+ else
+ pdata[i*4+1] = 1<<(0x0F&manager_servos[i].ccw_slope);
- ppacket[i*4] = ccslope;
- ppacket[i*4+1] = ccwslope;
- ppacket[i*4+2] = target_pos&0xFF;
- ppacket[i*4+3] = (target_pos>>8)&0xFF;
+ packets[i].data=(uint8_t *)&data[i];
}
-
- dxl_sync_tx_packet(manager_num_servos,(unsigned char *)servo_ids,CW_COMP_SLOPE,(unsigned char *)packet,4);
+ dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets);
}
void manager_balance(void)
@@ -66,9 +85,8 @@ void manager_balance(void)
if(gyro_enabled == 0x01)
{
// get x,y gyroscope
- gyro_x=get_adc_channel(X_GYRO_CHANNEL)-x_gyro_center;
- gyro_y=get_adc_channel(Y_GYRO_CHANNEL)-y_gyro_center;
-
+// gyro_x=get_adc_channel(X_GYRO_CHANNEL)-x_gyro_center;
+// gyro_y=get_adc_channel(Y_GYRO_CHANNEL)-y_gyro_center;
if(gyro_x > 200)
robot_fallen_state = 0x02;
@@ -150,110 +168,64 @@ uint16_t manager_get_servo_value(uint8_t servo_id)
return 2048;
}
-uint8_t manager_scan_servos(uint8_t n_servos)
+void manager_timer_init(void)
{
- uint8_t i;
- uint8_t num = 0;
- uint8_t error;
- for(i=1;i<=n_servos;i++)
- {
- error = dxl_ping(i);
- if(error==0)
- {
- num++;
- }
- }
- return num;
-}
-
-void timer_init(void)
-{
- timer0_overflow_count=0;
- // on the ATmega168, timer 0 is also used for fast hardware pwm
- // (using phase-correct PWM would mean that timer 0 overflowed half as often
- // resulting in different millis() behavior on the ATmega8 and ATmega168)
- TCCR0A|=(1<<WGM01);
- TCCR0A|=(1<<WGM00);
- TCCR0B|=(1<<WGM02);
-
- // set timer 0 prescale factor to 1024
- TCCR0B|=(1<<CS02);
- TCCR0B|=(1<<CS00);
- // Write the value 122 on TOP
- OCR0A=0x7A;
- // enable timer 0 overflow interrupt
- TIMSK0|=(1<<TOIE0);
}
-// interrupt handlers
-SIGNAL(TIMER0_OVF_vect)
+uint8_t manager_period_done(void)
{
- turn_led_on(LED_PROGRAM); //debug
- timer0_overflow_count++;
- OCR0A=0x7A;
-
- // call the action process
- action_process(); //action_ready
-
- // balance the robot
- manager_balance();
-
- // send the motion commands to the servos
- manager_send_motion_command();
- turn_led_off(LED_PROGRAM); //debug
}
// public functions
-void manager_init(unsigned char num_servos)
+void manager_init(uint8_t num_servos)
{
uint8_t i;
uint8_t found_servos;
+
+ /* initialize private variables */
robot_fallen_state = 0x00;
// enable power to the servos
- dxl_initialize(0,1);
+ dyn_master_init();
_delay_ms(500);
- if(num_servos>MANAGER_MAX_NUM_SERVOS)
- return;
-
- manager_num_servos=num_servos;
- found_servos = manager_scan_servos(num_servos);
- if(num_servos != found_servos)
- turn_led_on(LED_MANAGE);
+ /* scan the bus for all available servos */
+ dyn_master_scan(&manager_num_servos,servo_ids);
+ if(num_servos != manager_num_servos)
+ buzzer_start_alarm(NOTE_SOL,1000,10000);
for(i=0;i<manager_num_servos;i++)
{
//set_target_speed(i+1,0);
servo_offsets[i] = 0;
-
enable_servo(i+1);
- manager_servos[i].id=i+1;
+ manager_servos[i].id=servo_ids[i];
manager_servos[i].max_value=1023;
manager_servos[i].min_value=0;
manager_servos[i].center_value=512;
- manager_servos[i].current_value=get_current_position(i+1);
+ manager_servos[i].current_value=get_current_position(servo_ids[i]);
}
- timer_init();
+ /* initialize the period timer */
+ manager_timer_init();
}
-unsigned char manager_check_robot_fallen(void)
+uint8_t manager_check_robot_fallen(void)
{
- unsigned char state = robot_fallen_state;
+ uint8_t state = robot_fallen_state;
robot_fallen_state = 0x00;
return state;
}
-unsigned char manager_calibrate_gyro(void)
+uint8_t manager_calibrate_gyro(void)
{
- unsigned short int x_gyro_values[GYRO_CAL_NUM_SAMPLES];
- unsigned short int y_gyro_values[GYRO_CAL_NUM_SAMPLES];
+ uint16_t x_gyro_values[GYRO_CAL_NUM_SAMPLES];
+ uint16_t y_gyro_values[GYRO_CAL_NUM_SAMPLES];
float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0;
- unsigned char calibrated=0x00;
- unsigned char i=0,count=0;
+ uint8_t calibrated=0x00;
+ uint8_t i=0,count=0;
x_gyro_center=0;
y_gyro_center=0;
@@ -262,8 +234,8 @@ unsigned char manager_calibrate_gyro(void)
for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++)
{
_delay_ms(50);
- x_gyro_values[i]=get_adc_channel(X_GYRO_CHANNEL);
- y_gyro_values[i]=get_adc_channel(Y_GYRO_CHANNEL);
+// x_gyro_values[i]=get_adc_channel(X_GYRO_CHANNEL);
+// y_gyro_values[i]=get_adc_channel(Y_GYRO_CHANNEL);
x_gyro_average+=x_gyro_values[i];
y_gyro_average+=y_gyro_values[i];
}
@@ -280,8 +252,8 @@ unsigned char manager_calibrate_gyro(void)
if(std_x<20.0 && std_y<20.0)
{
calibrated=0x01;
- x_gyro_center=(unsigned short int)x_gyro_average;
- y_gyro_center=(unsigned short int)y_gyro_average;
+ x_gyro_center=(uint16_t)x_gyro_average;
+ y_gyro_center=(uint16_t)y_gyro_average;
}
else
{
@@ -303,7 +275,7 @@ void manager_disable_gyro(void)
gyro_enabled=0x00;
}
-unsigned char manager_is_gyro_enabled(void)
+uint8_t manager_is_gyro_enabled(void)
{
return gyro_enabled;
}