diff --git a/communications/Makefile b/communications/Makefile deleted file mode 100644 index cd714da6eb59cc665f3559b63d0fa3035d3048ca..0000000000000000000000000000000000000000 --- a/communications/Makefile +++ /dev/null @@ -1,46 +0,0 @@ -PROJECT=libcomm -######################################################## -# afegir tots els fitxers que s'han de compilar aquà -######################################################## -SOURCES=src/dynamixel.c src/dynamixel_master.c src/serial_console.c - -OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -INCLUDE_DIR=./include/ -BUILD_DIR=./build/ -BIN_DIR=./bin/ -LIB_DIR=./lib/ -CC=avr-gcc -OBJCOPY=avr-ar -MMCU=atmega2561 - -CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes - -ARFLAGS=rsc - -.PHONY: show_banner all - -all: show_banner $(PROJECT).a - -show_banner: - @echo "------------------------------------------------------"; - @echo " _____ "; - @echo " / _ \ "; - @echo " | |_| | The Humanoid Lab "; - @echo " ____\_____/____ "; - @echo " / \ http://apollo.upc.es/humanoide/ "; - @echo "/ _ _ \ "; - @echo "| | | | | | $(PROJECT) "; - @echo "| | | | | | "; - @echo "------------------------------------------------------"; - -$(PROJECT).a: $(OBJS) - $(OBJCOPY) $(ARFLAGS) $(LIB_DIR)$(PROJECT).a $(OBJS) -%.o:%.c - $(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $< -examples: - $(MAKE) -C src/examples -clean: - -rm $(LIB_DIR)$(PROJECT).* - -rm $(OBJS) - $(MAKE) -C src/examples clean diff --git a/communications/include/comm_cfg.h b/communications/include/comm_cfg.h deleted file mode 100644 index 454edd881749c3ab5c5fa892726b6ca083a13006..0000000000000000000000000000000000000000 --- a/communications/include/comm_cfg.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef _COMM_CFG_H -#define _COMM_CFG_H - -#ifndef DYN_MASTER_MAX_TX_BUFFER_LEN - #define DYN_MASTER_MAX_TX_BUFFER_LEN 128 -#endif - -#ifndef DYN_MASTER_MAX_RX_BUFFER_LEN - #define DYN_MASTER_MAX_RX_BUFFER_LEN 128 -#endif - -#ifndef DYN_MASTER_DEFAULT_BAUDRATE - #define DYN_MASTER_DEFAULT_BAUDRATE 1000000 -#endif - -#ifndef DYN_MASTER_DEFAULT_TIMEOUT_US - #define DYN_MASTER_DEFAULT_TIMEOUT_US 10000 -#endif - -#ifndef SERIAL_CONSOLE_MAX_BUFFER_LEN - #define SERIAL_CONSOLE_MAX_BUFFER_LEN 128 -#endif - -#endif diff --git a/communications/include/dynamixel.h b/communications/include/dynamixel.h deleted file mode 100644 index 40736bd92d89d9ce75a9ecaf9376a34c8ac3931b..0000000000000000000000000000000000000000 --- a/communications/include/dynamixel.h +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef _DYNAMIXEL_H -#define _DYNAMIXEL_H - -#include "dynamixel_common.h" -#include <avr/io.h> -#include <avr/interrupt.h> - -#define DYN_HEADER_OFF 0 -#define DYN_ID_OFF 2 -#define DYN_LENGTH_OFF 3 -#define DYN_INST_OFF 4 -#define DYN_ERROR_OFF 4 -#define DYN_DATA_OFF 5 - -void dyn_copy_packet(uint8_t *source, uint8_t *destination); -inline uint8_t dyn_get_id(uint8_t *packet) -{ - return packet[DYN_ID_OFF]; -} -inline uint8_t dyn_get_length(uint8_t *packet) -{ - return packet[DYN_LENGTH_OFF]; -} -inline TDynInstruction dyn_get_instruction(uint8_t *packet) -{ - return packet[DYN_INST_OFF]; -} -uint8_t dyn_check_checksum(uint8_t *packet); - -// instruction packet -/* -typedef struct{ - uint8_t header[2]; - uint8_t id; - uint8_t length; - uint8_t instruction; - uint8_t data[MAX_DATA_LENGTH]; - uint8_t checksum; -}TDynInst; -*/ -/* ping instruction */ -void dyn_init_ping_packet(uint8_t *packet,uint8_t id); -/* read instruction */ -void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length); -inline uint8_t dyn_get_read_length(uint8_t *packet) -{ - return packet[DYN_DATA_OFF+1]; -} -inline uint8_t dyn_get_read_address(uint8_t *packet) -{ - return packet[DYN_DATA_OFF]; -} -/* write instruction */ -void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data); -inline uint8_t dyn_get_write_address(uint8_t *packet) -{ - return packet[DYN_DATA_OFF]; -} -inline uint8_t dyn_get_write_length(uint8_t *packet) -{ - return packet[DYN_LENGTH_OFF]-3; -} -uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data); -/* registered write instruction */ -void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data); -inline uint8_t dyn_get_reg_write_address(uint8_t *packet) -{ - return packet[DYN_DATA_OFF]; -} -inline uint8_t dyn_get_reg_write_length(uint8_t *packet) -{ - return packet[DYN_LENGTH_OFF]-3; -} -uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data); -/* action instruction */ -void dyn_init_action_packet(uint8_t *packet); -/* reset instruction */ -void dyn_init_reset_packet(uint8_t *packet,uint8_t id); -/* sync write instruction */ -void dyn_init_sync_write_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t address,uint8_t length,TWriteData *data); -uint8_t dyn_sync_write_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length,uint8_t *data); -/* bulk read instruction */ -void dyn_init_bulk_read_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t *address,uint8_t *length); -uint8_t dyn_bulk_read_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length); - -// status packet -/* -typedef struct{ - uint8_t header[2]; - uint8_t id; - uint8_t length; - uint8_t error; - uint8_t data[MAX_DATA_LENGTH]; - uint8_t checksum; -}TDynStatus; -*/ - -void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t length,uint8_t *data); -inline TDynError dyn_get_status_error(uint8_t *packet) -{ - return packet[DYN_ERROR_OFF]; -} -/* read instruction status packet */ -uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data); - -#endif diff --git a/communications/include/dynamixel_common.h b/communications/include/dynamixel_common.h deleted file mode 100644 index ea125c550f3593b27a54b4f6469a947ac5f14446..0000000000000000000000000000000000000000 --- a/communications/include/dynamixel_common.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef _DYNAMIXEL_COMMON_H -#define _DYNAMIXEL_COMMON_H - -#define MAX_DATA_LENGTH 255 - -#define MAX_HEADER_LENGTH 6 - -// errors -#define DYN_SUCCESS 0 -#define DYN_BAD_FORMAT 1 -#define DYN_NO_DEVICE 2 -#define DYN_TIMEOUT 3 -#define DYN_COMM_ERROR 4 - -typedef struct{ - unsigned char *data_addr; -}TWriteData; - -typedef enum{no_return=0x00, - return_only_read=0x01, - return_all=0x02} return_level_t; - -// possible packet types -typedef enum{DYN_PING=0x01, - DYN_READ=0x02, - DYN_WRITE=0x03, - DYN_REG_WRITE=0x04, - DYN_ACTION=0x05, - DYN_RESET=0x06, - DYN_SYNC_READ=0x82, - DYN_SYNC_WRITE=0x83, - DYN_BULK_READ=0x92, - DYN_BULK_WRITE=0x93} TDynInstruction; - -// boradcast ID -#define DYN_BROADCAST_ID 0xFE - -// status packet -typedef enum{DYN_NO_ERROR=0x00, - DYN_INST_ERROR=0x40, - DYN_OVERLOAD_ERROR=0x20, - DYN_CHECKSUM_ERROR=0x10, - DYN_RANGE_ERROR=0x08, - DYN_OVERTEMP_ERROR=0x04, - DYN_ANGLE_ERROR=0x02, - DYN_VOLTAGE_ERROR=0x01} TDynError; - -typedef enum{DYN_VER1=0x01,DYN_VER2=0x02} TDynVersion; -#endif diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h deleted file mode 100644 index 9fa62737f3c26e3e75d2bfbc888cc93d2524dd66..0000000000000000000000000000000000000000 --- a/communications/include/dynamixel_master.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef _DYNAMIXEL_MASTER_H -#define _DYNAMIXEL_MASTER_H - -#include "dynamixel.h" - -/* public functions */ -void dyn_master_init(void); -void dyn_master_set_rx_timeout(uint16_t timeout_ms); -void dyn_master_set_return_level(return_level_t level); -return_level_t dyn_master_get_return_level(void); -void dyn_master_set_baudrate(uint32_t baudrate); -uint32_t dyn_master_get_baudrate(void); -void dyn_master_scan(uint8_t *num,uint8_t *ids); -uint8_t dyn_master_ping(uint8_t id); -uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data); -uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data); -uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data); -uint8_t dyn_master_write_byte(uint8_t id, uint16_t address, uint8_t data); -uint8_t dyn_master_write_word(uint8_t id, uint16_t address, uint16_t data); -uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data); -uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data); -uint8_t dyn_master_action(void); -uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data); -uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_t *length, TWriteData *data); - -#endif diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h deleted file mode 100644 index 2624cfa550762791faee2eec945068763002ddb4..0000000000000000000000000000000000000000 --- a/communications/include/serial_console.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef _SERIAL_CONSOLE_H -#define _SERIAL_CONSOLE_H - -#include <avr/io.h> -#include <avr/interrupt.h> - -void serial_console_init(uint32_t baudrate); - -#endif diff --git a/communications/src/dynamixel.c b/communications/src/dynamixel.c deleted file mode 100644 index cdff1dd42c5842829bfd1b1528bd6a85d620fcc9..0000000000000000000000000000000000000000 --- a/communications/src/dynamixel.c +++ /dev/null @@ -1,241 +0,0 @@ -#include "dynamixel.h" - -// provate functions -void dyn_set_checksum(uint8_t *packet) -{ - uint8_t checksum=0x00; - uint8_t i; - - checksum+=packet[DYN_ID_OFF]; - checksum+=packet[DYN_LENGTH_OFF]; - checksum+=packet[DYN_INST_OFF]; - for(i=0;i<packet[DYN_LENGTH_OFF]-2;i++) - checksum+=packet[DYN_DATA_OFF+i]; - packet[DYN_DATA_OFF+i]=~checksum; -} - -uint8_t dyn_check_checksum(uint8_t *packet) -{ - uint8_t checksum=0x00; - uint8_t i; - - checksum+=packet[DYN_ID_OFF]; - checksum+=packet[DYN_LENGTH_OFF]; - checksum+=packet[DYN_INST_OFF]; - for(i=0;i<packet[DYN_LENGTH_OFF]-2;i++) - checksum+=packet[DYN_DATA_OFF + i]; - checksum+=packet[DYN_DATA_OFF+i]; - - return checksum; -} - -// public functions -void dyn_copy_packet(uint8_t *source, uint8_t *destination) -{ - uint8_t i,length; - - length=source[DYN_LENGTH_OFF]+4; - for(i=0;i<length;i++) - destination[i]=source[i]; -} - -/* ping instruction*/ -void dyn_init_ping_packet(uint8_t *packet,uint8_t id) -{ - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x02; - packet[DYN_INST_OFF]=DYN_PING; - dyn_set_checksum(packet); -} - -/* read instruction */ -void dyn_init_read_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length) -{ - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x04; - packet[DYN_INST_OFF]=DYN_READ; - packet[DYN_DATA_OFF]=address; - packet[DYN_DATA_OFF+1]=length; - dyn_set_checksum(packet); -} - -/* write instruction */ -void dyn_init_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data) -{ - uint8_t i; - - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x03+length; - packet[DYN_INST_OFF]=DYN_WRITE; - packet[DYN_DATA_OFF]=address; - for(i=0;i<length;i++) - packet[DYN_DATA_OFF+i+1]=data[i]; - dyn_set_checksum(packet); -} - -uint8_t dyn_get_write_data(uint8_t *packet,uint8_t *data) -{ - uint8_t i; - - for(i=0;i<packet[DYN_LENGTH_OFF]-3;i++) - data[i]=packet[DYN_DATA_OFF+i+1]; - - return packet[DYN_LENGTH_OFF]-3; -} - -/* registered write instruction */ -void dyn_init_reg_write_packet(uint8_t *packet,uint8_t id,uint8_t address,uint8_t length,uint8_t *data) -{ - uint8_t i; - - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x03+length; - packet[DYN_INST_OFF]=DYN_REG_WRITE; - packet[DYN_DATA_OFF]=address; - for(i=0;i<length;i++) - packet[DYN_DATA_OFF+i+1]=data[i]; - dyn_set_checksum(packet); -} - -uint8_t dyn_get_reg_write_data(uint8_t *packet,uint8_t *data) -{ - uint8_t i; - - for(i=0;i<packet[DYN_LENGTH_OFF]-3;i++) - data[i]=packet[DYN_DATA_OFF+i+1]; - - return packet[DYN_LENGTH_OFF]-3; -} - -/* action instruction */ -void dyn_init_action_packet(uint8_t *packet) -{ - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=0xFE; - packet[DYN_LENGTH_OFF]=0x02; - packet[DYN_INST_OFF]=DYN_ACTION; - dyn_set_checksum(packet); -} - -/* reset instruction */ -void dyn_init_reset_packet(uint8_t *packet,uint8_t id) -{ - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x02; - packet[DYN_INST_OFF]=DYN_RESET; - dyn_set_checksum(packet); -} - -/* sync write instruction */ -void dyn_init_sync_write_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t address,uint8_t length,TWriteData *data) -{ - uint8_t i,j; - - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=0xFE; - packet[DYN_LENGTH_OFF]=0x04+(length+1)*num_servos; - packet[DYN_INST_OFF]=DYN_SYNC_WRITE; - packet[DYN_DATA_OFF]=address; - packet[DYN_DATA_OFF+1]=length; - for(i=0;i<num_servos;i++) - { - packet[DYN_DATA_OFF+2+(length+1)*i]=servo_ids[i]; - for(j=0;j<length;j++) - packet[DYN_DATA_OFF+2+(length+1)*i+j+1]=data[i].data_addr[j]; - } - dyn_set_checksum(packet); -} - -uint8_t dyn_sync_write_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length,uint8_t *data) -{ - uint8_t num_servos,i,j; - - num_servos=(packet[DYN_LENGTH_OFF]-0x04)/(packet[DYN_DATA_OFF+1]+1); - for(i=0;i<num_servos;i++) - if(packet[DYN_DATA_OFF+2+(packet[DYN_DATA_OFF+1]+1)*i]==id) - { - (*address)=packet[DYN_DATA_OFF]; - (*length)=packet[DYN_DATA_OFF+1]; - for(j=0;j<packet[DYN_DATA_OFF+1];j++) - data[j]=packet[DYN_DATA_OFF+2+(packet[DYN_DATA_OFF+1]+1)*i+j+1]; - return 0x01; - } - return 0x00; -} - -/* bulk read instruction */ -void dyn_init_bulk_read_packet(uint8_t *packet,uint8_t num_servos,uint8_t *servo_ids,uint8_t *address,uint8_t *length) -{ - uint8_t i; - - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=0xFE; - packet[DYN_LENGTH_OFF]=0x03; - packet[DYN_INST_OFF]=DYN_BULK_READ; - packet[DYN_DATA_OFF]=0x00; - for(i=0;i<num_servos;i++) - { - packet[DYN_DATA_OFF+1+i*3]=length[i]; - packet[DYN_DATA_OFF+1+i*3+1]=servo_ids[i]; - packet[DYN_DATA_OFF+1+i*3+2]=address[i]; - packet[DYN_LENGTH_OFF]+=0x03; - } - dyn_set_checksum(packet); -} - -uint8_t dyn_bulk_read_id_present(uint8_t *packet,uint8_t id,uint8_t *address,uint8_t *length) -{ - uint8_t num_servos,i,prev_id=0x00; - - num_servos=(packet[DYN_LENGTH_OFF]-0x03)/3; - for(i=0;i<num_servos;i++) - { - if(packet[DYN_DATA_OFF+1+i*3+1]==id) - { - (*address)=packet[DYN_DATA_OFF+1+i*3+2]; - (*length)=packet[DYN_DATA_OFF+1+i*3]; - return prev_id; - } - else - prev_id=packet[DYN_DATA_OFF+1+i*3+1]; - } - return 0xFF; -} - -void dyn_init_status_packet(uint8_t *packet,uint8_t id,TDynError error,uint8_t length,uint8_t *data) -{ - uint8_t i; - - packet[DYN_HEADER_OFF]=0xFF; - packet[DYN_HEADER_OFF+1]=0xFF; - packet[DYN_ID_OFF]=id; - packet[DYN_LENGTH_OFF]=0x02+length; - packet[DYN_ERROR_OFF]=error; - for(i=0;i<length;i++) - packet[DYN_DATA_OFF+i]=data[i]; - dyn_set_checksum(packet); -} - -/* read instruction status packet */ -uint8_t dyn_get_read_status_data(uint8_t *packet,uint8_t *data) -{ - uint8_t i; - - for(i=0;i<packet[DYN_LENGTH_OFF]-0x02;i++) - data[i]=packet[DYN_DATA_OFF+i]; - - return packet[DYN_LENGTH_OFF]-0x02; -} diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c deleted file mode 100644 index c1b445ac1b8fc594fa0d31aba5c2a31e909c09a3..0000000000000000000000000000000000000000 --- a/communications/src/dynamixel_master.c +++ /dev/null @@ -1,524 +0,0 @@ -#include "comm_cfg.h" -#include "dynamixel_master.h" - -/* private variables */ -uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN]; -uint8_t dyn_master_sent_bytes; -volatile uint8_t dyn_master_sent_done; -uint8_t dyn_master_rx_buffer[DYN_MASTER_MAX_RX_BUFFER_LEN]; -uint8_t dyn_master_received_bytes; -volatile uint8_t dyn_master_packet_ready; -return_level_t dyn_master_return_level; -uint8_t dyn_master_rx_no_answer; -uint8_t dyn_master_rx_num_packets; -uint32_t dyn_master_baudrate; -/* private timeout variables */ -uint16_t dyn_master_rx_timeout_us; - -/* private timeout functions */ -void dyn_master_start_timeout(void) -{ - uint32_t count; - - count=(((dyn_master_rx_timeout_us*(F_CPU/1000000))/1024)); - - TCNT0=255-count; - /* enable timer */ - TIFR0|=(1<<TOV0);// clear interrutp flag - TCCR0B=(1<<CS02)|(1<<CS00);// set prescaler to 1024 -} - -void dyn_master_cancel_timeout(void) -{ - /* disable timer */ - TCCR0B=0x00; - TIFR0|=(1<<TOV0);// clear interrutp flag - TCNT0=0x00; -} - -uint8_t dyn_master_timeout(void) -{ - if(TIFR0&(1<<TOV0)) - return 0x01; - else - return 0x00; -} - -/* private functions */ -void dyn_master_set_tx_mode(void) -{ - PORTE&=~0x08; - PORTE|=0x04; -} - -void dyn_master_set_rx_mode(void) -{ - PORTE&=~0x04; - PORTE|=0x08; -} - -/* interrupt handlers */ -ISR(USART0_TX_vect) -{ - if(dyn_master_sent_bytes==dyn_master_tx_buffer[3]+4) - { - dyn_master_set_rx_mode(); - dyn_master_sent_done=1; - } - else - { - UDR0=dyn_master_tx_buffer[dyn_master_sent_bytes]; - dyn_master_sent_bytes++; - } -} - -ISR(USART0_RX_vect) -{ - static uint8_t length; - - cli();// disable any other interrupt - dyn_master_start_timeout();// each time a byte is received, restart the timeout - dyn_master_rx_buffer[dyn_master_received_bytes]=UDR0; - switch(dyn_master_received_bytes) - { - case 0: if(dyn_master_rx_buffer[dyn_master_received_bytes]==0xFF) - dyn_master_received_bytes++; - break; - case 1: if(dyn_master_rx_buffer[dyn_master_received_bytes]==0xFF) - dyn_master_received_bytes++; - else - dyn_master_received_bytes--; - break; - case 2: dyn_master_received_bytes++; - break; - case 3: length=dyn_master_rx_buffer[dyn_master_received_bytes]+3; - dyn_master_received_bytes++; - break; - default: if(dyn_master_received_bytes==length) - { - dyn_master_received_bytes=0; - dyn_master_packet_ready=1; - dyn_master_cancel_timeout();// cancel timeout when reception is complete - } - else - dyn_master_received_bytes++; - break; - } - sei();// enable all interrutps -} - -uint8_t dyn_master_wait_transmission(void) -{ - while(dyn_master_sent_done==0); - - return DYN_SUCCESS; -} - -uint8_t dyn_master_send(void) -{ - uint8_t error; - - // wait until any previous transmission ends - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - return error; - // set the transfer - dyn_master_sent_bytes=1; - UDR0=dyn_master_tx_buffer[0]; - - return DYN_SUCCESS; -} - -uint8_t dyn_master_wait_reception(void) -{ - // set timeout - dyn_master_start_timeout(); - // wait for the status packet - if(dyn_master_rx_no_answer) - { - dyn_master_rx_no_answer=0x00; - return DYN_SUCCESS; - } - else - { - while(dyn_master_packet_ready==0x00) - { - if(dyn_master_timeout()) - { - dyn_master_cancel_timeout(); - return DYN_TIMEOUT; - } - } - dyn_master_rx_num_packets--; - if(dyn_master_rx_num_packets==0x00) - dyn_master_cancel_timeout(); - dyn_master_packet_ready=0x00; - // check the input packet checksum - if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF) - return dyn_get_status_error(dyn_master_rx_buffer); - else - return DYN_CHECKSUM_ERROR; - } -} - -/* public functions */ -void dyn_master_init(void) -{ - /* initialize internal variables */ - dyn_master_sent_done=0x01; - dyn_master_packet_ready=0x00; - dyn_master_sent_bytes=0x00; - dyn_master_received_bytes=0x00; - dyn_master_rx_no_answer=0x00; - dyn_master_rx_num_packets=0x00; - dyn_master_return_level=return_all; - - /* configure the IO ports */ - DDRE|=0x0E; - DDRE&=0xFE; - - DDRC=0x7F; - PORTC=0x7F; - - /* initialize the serial port */ - // set UART register A - //Bit 7: USART Receive Complete - //Bit 6: USART Transmit Complete - //Bit 5: USART Data Resigter Empty - //Bit 4: Frame Error - //Bit 3: Data OverRun - //Bit 2: Parity Error - //Bit 1: Double The USART Transmission Speed - //Bit 0: Multi-Processor Communication Mode - UCSR0A = 0b01000010; - - // set UART register B - // bit7: enable rx interrupt - // bit6: enable tx interrupt - // bit4: enable rx - // bit3: enable tx - // bit2: set sendding size(0 = 8bit) - UCSR0B = 0b11011000; - - // set UART register C - // bit6: communication mode (1 = synchronize, 0 = asynchronize) - // bit5,bit4: parity bit(00 = no parity) - // bit3: stop bit(0 = stop bit 1, 1 = stop bit 2) - // bit2,bit1: data size(11 = 8bit) - UCSR0C = 0b00000110; - dyn_master_set_baudrate((uint32_t)DYN_MASTER_DEFAULT_BAUDRATE); - - /* initialize the timeout timer */ - dyn_master_rx_timeout_us=DYN_MASTER_DEFAULT_TIMEOUT_US; - TCCR0A=0x00;// normal mode, output disabled on both channels - TCCR0B=0x00;// normal mode, prescaler 0 (timer disabled) - TIMSK0=0x00;// all interrupts disabled - TCNT0=0x00; - - dyn_master_set_rx_mode(); -} - -void dyn_master_set_rx_timeout(uint16_t timeout_us) -{ - if(timeout_us>16000) - dyn_master_rx_timeout_us=16000; - else - dyn_master_rx_timeout_us=timeout_us; -} - -void dyn_master_set_return_level(return_level_t level) -{ - dyn_master_return_level=level; -} - -return_level_t dyn_master_get_return_level(void) -{ - return dyn_master_return_level; -} - -void dyn_master_set_baudrate(uint32_t baudrate) -{ - uint16_t divisor; - - divisor = (uint16_t)((uint32_t)2000000 / baudrate) - 1; - UBRR0H = (uint8_t)((divisor & 0xFF00) >> 8); - UBRR0L = (uint8_t)(divisor & 0x00FF); -} - -uint32_t dyn_master_get_baudrate(void) -{ - return dyn_master_baudrate; -} - -void dyn_master_scan(uint8_t *num,uint8_t *ids) -{ - uint8_t i; - - *num=0; - for(i=0;i<254;i++) - { - if(dyn_master_ping(i)==DYN_SUCCESS)// the device exists - { - ids[*num]=i; - (*num)++; - } - } -} - -uint8_t dyn_master_ping(uint8_t id) -{ - uint8_t error; - - // generate the ping packet for the desired device - dyn_init_ping_packet(dyn_master_tx_buffer,id); - dyn_master_rx_num_packets=0x01; - dyn_master_rx_no_answer=0x00; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - error=dyn_master_wait_reception(); - - return error; -} - -uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data) -{ - return dyn_master_read_table(id,address,1,data); -} - -uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data) -{ - uint8_t error; - uint8_t data_int[2]; - - // generate the ping packet for the desired device - error=dyn_master_read_table(id,address,2,data_int); - (*data)=data_int[0]+data_int[1]*256; - - return error; -} - -uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data) -{ - uint8_t error; - - // generate the read packet for the desired device - dyn_init_read_packet(dyn_master_tx_buffer,id,address,length); - dyn_master_rx_num_packets=0x01; - if(dyn_master_return_level==no_return || id==DYN_BROADCAST_ID) - dyn_master_rx_no_answer=0x01; - else - dyn_master_rx_no_answer=0x00; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(dyn_master_return_level!=no_return && id!=DYN_BROADCAST_ID) - { - if((error=dyn_master_wait_reception())==DYN_SUCCESS) - { - if(dyn_get_read_status_data(dyn_master_rx_buffer,data)!=length)// not enough data - error=DYN_INST_ERROR; - } - } - - return error; -} - -uint8_t dyn_master_write_byte(uint8_t id, uint16_t address, uint8_t data) -{ - return dyn_master_write_table(id,address,1,&data); -} - -uint8_t dyn_master_write_word(uint8_t id, uint16_t address, uint16_t data) -{ - uint8_t data_int[2]; - - data_int[0]=data%256; - data_int[1]=data/256; - return dyn_master_write_table(id,address,2,data_int); -} - -uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - uint8_t error; - - // generate the write packet for the desired device - dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data); - dyn_master_rx_num_packets=0x01; - if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID) - dyn_master_rx_no_answer=0x00; - else - dyn_master_rx_no_answer=0x01; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID) - error=dyn_master_wait_reception(); - - return error; -} - -uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - uint8_t error; - - // generate the registered write packet for the desired device - dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data); - dyn_master_rx_num_packets=0x01; - if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID) - dyn_master_rx_no_answer=0x00; - else - dyn_master_rx_no_answer=0x01; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(dyn_master_return_level==return_all && id!=DYN_BROADCAST_ID) - error=dyn_master_wait_reception(); - - return error; -} - -uint8_t dyn_master_action(void) -{ - uint8_t error; - - // generate the action packet for the desired device - dyn_init_action_packet(dyn_master_tx_buffer); - dyn_master_rx_num_packets=0x01; - dyn_master_rx_no_answer=0x01; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - - return error; -} - -uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data) -{ - uint8_t error; - - // generate the write packet for the desired device - dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data); - dyn_master_rx_num_packets=0x01; - dyn_master_rx_no_answer=0x01; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - - return error; -} - -uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_t *length, TWriteData *data) -{ - uint8_t error,i,ver1_address[255],ver1_length[255]; - - // generate the read packet for the desired device - for(i=0;i<num;i++) - { - ver1_address[i]=address[i]; - ver1_length[i]=length[i]; - } - dyn_init_bulk_read_packet(dyn_master_tx_buffer,num,ids,ver1_address,ver1_length); - dyn_master_rx_num_packets=num; - if(dyn_master_return_level==no_return) - dyn_master_rx_no_answer=0x01; - else - dyn_master_rx_no_answer=0x00; - // enable transmission - dyn_master_set_tx_mode(); - // send the data - if((error=dyn_master_send())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission())!=DYN_SUCCESS) - { - dyn_master_set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(dyn_master_return_level!=no_return) - { - for(i=0;i<num;i++) - { - if((error=dyn_master_wait_reception())==DYN_SUCCESS) - { - if(dyn_get_read_status_data(dyn_master_rx_buffer,data[i].data_addr)!=length[i])// not enough data - error=DYN_INST_ERROR; - } - } - } - - return error; -} diff --git a/communications/src/examples/Makefile b/communications/src/examples/Makefile deleted file mode 100644 index fd3b35d327a4feadee4160ce1945fbf9222837d0..0000000000000000000000000000000000000000 --- a/communications/src/examples/Makefile +++ /dev/null @@ -1,40 +0,0 @@ -PROJECT=comm_ex -######################################################## -# afegir tots els fitxers que s'han de compilar aquà -######################################################## -SOURCES=main.c - -OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -INCLUDE_DIR=../../include/ -COMM_DIR=../../lib/ -CC=avr-gcc -OBJCOPY=avr-objcopy -MMCU=atmega2561 - -LIBS=$(COMM_DIR)libcomm.a - -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) -I$(INCLUDE_DIR) -o $@ $< - -download: $(MAIN_OUT_HEX) - avrdude -c avr109 -P /dev/ttyUSB0 -b 19200 -p m128 -U flash:w:$(PROJECT).hex - avrdude -c avr109 -P /dev/ttyUSB0 -b 19200 -p m128 -U eeprom:w:$(PROJECT)_eeprom.hex - -clean: - -rm $(PROJECT).* - -rm $(OBJS) diff --git a/communications/src/examples/main.c b/communications/src/examples/main.c deleted file mode 100755 index 6188a35168e74d4c7ab8c2dc24e99b65db3eeac3..0000000000000000000000000000000000000000 --- a/communications/src/examples/main.c +++ /dev/null @@ -1,31 +0,0 @@ -#include <avr/io.h> -#include <util/delay.h> -#include <avr/interrupt.h> -#include "dynamixel_master.h" -#include "serial_console.h" -#include <stdio.h> - -int16_t main(void) -{ - unsigned char num; - unsigned char ids[32]; - - dyn_master_init(); - serial_console_init(57600); - sei(); - - while (1) - { - dyn_master_scan(&num,ids); - if(num==1) - { - printf("Found one device\n"); - PORTC=0x3F; - } - else - { - printf("No device found\n"); - PORTC=0x7F; - } - } -} diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c deleted file mode 100644 index 7842eb7d91bf8866bd014625a7a30f234339bbfd..0000000000000000000000000000000000000000 --- a/communications/src/serial_console.c +++ /dev/null @@ -1,99 +0,0 @@ -#include "comm_cfg.h" -#include "serial_console.h" -#include <stdio.h> - -/* private variables */ -volatile uint8_t serial_console_buffer[SERIAL_CONSOLE_MAX_BUFFER_LEN]; -volatile uint8_t serial_console_read_ptr; -volatile uint8_t serial_console_write_ptr; -volatile uint8_t serial_console_num_data; -static FILE *device; - -/* interrupt handlers */ -SIGNAL(USART1_RX_vect) -{ - if(serial_console_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN) - { - serial_console_buffer[serial_console_write_ptr]=UDR1; - serial_console_write_ptr=(serial_console_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN; - serial_console_num_data++; - } -} - -/* private functions */ -void serial_console_set_baudrate(uint32_t baudrate) -{ - uint16_t divisor; - - divisor = (uint16_t)((uint32_t)2000000 / baudrate) - 1; - UBRR1H = (uint8_t)((divisor & 0xFF00) >> 8); - UBRR1L = (uint8_t)(divisor & 0x00FF); -} - -int serial_console_putchar(char c,FILE *dev) -{ - if( c == '\n' ) - { - while((UCSR1A&(1<<UDRE1))==0x00); - UDR1 = '\r'; - } - - while((UCSR1A&(1<<UDRE1))==0x00); - UDR1 = c; - - return 0; -} - -int serial_console_getchar(FILE *dev) -{ - int8_t rx; - - while(serial_console_num_data==0); - rx=serial_console_buffer[serial_console_read_ptr]; - serial_console_read_ptr=(serial_console_read_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN; - serial_console_num_data--; - if(rx=='\r') - rx='\n'; - - return rx; -} - -/* pubic functions */ -void serial_console_init(uint32_t baudrate) -{ - // set UART register A - //Bit 7: USART Receive Complete - //Bit 6: USART Transmit Complete - //Bit 5: USART Data Resigter Empty - //Bit 4: Frame Error - //Bit 3: Data OverRun - //Bit 2: Parity Error - //Bit 1: Double The USART Transmission Speed - //Bit 0: Multi-Processor Communication Mode - UCSR1A = 0b01000010; - - // set UART register B - // bit7: enable rx interrupt - // bit6: enable tx interrupt - // bit4: enable rx - // bit3: enable tx - // bit2: set sendding size(0 = 8bit) - UCSR1B = 0b10011000; - - // set UART register C - // bit6: communication mode (1 = synchronize, 0 = asynchronize) - // bit5,bit4: parity bit(00 = no parity) - // bit3: stop bit(0 = stop bit 1, 1 = stop bit 2) - // bit2,bit1: data size(11 = 8bit) - UCSR1C = 0b00000110; - - // initialize - UDR1 = 0xFF; - serial_console_read_ptr=0; - serial_console_write_ptr=0; - serial_console_num_data=0; - - // set baudrate - serial_console_set_baudrate(baudrate); - device=fdevopen(serial_console_putchar,serial_console_getchar); -} diff --git a/controllers/Makefile b/controllers/Makefile index 1c0c8bca77ce8044316e1d248b82b8849706f9f4..04d436420db8020725cf3adb28accb98ea038f9a 100755 --- a/controllers/Makefile +++ b/controllers/Makefile @@ -1,30 +1,53 @@ #AVR-GCC Makefile +ROBOTIS_C_DIR ?= + +ifdef ROBOTIS_C_DIR + ROBOTIS_C_DIR_FOUND=true +else + ROBOTIS_C_DIR_FOUND=false +endif + PROJECT=libcontrollers -SOURCES=src/cm510.c src/gpio.c src/adc.c src/buzzer.c src/user_time.c +SOURCES=src/cm510/cm510.c OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -BIN_DIR=./build/ -LIB_DIR=./lib/ -DEV_DIR=../dyn_devices/ -COMM_DIR=../communications/ -MAN_DIR=../motion/ -CONT_DIR=./ +SDIR=./src/ +IDIR=./include/ +BDIR=./build/ +LDIR=./lib/ CC=avr-gcc -OBJCOPY=avr-ar +OBJCOPY=avr-ar rsc MMCU=atmega2561 -INC_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(DEV_DIR)include -I$(MAN_DIR)include - -LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a $(MAN_DIR)lib/libmotion_manager.a +ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/ +ROBOTIS_LIBRARY_DIR=$(ROBOTIS_C_DIR)/lib/ CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes -ARFLAGS=rsc - -.PHONY: all show_banner clean - -all: show_banner communications dyn_devices motion_manager $(PROJECT).a +.PHONY: all check_robotis_c show_banner clean + +all: check_robotis_c show_banner $(PROJECT).a + +check_robotis_c: + @if ! $(ROBOTIS_C_DIR_FOUND); then \ + echo " IMPORTANT !!! "; \ + echo " ROBOTIS_C_DIR variable is not set! The software won't compile"; \ + echo " unless you specify the path to Robotis embedded C software."; \ + echo ""; \ + echo " You should:"; \ + echo " 1. Edit the makefile, at the begging set up ROBOTIS_C_DIR"; \ + echo " 2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \ + echo ""; \ + echo " Beware of:"; \ + echo " 1. Do not use '(' in the directory name (as it comes from robotis) "; \ + echo " 2. Absolute path is preferred to avoid problems with submake calls"; \ + exit 2; \ + fi + + @if [ ! -d $(ROBOTIS_C_DIR) ]; then \ + echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it."; \ + exit 2; \ + fi show_banner: @echo "------------------------------------------------------"; @@ -39,33 +62,11 @@ show_banner: @echo "------------------------------------------------------"; $(PROJECT).a: ${OBJS} - $(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS) + $(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS) %.o: %.c - $(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $< - -communications: - $(MAKE) -C ../communications - -dyn_devices: - $(MAKE) -C ../dyn_devices - -motion_manager: - $(MAKE) -C ../motion - -examples: - $(MAKE) -C src/examples - $(MAKE) -C ../communications examples - $(MAKE) -C ../dyn_devices examples - $(MAKE) -C ../motion examples - -download: - $(MAKE) -C src/examples download + $(CC) -c $(CFLAGS) -I$(IDIR) -o $@ $< clean: - rm -f ${LIB_DIR}$(PROJECT).a + rm -f ${LDIR}$(PROJECT).a rm -f $(OBJS) - $(MAKE) -C src/examples clean - $(MAKE) -C ../communications clean - $(MAKE) -C ../dyn_devices clean - $(MAKE) -C ../motion clean diff --git a/controllers/include/adc.h b/controllers/include/adc.h deleted file mode 100644 index a5b45094cf4da6a4d3b5f3280aeef76c3c175a63..0000000000000000000000000000000000000000 --- a/controllers/include/adc.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef _ADC_H -#define _ADC_H - -#include <avr/io.h> -#include <avr/interrupt.h> - -// ADC channel identifiers -typedef enum {ADC_VCC=0,ADC_PORT_1=1,ADC_PORT_2=2,ADC_PORT_3=3,ADC_PORT_4=4,ADC_PORT_5=5,ADC_PORT_6=6} adc_t; -#define NUM_ADC 7 - -/** - * \brief Function to initialize the ADC module - * - * This function initializes the ADC module of the CM510 controller. This includes - * configuring the portA as output to sequentially enable each of the ADC channels - * before starting the conversion, configuring the operation of the ADC module of - * the micro and start the first conversion. - * - * Since the power supply to the external device is disabled by default, and only - * activated when the conversion is going to start, it is necessary to wait for a while - * before actually starting the conversion to enssure the correct operation of the - * device (12 us in this case). - * - * A part from the ADC module itself, this module also uses most of the PORT A of the - * microcontroller (the 6 most significant bits). - * - * \return this function always returns 0 - */ -void init_adc(void); -/** - * \brief Function to get the last conversion - * - * This functions returns the digital codification in raw binary of the analog - * voltage present at the given channel. In Asynchronous mode, this function - * returns immediatelly with the last caonverted value of the desired channel, - * which can be up to 0.5 ms old in the worst case. - * - * In synchronous mode, the conversion is started when this function is called, - * and it does not return until the conversion id done, which takes around - * 125 us to complete. - * - * \param channel_id identifier of the ADC channel to be converted. It must be - * one of the macros defined above. - * - * \return 0 if the functions is successfull and 0xFF in case of any error. - */ -uint16_t get_adc_channel(adc_t channel_id); -uint16_t get_adc_avg_channel(adc_t channel_id); - -#endif diff --git a/controllers/include/buzzer.h b/controllers/include/buzzer.h deleted file mode 100644 index 93c96b8ac81afee2d095a0ca58ff3727bcb06c92..0000000000000000000000000000000000000000 --- a/controllers/include/buzzer.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef _BUZZER_H -#define _BUZZER_H - -#include <avr/io.h> -#include <avr/interrupt.h> - -// 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 - -/** - * \brief Function to initialize the buzzed module - * - * This function initializes the buzzer module of the CM510 controller. The buzzer - * uses a single GPIO pin (bit 5 of port B) which is a PWM output and can be used - * to generate different frequencies. The PWM module uses the timer 1. - * - * This function also initializes the table with the correspondence between the - * notes and the actual value that has to be loaded to the PWM module. - * - * \return this function always returns 0 - */ -void init_buzzer(void); -/** - * \brief Function to start playing a note with the buzzer - * - * This function is used to start playing the given note with the buzzer. The - * note is identified with the macros defined above. Once the buzzer module - * is started, it will continue playing the same note until the buzzer_stop() - * or the buzzer_chnage_note() functions are called. - * - * \param note identifier of the note to be played. - * - * \return 0 if the function completes successfully and 0xFF otherwise. - */ -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 - * - * This function changes the note that is being played if the buzzer is - * started. Otherwise it has no effect. The note is identified with one of - * the macors defined above. - * - * \param note identifier of the note to be played. - * - * \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 - * - * This functions stops playing any note that is being played immediatelly. - * - * \return this function always returns 0 - */ -void buzzer_stop(void); - -uint8_t buzzer_is_playing(void); -uint8_t buzzer_is_playing_alarm(void); - -#endif diff --git a/controllers/include/cm510.h b/controllers/include/cm510.h index 8fee1b362e0ce6b1a6c33b0dde5d4a02b5ecb1dc..13c441dc9171989cfb1c88e4d08f1c95b535da36 100755 --- a/controllers/include/cm510.h +++ b/controllers/include/cm510.h @@ -18,17 +18,6 @@ #ifndef _CM510_H #define _CM510_H -#include "cont_cfg.h" -#include "motion_cfg.h" -#include "comm_cfg.h" -#include "gpio.h" -#include "adc.h" -#include "buzzer.h" -#include "motion_manager.h" -#include "action.h" -#include "user_time.h" -#include "serial_console.h" - // general interface /** * \brief Function to initialize all the cm510 modules @@ -47,8 +36,353 @@ * * After all the initializations are done, the global interrupts are enabled. * + * \param adc_mode identifier of the desired operation mode of the analog to + * digital converter. The possible values are SYNC and ASYNC. + * * \return 0 if all the initializations complete successfully and 0xFF otherwise. */ -void init_cm510(void); +unsigned char init_cm510(unsigned char adc_mode); + +// leds interface +#define LED_BAT 0x01 +#define LED_TxD 0x02 +#define LED_RxD 0x04 +#define LED_AUX 0x08 +#define LED_MANAGE 0x10 +#define LED_PROGRAM 0x20 +#define LED_PLAY 0x40 +/** + * \brief Function to initialize the LED's module + * + * This function initializes the GPIO pins associated to the LED's as outputs. + * The LED's are connected to the 7 lower bits of port C. + * + * \return this function always returns 0 + */ +unsigned char init_leds(void); +/** + * \brief Function to turn on a given LED + * + * This function turns on a given LED, what ever its current state. The LED + * is identified by one of the macros defined above. + * + * \param led_id identifier of the LED to turn on + * + * \return 0 if the function is successfull, 0xFF otherwise + */ +unsigned char turn_led_on(unsigned char led_id); +/** + * \brief Function to turn off a given LED + * + * This function turns off a given LED, what ever its current state. The LED + * is identified by one of the macros defined above. + * + * \param led_id identifier of the LED to turn off + * + * \return 0 if the function is successfull, 0xFF otherwise + */ +unsigned char turn_led_off(unsigned char led_id); +/** + * \brief Function to toggle the current state of a LED + * + * This function changes the current state of a given LED. If it is on, it is turned + * off, and viceversa. The LED is identified by one of the macros defined above. + * + * \param led_id identifier of the LED to toggle + * + * \return 0 if the function is successfull, 0xFF otherwise + */ +unsigned char toggle_led(unsigned char led_id); +/** + * \brief Function to check wether the LED is on or not + * + * This function checks if a given LED is turned on or off. The LED is identified + * by one of the macros defined above. + * + * \param led_id identifier of the LED to check + * + * \return 0 if the LED is turned off, 1 if the led is turned on and 0xFF in + * case of any error. + */ +unsigned char is_led_on(unsigned char led_id); +// buttons interface +#define BTN_UP 0x10 +#define BTN_DOWN 0x20 +#define BTN_LEFT 0x40 +#define BTN_RIGHT 0x80 +#define BTN_START 0x01 +#define NUM_BUTTONS 5 +/** + * \brief Function to initialize the buttons module + * + * This function initializes the GPIO pins associated to the buttons as inputs. + * The buttons are connected to the four most significant bits of port E and the + * least significant bit of port D. + * + * This function also sets to NULL the callback functions for all the buttons and + * initializes the current state of the buttons. + * + * \return this function always returns 0 + */ +unsigned char init_buttons(void); +/** + * \brief Function to check whether a button is pressed or not + * + * This functions checks if a given value is pressed or not. The desired button + * is identified with one of the macros defined above. + * + * \param button_id identifier of the button to assign a callback. This must be any of + * the macros defined above. + * + * \return 0 if the button is not pressed, 1 if it is pressed and 0xFF in + * case of any error. + */ +unsigned char is_button_pressed(unsigned char button_id); +/** + * \brief Function to assign a callback function to a button + * + * This function assigns a callback function to a button that will be called whenever + * the button is pressed (falling edge). The prototype of the callback function is as + * follows: + * + * void callback_function(void *param) + * + * The callback function accepts a generic pointer that can be used to pass the necessary + * information to the callback function, if necessary. + * + * \param button_id identifier of the button to assign a callback. This must be any of + * the macros defined above. + * + * \param function pointer to the callback function to be assigned to the button. + * + * \param param a pointer to any data needed inside the callback function to perform its + * function. + * + * \return 0 if the function completes successfully, 0xFF otherwise. + */ +unsigned char button_assign_callback(unsigned char button_id, void (*function)(void *param),void *param); +// ADC interface +// ADC channel identifiers +#define ADC_PORT_1 1 +#define ADC_PORT_2 2 +#define ADC_PORT_3 3 +#define ADC_PORT_4 4 +#define ADC_PORT_5 5 +#define ADC_PORT_6 6 +#define NUM_ADC 6 +// ADC operation modes +#define SYNC 0 +#define ASYNC 1 +/** + * \brief Function to initialize the ADC module + * + * This function initializes the ADC module of the CM510 controller. This includes + * configuring the portA as output to sequentially enable each of the ADC channels + * before starting the conversion, configuring the operation of the ADC module of + * the micro and start the first conversion. + * + * The ADC module can work in two different mode: + * + * * Synchronous mode: in this case the module is idle until the get_adc_channel() + * function is called. At that time, the desired channel is + * enabled and the conversion started. The function block until + * the conversion is done. + * + * * Asynchronous mode: in this case the module is always sequentially converting each + * of the channels and saving the converted values in internal + * memory. When the get_adc_channel_function() is called, it + * returns immediatelly with the last converted value of the + * desired channel. In the worst case, the sample will be 0.6 ms + * old. + * + * Since the power supply to the external device is disabled by default, and only + * activated when the conversion is going to start, it is necessary to wait for a while + * before actually starting the conversion to enssure the correct operation of the + * device (12 us in this case). + * + * A part from the ADC module itself, this module also uses most of the PORT A of the + * microcontroller (the 6 most significant bits). + * + * \param mode the desired operation mode of the ADC module. This value can be either + * SYNC or ASYNC. + * + * \return this function always returns 0 + */ +unsigned char init_adc(unsigned char mode); +/** + * \brief Function to get the last conversion + * + * This functions returns the digital codification in raw binary of the analog + * voltage present at the given channel. In Asynchronous mode, this function + * returns immediatelly with the last caonverted value of the desired channel, + * which can be up to 0.5 ms old in the worst case. + * + * In synchronous mode, the conversion is started when this function is called, + * and it does not return until the conversion id done, which takes around + * 125 us to complete. + * + * \param channel_id identifier of the ADC channel to be converted. It must be + * one of the macros defined above. + * + * \return 0 if the functions is successfull and 0xFF in case of any error. + */ +unsigned short int get_adc_channel(unsigned char channel_id); +// buzzer interface +#define NOTE_DO 0 +#define NOTE_RE 1 +#define NOTE_MI 2 +#define NOTE_FA 3 +#define NOTE_SOL 4 +#define NOTE_LA 5 +#define NOTE_SI 6 +#define NUM_NOTES 7 +/** + * \brief Function to initialize the buzzed module + * + * This function initializes the buzzer module of the CM510 controller. The buzzer + * uses a single GPIO pin (bit 5 of port B) which is a PWM output and can be used + * to generate different frequencies. The PWM module uses the timer 1. + * + * This function also initializes the table with the correspondence between the + * notes and the actual value that has to be loaded to the PWM module. + * + * \return this function always returns 0 + */ +unsigned char init_buzzer(void); +/** + * \brief Function to start playing a note with the buzzer + * + * This function is used to start playing the given note with the buzzer. The + * note is identified with the macros defined above. Once the buzzer module + * is started, it will continue playing the same note until the buzzer_stop() + * or the buzzer_chnage_note() functions are called. + * + * \param note identifier of the note to be played. + * + * \return 0 if the function completes successfully and 0xFF otherwise. + */ +unsigned char buzzer_start(unsigned char note); +/** + * \brief Function to change the note is being played + * + * This function changes the note that is being played if the buzzer is + * started. Otherwise it has no effect. The note is identified with one of + * the macors defined above. + * + * \param note identifier of the note to be played. + * + * \return 0 if the function completes successfully and 0xFF otherwise. + */ +unsigned char buzzer_change_note(unsigned char note); +/** + * \brief Function to stop the buzzer module + * + * This functions stops playing any note that is being played immediatelly. + * + * \return this function always returns 0 + */ +unsigned char buzzer_stop(void); +// timer interface +/** + * \brief Function to initialize the user timer module + * + * This function initializes the user timer by default, but it does not start it. + * This modules uses the timer 3 of the microcontroller, but it does not use any + * GPIO pin. + * + * This timer can be used to wait for a given ammount of time blocking the processor + * or else to schedule a given task some time in the future. To set the desired + * ammount of time use the timer_set_time(function). Once started with the + * timer_start() function, the timer can be stopped (timer_stop()) or waites until + * it is done (is_timer_done() or timer_wait()). + * + * A callback function can be assigned so that it will be called when the desired + * time has ellapsed. The declaration of this function must be as follows: + * + * void <function name>(void *param) + * + * \return this function always returns 0x00 + */ +unsigned char init_timer(void); +/** + * \brief Function to set the desired period + * + * This function sets the desired period of the timer in miliseconds. At the + * moment only periods inferior to 4 seconds (4000 ms) are supported. The + * lower bound is around 1 msdur to functional issues. This function configures + * the internal timer, but it does not start it. In order to do so, call the + * timer_start() function. + * + * \param time_ms the desired period of time in miliseconds. + * + * \return 0 if the function completes successfully and 0xFF otherwise + */ +unsigned char timer_set_time(unsigned short int time_ms); +/** + * \brief Function to start the timer module + * + * This function actually starts the timer module. After calling this function, + * the desired period of time begins counting. This function has no effect if the + * timer has not been set with the timer_set_time() function. + * + * \return 0 if the function completes successfully and 0xFF otherwise. + */ +unsigned char timer_start(void); +/** + * \brief Function to assign a callback function to the timer + * + * This function assigns a callback function to the timer. The assigned function + * will be called when the desired time is done. If not changed, the same function + * will be called if the timer is started again. + * + * It is not mandatory to assign a callback function to the timer. The declaration + * of this callback function must be as follows: + * + * void <function name>(void *param); + * + * \param function a pointer to the desired callback function. It can be set to 0x0000 + * to remove a previously assigned callback function. + * + * \param param an optional pointer to any data required by the callback function. This + * parameter can be set to 0x0000 if no data is required. + * + * \return 0 if the function completes successfully and 0xFF otherwise + */ +unsigned char timer_set_callback(void (*function)(void *param),void *param); +/** + * \brief Function to stop the current timer + * + * This function stops the timer in case it has been started. Otherwise nothing + * is done. If the timer is stopped before completion, the callback function will + * not be called, and the timer will report is has completed the desired counting. + * + * If the timer is re-started after stopping it, it will count the whole period + * configured by the timer_set_time(function). + * + * \return 0 if the function completes successfully and 0xFF otherwise + */ +unsigned char timer_stop(void); +/** + * \brief Function to check whether the timer is done or not + * + * This function checks if the desired period has ellapsed or not. This function + * only checks the current state of the timer and returns immediatelly. To wait + * for the completion of the timer, use the timer_wait() function. + * + * \return if the timer nas not been started ot is has finished counting, this + * function returns 0x01, if the counter is still counting, it returns + * 0x00, and 0xFF in case of any error. + */ +unsigned char is_timer_done(void); +/** + * \brief Function to wait for the completion of the timer + * + * This function blocks until the desired time has ellapsed. It blocks until the + * timer reaches the desired time. In order to check if the timer has ellapsed or + * not use the is_timer_done() function. + * + * \return 0 if the functions completes successfully and 0xFF otherwise. + */ +unsigned char timer_wait(void); #endif diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h deleted file mode 100644 index e46833f4107bd561f7ab31449eb09f2758ac060b..0000000000000000000000000000000000000000 --- a/controllers/include/cm510_cfg.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _CM510_CFG_H -#define _CM510_CFG_H - -#include "buzzer.h" - -// controller configuration parameters -#define ADC_MAX_NUM_SAMPLES 16 -#define ADC_SAMPLE_PERIOD_MS 10 -#define ADC_VOLTAGE_ALARM_TH 559 -#define ADC_VOLTAGE_ALARM_WINDOW 20 -#define ADC_VOLTAGE_ALARM_NOTE NOTE_LA -#define ADC_VOLTAGE_ALARM_TIME_ON 30 -#define ADC_VOLTAGE_ALARM_TIME_OFF 30 - -// communication configuration parameters -#define DYN_MASTER_MAX_TX_BUFFER_LEN 128 -#define DYN_MASTER_MAX_RX_BUFFER_LEN 128 -#define DYN_MASTER_DEFAULT_BAUDRATE 1000000 -#define DYN_MASTER_DEFAULT_TIMEOUT_US 10000 -#define SERIAL_CONSOLE_MAX_BUFFER_LEN 128 - -// motion configuration parameters -#define MANAGER_MAX_NUM_SERVOS 18 -#define MANAGER_MISSING_SERVOS_ALARM_NOTE NOTE_SOL -#define MANAGER_MISSING_SERVOS_ALARM_TIME_ON 10 -#define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF 100 -#define BALANCE_GYRO_CAL_NUM_SAMPLES 10 -#define BALANCE_GYRO_X_CHANNEL 3 -#define BALANCE_GYRO_Y_CHANNEL 4 -#define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE NOTE_SOL -#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON 10 -#define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF 100 -#define BALANCE_MAX_CAL_GYRO_ERROR 20.0 -#define BALANCE_FORWARD_FALL_THD_VALUE (-200) -#define BALANCE_BACKWARD_FALL_THD_VALUE 200 -#define BALANCE_KNEE_GAIN (4.0/54.0) -#define BALANCE_ANKLE_ROLL_GAIN (4.0/18.0) -#define BALANCE_HIP_PITCH_GAIN (4.0/20.0) -#define BALANCE_ANKLE_PITCH_GAIN (4.0/40.0) - -#endif diff --git a/controllers/include/cont_cfg.h b/controllers/include/cont_cfg.h deleted file mode 100644 index 985b7b9a4e2abfc84c834cdcba1f249ac229ad84..0000000000000000000000000000000000000000 --- a/controllers/include/cont_cfg.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef _CONT_CFG_H -#define _CONT_CFG_H - -#include "buzzer.h" - -// number of samples to average -#ifndef ADC_MAX_NUM_SAMPLES - #define ADC_MAX_NUM_SAMPLES 16 -#endif - -#ifndef ADC_SAMPLE_PERIOD_MS - #define ADC_SAMPLE_PERIOD_MS 10 -#endif - -#ifndef ADC_VOLTAGE_ALARM_TH - #define ADC_VOLTAGE_ALARM_TH 559 -#endif - -#ifndef ADC_VOLTAGE_ALARM_WINDOW - #define ADC_VOLTAGE_ALARM_WINDOW 20 -#endif - -#ifndef ADC_VOLTAGE_ALARM_NOTE - #define ADC_VOLTAGE_ALARM_NOTE NOTE_LA -#endif - -#ifndef ADC_VOLTAGE_ALARM_TIME_ON - #define ADC_VOLTAGE_ALARM_TIME_ON 30 -#endif - -#ifndef ADC_VOLTAGE_ALARM_TIME_OFF - #define ADC_VOLTAGE_ALARM_TIME_OFF 30 -#endif - -#endif diff --git a/controllers/include/gpio.h b/controllers/include/gpio.h deleted file mode 100644 index 994899170dd675a33a5552c74a97f4320801ed4e..0000000000000000000000000000000000000000 --- a/controllers/include/gpio.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef _GPIO_H -#define _GPIO_H - -#include <avr/io.h> -#include <avr/interrupt.h> - -// leds interface -typedef enum {LED_BAT=0x01,LED_TxD=0x02,LED_RxD=0x04,LED_AUX=0x08,LED_MANAGE=0x10,LED_PROGRAM=0x20,LED_PLAY=0x40} leds_t; -#define NUM_LEDS 7 - -// buttons interface -typedef enum {BTN_UP=0x10,BTN_DOWN=0x20,BTN_LEFT=0x40,BTN_RIGHT=0x80,BTN_START=0x01} pushbuttons_t; -#define NUM_BUTTONS 5 - -/** - * \brief Function to initialize the LED's module - * - * This function initializes the GPIO pins associated to the LED's as outputs. - * The LED's are connected to the 7 lower bits of port C. - * - * \return this function always returns 0 - */ -void init_leds(void); -/** - * \brief Function to turn on a given LED - * - * This function turns on a given LED, what ever its current state. The LED - * is identified by one of the macros defined above. - * - * \param led_id identifier of the LED to turn on - * - * \return 0 if the function is successfull, 0xFF otherwise - */ -void turn_led_on(leds_t led_id); -/** - * \brief Function to turn off a given LED - * - * This function turns off a given LED, what ever its current state. The LED - * is identified by one of the macros defined above. - * - * \param led_id identifier of the LED to turn off - * - * \return 0 if the function is successfull, 0xFF otherwise - */ -void turn_led_off(leds_t led_id); -/** - * \brief Function to toggle the current state of a LED - * - * This function changes the current state of a given LED. If it is on, it is turned - * off, and viceversa. The LED is identified by one of the macros defined above. - * - * \param led_id identifier of the LED to toggle - * - * \return 0 if the function is successfull, 0xFF otherwise - */ -void toggle_led(leds_t led_id); -/** - * \brief Function to check wether the LED is on or not - * - * This function checks if a given LED is turned on or off. The LED is identified - * by one of the macros defined above. - * - * \param led_id identifier of the LED to check - * - * \return 0 if the LED is turned off, 1 if the led is turned on and 0xFF in - * case of any error. - */ -uint8_t is_led_on(leds_t led_id); - -/** - * \brief Function to initialize the buttons module - * - * This function initializes the GPIO pins associated to the buttons as inputs. - * The buttons are connected to the four most significant bits of port E and the - * least significant bit of port D. - * - * This function also sets to NULL the callback functions for all the buttons and - * initializes the current state of the buttons. - * - * \return this function always returns 0 - */ -void init_buttons(void); -/** - * \brief Function to check whether a button is pressed or not - * - * This functions checks if a given value is pressed or not. The desired button - * is identified with one of the macros defined above. - * - * \param button_id identifier of the button to assign a callback. This must be any of - * the macros defined above. - * - * \return 0 if the button is not pressed, 1 if it is pressed and 0xFF in - * case of any error. - */ -uint8_t is_button_pressed(pushbuttons_t button_id); - -uint8_t is_button_rising_edge(pushbuttons_t button_id); - -uint8_t is_button_falling_edge(pushbuttons_t button_id); - -#endif diff --git a/controllers/include/user_time.h b/controllers/include/user_time.h deleted file mode 100644 index ab080482202830d0982cb083051b1ee26e623483..0000000000000000000000000000000000000000 --- a/controllers/include/user_time.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef _USER_TIME_H -#define _USER_TIME_H - -#include <avr/io.h> -#include <avr/interrupt.h> - -/* public functions */ -void init_user_time(void); -void user_time_set_period(uint16_t period_ms); -void user_time_set_one_time(uint16_t time_ms); -uint8_t user_time_is_period_done(void); -uint8_t user_time_is_done(void); -void user_time_stop(void); - -#endif diff --git a/controllers/src/adc.c b/controllers/src/adc.c deleted file mode 100644 index be4e5efc8944cddb57337f488729b31ff71b683d..0000000000000000000000000000000000000000 --- a/controllers/src/adc.c +++ /dev/null @@ -1,167 +0,0 @@ -#include "cm510_cfg.h" -#include "adc.h" -#include "buzzer.h" -#include <util/delay.h> - -/* external functions */ -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); - -/* private variables */ -volatile uint16_t adc_ch_data[NUM_ADC][ADC_MAX_NUM_SAMPLES]; -volatile uint16_t adc_values[NUM_ADC]; -volatile uint16_t adc_avg_values[NUM_ADC]; -volatile uint8_t adc_current_ch; -volatile uint8_t adc_current_sample; -volatile uint8_t adc_voltage_alarm; - -/* private functions */ -void adc_set_channel(uint8_t ch_id) -{ - ADMUX&=0xF8; - ADMUX|=(ch_id&0x07); - PORTA=~(0x80>>ch_id);// enable the desired channel -} - -void adc_set_sample_period(void) -{ - uint16_t prescaler_values[7]={1,8,32,64,128,256,1024}; - uint16_t compare_value; - uint8_t i; - - for(i=0;i<5;i++) - { - compare_value=(((F_CPU/1000)*ADC_SAMPLE_PERIOD_MS)/(prescaler_values[i]*NUM_ADC))-1; - if(compare_value<256) - { - OCR2A=(uint8_t)compare_value; - TCCR2B=i+1; - break; - } - } -} - -void adc_start_conv(void) -{ - ADCSRA|=(1<<ADSC); -} - -uint8_t adc_is_conversion_done(void) -{ - if(ADCSRA&(1<<ADIF)) - return 0x01; - else - return 0x00; -} - -uint8_t adc_is_period_done(void) -{ - if(TIFR2&(1<<OCF2A)) - return 0x01; - else - return 0x00; -} - -void adc_loop(void) -{ - uint8_t i; - uint16_t data; - - if(adc_is_period_done()) - { - TIFR2|=(1<<OCF2A)|(1<<OCF2B)|(1<<TOV2);// clear interrupt flag - TCNT2=0x00;// start from 0 - // start a new conversion - adc_start_conv(); - } - else - { - if(adc_is_conversion_done()) - { - ADCSRA|=(1<<ADIF);// clear interrupt flag - data=(ADCL | (ADCH<<8)); - adc_values[adc_current_ch]=data; - adc_ch_data[adc_current_ch][adc_current_sample]=data; - // compute the average for the current channel - data=0; - for(i=0;i<ADC_MAX_NUM_SAMPLES;i++) - data+=adc_ch_data[adc_current_ch][i]; - data=data/ADC_MAX_NUM_SAMPLES; - adc_avg_values[adc_current_ch]=data; - if(adc_current_ch==NUM_ADC-1)// last channel - adc_current_sample=(adc_current_sample+1)%ADC_MAX_NUM_SAMPLES; - if(adc_current_ch==ADC_VCC)// monitor the Voltage supply - { -// voltage_mv=((uint32_t)data*(uint32_t)5000*(uint32_t)133)/((uint16_t)1024*(uint16_t)33); - // compare with hysteresis - if(adc_voltage_alarm==0x01) - { - if(data>=(ADC_VOLTAGE_ALARM_TH+ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V - { - buzzer_stop_alarm(); - adc_voltage_alarm=0x00; - } - } - else - { - if(data<(ADC_VOLTAGE_ALARM_TH-ADC_VOLTAGE_ALARM_WINDOW))// voltage under 11 V - { - buzzer_start_alarm(NOTE_LA,30,30); - adc_voltage_alarm=0x01; - } - } - } - adc_current_ch=(adc_current_ch+1)%NUM_ADC; - adc_set_channel(adc_current_ch); - } - } -} - -/* public functions */ -void init_adc(void) -{ - uint8_t i=0,j=0; - - ADCSRA=(1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);// enable ADC set prescaler 1/128 - // disable auto trigger, interrupts disabled - ADCSRB=0x00;// free running mode - ADMUX=0x00;// use external reference, right ajust the result - DIDR0=0x00;// enable all ADC channels - DIDR2=0x00;// enable all adc channels - - // configure the GPIO to enable the sensors - DDRA = 0xFC; - PORTA = 0xFC;// all sensors disabled - - // initialize internal sample variables to 12V to avoid initial voltage alarm - for(i=0;i<NUM_ADC;i++) - { - adc_values[i]=0x0266;//equivalent to 12 V - adc_avg_values[i]=0x0266;//equivalent to 12 V - for(j=0;j<ADC_MAX_NUM_SAMPLES;j++) - adc_ch_data[i][j]=0x0266; - } - adc_current_ch=0; - adc_voltage_alarm=0x00; - - // configure the timer 2 to perform periodic conversions (1 ms period) - TCCR2A=(1<<WGM21);// CTC mode, no output - TCNT2=0x00;// start from 0 - TIMSK2=0x00;// interrupts not enabled - TIFR2=0x07;// clear any pending interrupt - adc_set_sample_period();// set the default sample period - - // enable the first ADC channel - adc_set_channel(adc_current_ch); -} - -uint16_t get_adc_channel(adc_t channel_id) -{ - return adc_values[(uint8_t)channel_id]; -} - -uint16_t get_adc_avg_channel(adc_t channel_id) -{ - return adc_avg_values[(uint8_t)channel_id]; -} diff --git a/controllers/src/buzzer.c b/controllers/src/buzzer.c deleted file mode 100644 index 115cd534a008eb8e8198e3d11f7cd0465bee8f8b..0000000000000000000000000000000000000000 --- a/controllers/src/buzzer.c +++ /dev/null @@ -1,239 +0,0 @@ -#include "buzzer.h" -#include "adc.h" -#include "gpio.h" - -/* private variables */ -uint8_t buzzer_note_freq[NUM_NOTES]; -uint8_t buzzer_playing; -uint8_t buzzer_playing_alarm; -uint16_t buzzer_time_on_100ms; -uint16_t buzzer_time_off_100ms; -uint16_t buzzer_note; - -/* private functions */ -void buzzer_loop(void) -{ - static uint8_t playing=0x00; - static uint16_t current_time_on=0,current_time_off=0; - - if(TIFR3&(1<<OCF3A)) - { - TIFR3|=(1<<OCF3A);// clear any interrupt - TCNT3H=0x00; - TCNT3L=0x00; - if(playing==0x00)// currently not playing - { - 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 - { - 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_stop_alarm(void) -{ - if(buzzer_playing_alarm==0x01) - { - // disable PWM output - TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0))); - buzzer_playing_alarm=0x00; - } -} - -void buzzer_start_alarm(note_t note, uint16_t on_time_100ms,uint16_t off_time_100ms) -{ - if(buzzer_playing_alarm)// the alram is not playing - buzzer_stop_alarm(); - else if(buzzer_playing)// if the normal buzzer is playing, stop it. - buzzer_stop(); - // 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; - 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); - buzzer_playing_alarm=0x01; -} - -/* public functions */ -void init_buzzer(void) -{ - // configure the GPIO - DDRB |= 0x20;// configure bit 5 as output - PORTB &= 0xDF;// clear the ouput - - // initialize timer 1 to work as PWM (Fast PWM mode) - 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; - buzzer_note_freq[1]=26; - buzzer_note_freq[2]=24; - buzzer_note_freq[3]=22; - buzzer_note_freq[4]=20; - buzzer_note_freq[5]=18; - buzzer_note_freq[6]=16; - buzzer_playing=0x00; - buzzer_playing_alarm=0x00; - buzzer_time_on_100ms=0x01; - buzzer_time_off_100ms=0x01; - buzzer_note=NOTE_DO; -} - -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) - { - // 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; - 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; - } - } -} - -void buzzer_change_note(note_t note) -{ - if(buzzer_playing_alarm==0x00) - { - if(buzzer_playing==0x01) - { - // 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; - 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==0x01) - { - // disable PWM output - TCCR1A&=(~((1<<COM1A1)|(1<<COM1A0))); - buzzer_playing=0x00; - } - } -} - -uint8_t buzzer_is_playing(void) -{ - return buzzer_playing; -} - -uint8_t buzzer_is_playing_alarm(void) -{ - return buzzer_playing_alarm; -} diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c deleted file mode 100755 index 316ab01dded22f064b6e417654f27ecc27bafcc7..0000000000000000000000000000000000000000 --- a/controllers/src/cm510.c +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (c) 2011 Humanoid Lab - * - * This file is part of iri-libbioloid. iri-libbioloid is free software: you - * can redistribute it and/or modify it under the terms of the GNU Lesser - * Public License as published by the Free Software Foundation, either - * version 3 of the License, or (at your option) any later version. - * - * iri-libbioloid 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 Lesser Public License for more details. - * - * You should have received a copy of the GNU Lesser Public License - * along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>. - */ - -#include <avr/io.h> -#include <avr/interrupt.h> -#include <inttypes.h> -#include <util/delay.h> -#include "cm510_cfg.h" -#include "cm510.h" - -/* external user functions */ -extern void user_init(void); -extern void user_loop(void); - -/* internal loop functions */ -extern void pushbuttons_loop(void); -extern void adc_loop(void); -extern void buzzer_loop(void); -extern void manager_loop(void); -extern void user_time_loop(void); - -// general interface -void init_cm510(void) -{ - init_leds(); - init_buttons(); - init_adc(); - init_buzzer(); - init_user_time(); -} - -int16_t main(void) -{ - init_cm510(); - sei(); - manager_init(MANAGER_MAX_NUM_SERVOS); - /* call the user initialization function */ - user_init(); - // turn BAT_LED on to indicate the initialization is done - turn_led_on(LED_BAT); - - while(1) - { - pushbuttons_loop(); - adc_loop(); - buzzer_loop(); - manager_loop(); - user_time_loop(); - user_loop(); - } -} diff --git a/controllers/src/cm510/cm510.c b/controllers/src/cm510/cm510.c new file mode 100755 index 0000000000000000000000000000000000000000..b038601554ec3a25d7655ca539c2a7dc48cc09b8 --- /dev/null +++ b/controllers/src/cm510/cm510.c @@ -0,0 +1,580 @@ +/* + * Copyright (c) 2011 Humanoid Lab + * + * This file is part of iri-libbioloid. iri-libbioloid is free software: you + * can redistribute it and/or modify it under the terms of the GNU Lesser + * Public License as published by the Free Software Foundation, either + * version 3 of the License, or (at your option) any later version. + * + * iri-libbioloid 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 Lesser Public License for more details. + * + * You should have received a copy of the GNU Lesser Public License + * along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>. + */ + +#include "cm510.h" +#include <avr/io.h> +#include <avr/interrupt.h> +#include <inttypes.h> +#include <util/delay.h> + +// global private variables +typedef void (*func_ptr)(void *param); +volatile unsigned short int timer_prescaler_values[6]; + +// button private variables +volatile func_ptr button_func_ptr[NUM_BUTTONS]; +void *button_func_param[NUM_BUTTONS]; +volatile unsigned char button_old_value[NUM_BUTTONS]; + +// ADC private variables +volatile unsigned short int adc_values[NUM_ADC]; +volatile unsigned char adc_current_channel; +volatile unsigned char adc_mode; + +// buzzer private variables +unsigned char buzzer_note_freq[NUM_NOTES]; + +// timer private variables +volatile func_ptr timer_func; +void *timer_param; +volatile unsigned char timer_done; +volatile unsigned char timer_prescaler; + +// initialize the timer 0 for several internal functions (T=8ms) +SIGNAL(TIMER4_OVF_vect) +{ + unsigned char button_new_value[NUM_BUTTONS]; + + // reset the timer + TCNT4H=0xC1; // set the period to 1ms TCNT2 + TCNT4L=0x7F; + + // detect falling edge on the buttons + button_new_value[0]=((~PINE & BTN_UP)>>4); + if(button_new_value[0]==0x01 && button_old_value[0]==0x00) + if(button_func_ptr[0]!=0x0000) + button_func_ptr[0](button_func_param[0]);// call the callback function + button_old_value[0]=button_new_value[0]; + button_new_value[1]=((~PINE & BTN_DOWN)>>5); + if(button_new_value[1]==0x01 && button_old_value[1]==0x00) + if(button_func_ptr[1]!=0x0000) + button_func_ptr[1](button_func_param[1]);// call the callback function + button_old_value[1]=button_new_value[1]; + button_new_value[2]=((~PINE & BTN_LEFT)>>6); + if(button_new_value[2]==0x01 && button_old_value[2]==0x00) + if(button_func_ptr[2]!=0x0000) + button_func_ptr[2](button_func_param[2]);// call the callback function + button_old_value[2]=button_new_value[2]; + button_new_value[3]=((~PINE & BTN_RIGHT)>>7); + if(button_new_value[3]==0x01 && button_old_value[3]==0x00) + if(button_func_ptr[3]!=0x0000) + button_func_ptr[3](button_func_param[3]);// call the callback function + button_old_value[3]=button_new_value[3]; + button_new_value[4]=((~PIND & BTN_START)); + if(button_new_value[4]==0x01 && button_old_value[4]==0x00) + if(button_func_ptr[4]!=0x0000) + button_func_ptr[4](button_func_param[4]);// call the callback function + button_old_value[4]=button_new_value[4]; + if(adc_mode==ASYNC) + { + // convert the next channel + ADCSRA |= (1 << ADIF); // AD-Conversion Interrupt Flag Clear + adc_values[adc_current_channel]=ADC; + adc_current_channel=(adc_current_channel+1)%NUM_ADC; + // enable next channel + PORTA=~(0x80>>adc_current_channel); + // select next channel + ADMUX=(adc_current_channel+1); + // wait for the signal to be stable + _delay_us(12); + // start conversion on first channel + ADCSRA|=(1 << ADSC); + } +} + +void init_cm510_timer(void) +{ + // initialize timer 4 + TCCR4B=((TCCR4B & ~0x07) | 0x02);// set the prescaler to 64 + TCNT4H=0xC1; // set the period to 8ms TCNT4 + TCNT4L=0x7F; + TIMSK4|=(0x01<<TOIE4); // enable TCNT4 overflow +} + +// general interface +unsigned char init_cm510(unsigned char adc_mode) +{ + timer_prescaler_values[0]=0; + timer_prescaler_values[1]=1; + timer_prescaler_values[2]=8; + timer_prescaler_values[3]=64; + timer_prescaler_values[4]=256; + timer_prescaler_values[5]=1024; + + init_leds(); + init_buttons(); + if(init_adc(adc_mode)==0xFF) + return 0xFF; + init_buzzer(); + init_timer(); + init_cm510_timer(); + + sei(); + + return 0x00; +} + +// leds interface +unsigned char init_leds(void) +{ + // configure the PORTC as outputs for the LED's + DDRC = 0x7F; + // all LED's off except for the Battery LED + PORTC = 0x7E; + + return 0x00; +} + +unsigned char turn_led_on(unsigned char led_id) +{ + unsigned char ret_value=0x00; + + switch(led_id) + { + case LED_BAT: PORTC &= ~LED_BAT; + break; + case LED_TxD: PORTC &= ~LED_TxD; + break; + case LED_RxD: PORTC &= ~LED_RxD; + break; + case LED_AUX: PORTC &= ~LED_AUX; + break; + case LED_MANAGE: PORTC &= ~LED_MANAGE; + break; + case LED_PROGRAM: PORTC &= ~LED_PROGRAM; + break; + case LED_PLAY: PORTC &= ~LED_PLAY; + break; + default: ret_value=0xFF; + break; + } + + return ret_value; +} + +unsigned char turn_led_off(unsigned char led_id) +{ + unsigned char ret_value=0x00; + + switch(led_id) + { + case LED_BAT: PORTC |= LED_BAT; + break; + case LED_TxD: PORTC |= LED_TxD; + break; + case LED_RxD: PORTC |= LED_RxD; + break; + case LED_AUX: PORTC |= LED_AUX; + break; + case LED_MANAGE: PORTC |= LED_MANAGE; + break; + case LED_PROGRAM: PORTC |= LED_PROGRAM; + break; + case LED_PLAY: PORTC |= LED_PLAY; + break; + default: ret_value=0xFF; + break; + } + + return ret_value; +} + +unsigned char toggle_led(unsigned char led_id) +{ + unsigned char ret_value=0x00; + + switch(led_id) + { + case LED_BAT: PORTC ^= LED_BAT; + break; + case LED_TxD: PORTC ^= LED_TxD; + break; + case LED_RxD: PORTC ^= LED_RxD; + break; + case LED_AUX: PORTC ^= LED_AUX; + break; + case LED_MANAGE: PORTC ^= LED_MANAGE; + break; + case LED_PROGRAM: PORTC ^= LED_PROGRAM; + break; + case LED_PLAY: PORTC ^= LED_PLAY; + break; + default: ret_value=0xFF; + break; + } + + return ret_value; +} + +unsigned char is_led_on(unsigned char led_id) +{ + unsigned char value=0x00; + + switch(led_id) + { + case LED_BAT: value = (PINC & LED_BAT)>>6; + break; + case LED_TxD: value = (PINC & LED_TxD)>>5; + break; + case LED_RxD: value = (PINC & LED_RxD)>>4; + break; + case LED_AUX: value = (PINC & LED_AUX)>>3; + break; + case LED_MANAGE: value = (PINC & LED_MANAGE)>>2; + break; + case LED_PROGRAM: value = (PINC & LED_PROGRAM)>>1; + break; + case LED_PLAY: value = (PINC & LED_PLAY); + break; + default: value=0xFF; + break; + } + + return value; +} + +// buttons interface +unsigned char init_buttons(void) +{ + unsigned char i=0; + + DDRD &= 0xFE; + PORTD |= 0x01; + + DDRE &= 0x0F; + PORTE |= 0xF0; + + for(i=0;i<NUM_BUTTONS;i++) + { + button_func_ptr[i]=0x0000; + button_func_ptr[i]=0x0000; + button_old_value[i]=0x00; + } + + return 0x00; +} + +unsigned char is_button_pressed(unsigned char button_id) +{ + unsigned char value; + + switch(button_id) + { + case BTN_UP: value=(~PINE & BTN_UP) >> 4; + break; + case BTN_DOWN: value=(~PINE & BTN_DOWN) >> 5; + break; + case BTN_LEFT: value=(~PINE & BTN_LEFT) >> 6; + break; + case BTN_RIGHT: value=(~PINE & BTN_RIGHT) >> 7; + break; + case BTN_START: value=(~PIND & BTN_START); + break; + default: value=0xFF; + break; + } + + return value; +} + +unsigned char button_assign_callback(unsigned char button_id, void (*function)(void *param),void *param) +{ + unsigned char value=0x00; + + switch(button_id) + { + case BTN_UP: button_func_ptr[0]=function; + button_func_param[0]=param; + break; + case BTN_DOWN: button_func_ptr[1]=function; + button_func_param[1]=param; + break; + case BTN_LEFT: button_func_ptr[2]=function; + button_func_param[2]=param; + break; + case BTN_RIGHT: button_func_ptr[3]=function; + button_func_param[3]=param; + break; + case BTN_START: button_func_ptr[4]=function; + button_func_param[4]=param; + break; + default: value=0xFF; + break; + } + + return value; +} + +// ADC interface +ISR(ADC_vect) +{ + ADCSRA |= (1 << ADIF); // AD-Conversion Interrupt Flag Clear + adc_values[adc_current_channel]=ADC; + adc_current_channel=(adc_current_channel+1)%NUM_ADC; + // enable next channel + PORTA=~(0x80>>adc_current_channel); + // select next channel + ADMUX=(adc_current_channel+1); + // wait for the signal to be stable + _delay_us(12); + // start conversion on first channel + ADCSRA|=(1 << ADSC); +} + +unsigned char init_adc(unsigned char mode) +{ + unsigned char i=0; + + ADCSRA=(1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);// enable ADC set prescaler 1/128 + // disable auto trigger + ADCSRB=0x00; + ADMUX=ADC_PORT_1; // use external reference, right ajust the result + // select first channel + + // configure the GPIO to enable the sensors + DDRA = 0xFC; + PORTA = 0xFC;// all sensors disabled + + for(i=0;i<NUM_ADC;i++) + adc_values[i]=0x0000; + adc_current_channel=0; + adc_mode=mode; + + if(mode==ASYNC) + { +// ADCSRA|=(1<<ADIE);// enable interrupts + // enable first sensor + PORTA&= ~0x80; + // wait for the signal to be stable + _delay_us(12); + // start conversion on first channel + ADCSRA|=(1 << ADSC); + } + + return 0x00; +} + +unsigned short int get_adc_channel(unsigned char channel_id) +{ + if(channel_id>NUM_ADC) + return 0xFFFF; + else + { + if(adc_mode==ASYNC) + return adc_values[channel_id-1]; + else + { + PORTA=~(0x80>>(channel_id-1));// enable the desired channel + _delay_us(12); + ADMUX=((ADMUX&0xF8)|channel_id); + ADCSRA |= (1 << ADIF);// AD-Conversion Interrupt Flag Clear + ADCSRA |= (1 << ADSC);// start the conversion + while( !(ADCSRA & (1 << ADIF)) );// Wait until AD-Conversion completes + PORTA = 0xFC;// disable all sensors + return ADC; + } + } +} + +// buzzer interface +unsigned char init_buzzer(void) +{ + // configure the GPIO + DDRB |= 0x20;// configure bit 5 as output + PORTB &= 0xDF;// clear the ouput + + // initialize timer 1 to work as PWM (Fast PWM mode) + TCCR1B=((TCCR1B & ~0x07) | 0x00);// set the prescaler to 1024 + TCNT1H=0x00; + TCNT1L=0x00; + // set PWM mode with ICR top-count + TCCR1A&=~(1<<WGM10); + TCCR1A|=(1<<WGM11); + TCCR1B|=(1<<WGM12); + TCCR1B|=(1<<WGM13); + // clear output compare value A + OCR1AH=0x00; + OCR1AL=0x00; + + // initialize the note frequencies + buzzer_note_freq[0]=29; + buzzer_note_freq[1]=26; + buzzer_note_freq[2]=24; + buzzer_note_freq[3]=22; + buzzer_note_freq[4]=20; + buzzer_note_freq[5]=18; + buzzer_note_freq[6]=16; + + return 0x00; +} + +unsigned char buzzer_start(unsigned char note) +{ + if(note>=NUM_NOTES) + return 0xFF; + else + { + 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 + TCCR1A|=(1<<COM1A1); + TCCR1A&=(~(1<<COM1A0)); + + return 0x00; + } +} + +unsigned char buzzer_change_note(unsigned char note) +{ + if(note>=NUM_NOTES) + return 0xFF; + else + { + 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(); + + return 0x00; + } +} + +unsigned char buzzer_stop(void) +{ + TCCR1B=((TCCR1B & ~0x07) | 0x00);// set the prescaler to 1024 + // turn off channel A (OC1A) PWM output + // set OC1A (OutputCompare action) to none + TCCR1A&=(~(1<<COM1A1)); + TCCR1A&=(~(1<<COM1A0)); + + return 0x00; +} + +// timer interface +SIGNAL(TIMER3_OVF_vect) +{ + if(timer_func!=0x0000) + timer_func(timer_param); + + timer_done=0x01; + + TCCR3B=(TCCR3B & 0xF8);// set the prescaler to 0 (timer disabled) + TIMSK3&=(~(0x01<<TOIE3));// disable TCNT2 overflow +} + +unsigned char init_timer(void) +{ + timer_func=0x0000; + timer_param=0x0000; + timer_done=0x01; + + timer_prescaler = 0; + + // initialize timer 2 + TCCR3B=(TCCR3B & 0xF8);// set the prescaler to 0 (timer disabled) + TCNT3H=0x00; + TCNT3L=0x00; + // disable interrupts + TIMSK3&=(~(0x01<<TOIE3)); + + return 0x00; +} + +unsigned char timer_set_time(unsigned short int time_ms) +{ + unsigned char i; + unsigned long int count=0; + + for(i=1;i<6;i++) + { + count=time_ms*(F_CPU/1000)/timer_prescaler_values[i]; + if(count<0xFFFF) + break; + } + if(count>0xFFFF) + return 0xFF; + else + { + timer_prescaler=i; + + TCNT3H=(((0xFFFF-count)>>8)&0xFF); + TCNT3L=((0xFFFF-count)&0xFF); + + return 0x00; + } +} + +unsigned char timer_start(void) +{ + if(timer_prescaler!=0) + { + timer_done=0x00; + + // set the prescaler to a value != 0 to enable the timer + TCCR3B=((TCCR3B & 0xF8) | timer_prescaler);// set the prescaler to correct value + TIMSK3|=(0x01<<TOIE3); // enable TCNT3 overflow + + return 0x00; + } + else + return 0xFF; +} + +unsigned char timer_set_callback(void (*function)(void *param),void *param) +{ + timer_func=function; + timer_param=param; + + return 0x00; +} + +unsigned char timer_stop(void) +{ + timer_done=0x01; + + // set prescaler to 0 + TCCR3B=((TCCR3B & 0xF8) | timer_prescaler_values[0]);// set the prescaler to correct value + // reset count value + TCNT3H=0x00; + TCNT3L=0x00; + // disable interrupts + TIMSK3&=(~(0x01<<TOIE3)); + + return 0x00; +} + +unsigned char is_timer_done(void) +{ + return timer_done; +} + +unsigned char timer_wait(void) +{ + while(timer_done==0x00); + + return 0x00; +} diff --git a/controllers/src/examples/Makefile b/controllers/src/examples/Makefile deleted file mode 100644 index db83592ea8c4d3501365f3b0ed1bd8bc74b83a87..0000000000000000000000000000000000000000 --- a/controllers/src/examples/Makefile +++ /dev/null @@ -1,43 +0,0 @@ -PROJECT=cm510_ex -######################################################## -# afegir tots els fitxers que s'han de compilar aquà -######################################################## -SOURCES=main.c - -OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -DEV_DIR=../../../dyn_devices/ -COMM_DIR=../../../communications/ -MAN_DIR=../../../motion/ -CONT_DIR=../../ -CC=avr-gcc -OBJCOPY=avr-objcopy -MMCU=atmega2561 - -INCLUDE_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(MAN_DIR)include -I$(DEV_DIR)include - -LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a - -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) $(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/controllers/src/examples/main.c b/controllers/src/examples/main.c index fa6923951b9dc0c57f42948db90fbaedee7b255d..a5be94820c12ccc1f9a080bdecd68bf191c9375a 100755 --- a/controllers/src/examples/main.c +++ b/controllers/src/examples/main.c @@ -15,32 +15,66 @@ * along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>. */ -#include "cm510.h" -#include <stdio.h> -#include <util/delay.h> - -void user_init(void) -{ - serial_console_init(57600); - user_time_set_period(10); -} - -void user_loop(void) -{ - if(user_time_is_period_done()) - { - printf("Gyro X: %d\n",get_adc_avg_channel(ADC_PORT_3)); - printf("Gyro Y: %d\n",get_adc_avg_channel(ADC_PORT_4)); - } - if(is_button_pressed(BTN_START)) - turn_led_on(LED_AUX); - else - turn_led_off(LED_AUX); - if(is_button_falling_edge(BTN_UP)) - buzzer_start(NOTE_SOL,30,30); - else - { - if(is_button_falling_edge(BTN_DOWN)) - buzzer_stop(); - } -} +#include "cm510.h" +#include "serial.h" +#include <stdio.h> +#include <util/delay.h> + +void toggle_aux_led(void *param) +{ + toggle_led(LED_AUX); +} + +void play_do(void *param) +{ + toggle_led(LED_MANAGE); +} + +void play_re(void *param) +{ + toggle_led(LED_PROGRAM); +} + +void play_mi(void *param) +{ + toggle_led(LED_PLAY); +} + +void play_fa(void *param) +{ + toggle_led(LED_TxD); +} + +void play_stop(void *param) +{ + toggle_led(LED_RxD); +} + +int main(void) +{ + init_cm510(ASYNC); + + serial_initialize(57600); + + timer_set_time(1000); + timer_set_callback(toggle_aux_led,0x0000); + button_assign_callback(BTN_UP,play_do,0x0000); + button_assign_callback(BTN_DOWN,play_re,0x0000); + button_assign_callback(BTN_LEFT,play_mi,0x0000); + button_assign_callback(BTN_RIGHT,play_fa,0x0000); + button_assign_callback(BTN_START,play_stop,0x0000); + + timer_start(); + timer_wait(); + + while(1) + { +/* printf("%d, %d, %d, %d, %d, %d\n",get_adc_channel(ADC_PORT_1), + get_adc_channel(ADC_PORT_2), + get_adc_channel(ADC_PORT_3), + get_adc_channel(ADC_PORT_4), + get_adc_channel(ADC_PORT_5), + get_adc_channel(ADC_PORT_6));*/ + _delay_ms(100); + } +} diff --git a/controllers/src/gpio.c b/controllers/src/gpio.c deleted file mode 100644 index 3e0ae9fd481933984aea8f102de344b7c5cb6343..0000000000000000000000000000000000000000 --- a/controllers/src/gpio.c +++ /dev/null @@ -1,357 +0,0 @@ -#include "gpio.h" - -typedef struct{ - volatile uint8_t pressed; - volatile uint8_t rising_edge; - volatile uint8_t falling_edge; - volatile uint8_t old_state; -}TButtonState; - -/* private variables */ -TButtonState button_up; -TButtonState button_down; -TButtonState button_left; -TButtonState button_right; -TButtonState button_start; - -/* private functions */ -void pushbuttons_loop(void) -{ - if((~PINE & BTN_UP)!=0x00) - { - button_up.pressed=0x01; - if(button_up.old_state==0x00) - { - button_up.rising_edge=0x01; - button_up.old_state=0x01; - } - } - else - { - if(button_up.old_state==0x01) - { - button_up.falling_edge=0x01; - button_up.old_state=0x00; - } - } - if((~PINE & BTN_DOWN)!=0x00) - { - button_down.pressed=0x01; - if(button_down.old_state==0x00) - { - button_down.rising_edge=0x01; - button_down.old_state=0x01; - } - } - else - { - if(button_down.old_state==0x01) - { - button_down.falling_edge=0x01; - button_down.old_state=0x00; - } - } - if((~PINE & BTN_LEFT)!=0x00) - { - button_left.pressed=0x01; - if(button_left.old_state==0x00) - { - button_left.rising_edge=0x01; - button_left.old_state=0x01; - } - } - else - { - if(button_left.old_state==0x01) - { - button_left.falling_edge=0x01; - button_left.old_state=0x00; - } - } - if((~PINE & BTN_RIGHT)!=0x00) - { - button_right.pressed=0x01; - if(button_right.old_state==0x00) - { - button_right.rising_edge=0x01; - button_left.old_state=0x01; - } - } - else - { - if(button_right.old_state==0x01) - { - button_right.falling_edge=0x01; - button_left.old_state=0x00; - } - } - if((~PIND & BTN_START)!=0x00) - { - button_start.pressed=0x01; - if(button_start.old_state==0x00) - { - button_start.rising_edge=0x01; - button_left.old_state=0x01; - } - } - else - { - if(button_start.old_state==0x01) - { - button_start.falling_edge=0x01; - button_left.old_state=0x00; - } - } -} - -/* public functions */ -void init_leds(void) -{ - // configure the PORTC as outputs for the LED's - DDRC = 0x7F; - // all LED's off except for the Battery LED - PORTC = 0x7E; - - /* initialize internal variables */ - button_up.pressed=0x00; - button_up.rising_edge=0x00; - button_up.falling_edge=0x00; - button_up.old_state=0x00; - button_down.pressed=0x00; - button_down.rising_edge=0x00; - button_down.falling_edge=0x00; - button_down.old_state=0x00; - button_left.pressed=0x00; - button_left.rising_edge=0x00; - button_left.falling_edge=0x00; - button_left.old_state=0x00; - button_right.pressed=0x00; - button_right.rising_edge=0x00; - button_right.falling_edge=0x00; - button_right.old_state=0x00; - button_start.pressed=0x00; - button_start.rising_edge=0x00; - button_start.falling_edge=0x00; - button_start.old_state=0x00; -} - -void turn_led_on(leds_t led_id) -{ - switch(led_id) - { - case LED_BAT: PORTC &= ~LED_BAT; - break; - case LED_TxD: PORTC &= ~LED_TxD; - break; - case LED_RxD: PORTC &= ~LED_RxD; - break; - case LED_AUX: PORTC &= ~LED_AUX; - break; - case LED_MANAGE: PORTC &= ~LED_MANAGE; - break; - case LED_PROGRAM: PORTC &= ~LED_PROGRAM; - break; - case LED_PLAY: PORTC &= ~LED_PLAY; - break; - } -} - -void turn_led_off(leds_t led_id) -{ - switch(led_id) - { - case LED_BAT: PORTC |= LED_BAT; - break; - case LED_TxD: PORTC |= LED_TxD; - break; - case LED_RxD: PORTC |= LED_RxD; - break; - case LED_AUX: PORTC |= LED_AUX; - break; - case LED_MANAGE: PORTC |= LED_MANAGE; - break; - case LED_PROGRAM: PORTC |= LED_PROGRAM; - break; - case LED_PLAY: PORTC |= LED_PLAY; - break; - } -} - -void toggle_led(leds_t led_id) -{ - switch(led_id) - { - case LED_BAT: PORTC ^= LED_BAT; - break; - case LED_TxD: PORTC ^= LED_TxD; - break; - case LED_RxD: PORTC ^= LED_RxD; - break; - case LED_AUX: PORTC ^= LED_AUX; - break; - case LED_MANAGE: PORTC ^= LED_MANAGE; - break; - case LED_PROGRAM: PORTC ^= LED_PROGRAM; - break; - case LED_PLAY: PORTC ^= LED_PLAY; - break; - } -} - -uint8_t is_led_on(leds_t led_id) -{ - uint8_t value=0x00; - - switch(led_id) - { - case LED_BAT: value = (PINC & LED_BAT)>>6; - break; - case LED_TxD: value = (PINC & LED_TxD)>>5; - break; - case LED_RxD: value = (PINC & LED_RxD)>>4; - break; - case LED_AUX: value = (PINC & LED_AUX)>>3; - break; - case LED_MANAGE: value = (PINC & LED_MANAGE)>>2; - break; - case LED_PROGRAM: value = (PINC & LED_PROGRAM)>>1; - break; - case LED_PLAY: value = (PINC & LED_PLAY); - break; - } - - return value; -} - -void init_buttons(void) -{ - DDRD &= 0xFE; - PORTD |= 0x01; - - DDRE &= 0x0F; - PORTE |= 0xF0; -} - -uint8_t is_button_pressed(pushbuttons_t button_id) -{ - unsigned char value=0x00; - - switch(button_id) - { - case BTN_UP: if(button_up.pressed) - { - value=0x01; - button_up.pressed=0x00; - } - break; - case BTN_DOWN: if(button_down.pressed) - { - value=0x01; - button_down.pressed=0x00; - } - break; - case BTN_LEFT: if(button_left.pressed) - { - value=0x01; - button_left.pressed=0x00; - } - break; - case BTN_RIGHT: if(button_right.pressed) - { - value=0x01; - button_right.pressed=0x00; - } - break; - case BTN_START: if(button_start.pressed) - { - value=0x01; - button_start.pressed=0x00; - } - break; - } - - return value; -} - -uint8_t is_button_rising_edge(pushbuttons_t button_id) -{ - unsigned char value=0x00; - - switch(button_id) - { - case BTN_UP: if(button_up.rising_edge) - { - value=0x01; - button_up.rising_edge=0x00; - } - break; - case BTN_DOWN: if(button_down.rising_edge) - { - value=0x01; - button_down.rising_edge=0x00; - } - break; - case BTN_LEFT: if(button_left.rising_edge) - { - value=0x01; - button_left.rising_edge=0x00; - } - break; - case BTN_RIGHT: if(button_right.rising_edge) - { - value=0x01; - button_right.rising_edge=0x00; - } - break; - case BTN_START: if(button_start.rising_edge) - { - value=0x01; - button_start.rising_edge=0x00; - } - break; - } - - return value; -} - -uint8_t is_button_falling_edge(pushbuttons_t button_id) -{ - unsigned char value=0x00; - - switch(button_id) - { - case BTN_UP: if(button_up.falling_edge) - { - value=0x01; - button_up.falling_edge=0x00; - } - break; - case BTN_DOWN: if(button_down.falling_edge) - { - value=0x01; - button_down.falling_edge=0x00; - } - break; - case BTN_LEFT: if(button_left.falling_edge) - { - value=0x01; - button_left.falling_edge=0x00; - } - break; - case BTN_RIGHT: if(button_right.falling_edge) - { - value=0x01; - button_right.falling_edge=0x00; - } - break; - case BTN_START: if(button_start.falling_edge) - { - value=0x01; - button_start.falling_edge=0x00; - } - break; - } - - return value; -} - diff --git a/controllers/src/user_time.c b/controllers/src/user_time.c deleted file mode 100644 index 90a29a408040df38c986d49bd3ea1a76e0badb42..0000000000000000000000000000000000000000 --- a/controllers/src/user_time.c +++ /dev/null @@ -1,126 +0,0 @@ -#include "user_time.h" - -/* private variables */ -uint8_t user_time_single; -uint16_t user_time_time; -uint16_t user_time_period; -volatile uint8_t user_time_done; -volatile uint8_t user_time_period_done; - -/* private funcitons */ -void user_time_loop(void) -{ - static uint16_t count=0; - - if(TIFR5&(1<<OCF5A)) - { - TIFR5|=(1<<OCF5A);// clear any interrupt - TCNT5H=0x00; - TCNT5L=0x00; - if(user_time_single==0x01) - { - if(count==user_time_time) - { - count=0; - user_time_done=0x01; - TCCR5B&=0xF8;// disable timer - } - else - count++; - } - else - { - if(count==user_time_period) - { - count=0; - user_time_period_done=0x01; - } - else - count++; - } - } -} - -/* public functions */ -void init_user_time(void) -{ - // initialize timer3 for buzzer time control (100ms period) - TIMSK5=0x00;// disable all interrupts - TIFR5=0x2F;// clear any pending interrupt - TCNT5H=0x00; - TCNT5L=0x00; - // set PWM mode with ICR top-count (CTC mode) - TCCR5A=0x00; - TCCR5A&=~(1<<WGM50); - TCCR5A&=~(1<<WGM51); - TCCR5B=0x00; - TCCR5B|=(1<<WGM52); - TCCR5B&=~(1<<WGM53); - // clear output compare value A - OCR5AH=0x18; - OCR5AL=0x6A; - TCCR5B&=0xF8;// disable timer by default - - /* initialize internal variables */ - user_time_single=0x00; - user_time_time=0x00; - user_time_period=0x00; - user_time_done=0x00; - user_time_period_done=0x00; -} - -void user_time_set_period(uint16_t period_ms) -{ - user_time_single=0x00; - user_time_period=period_ms/100; - user_time_period_done=0x00; - TCNT5H=0x00; - TCNT5L=0x00; - TIFR5=0x2F;// clear any pending interrupt - TCCR5B&=0xF8;// disable timer by default - TCCR5B|=0x04; -} - -void user_time_set_one_time(uint16_t time_ms) -{ - user_time_single=0x01; - user_time_time=time_ms/100; - user_time_done=0x00; - TCNT5H=0x00; - TCNT5L=0x00; - TIFR5=0x2F;// clear any pending interrupt - TCCR5B&=0xF8;// disable timer by default - TCCR5B|=0x04; -} - -uint8_t user_time_is_period_done(void) -{ - if(user_time_period_done==0x01) - { - user_time_period_done=0x00; - return 0x01; - } - else - return 0x00; -} - -uint8_t user_time_is_done(void) -{ - if(user_time_done==0x01) - { - user_time_done=0x00; - return 0x01; - } - else - return 0x00; -} - -void user_time_stop(void) -{ - TCCR5B&=0xF8;// disable timer - TIFR5|=(1<<OCF5A);// clear any interrupt - TCNT5H=0x00; - TCNT5L=0x00; - user_time_period_done=0x00; - user_time_done=0x00; -} diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile index 44b6332e1d6be66947d0201478045048d607c92e..2e08fdf5ad7ea3030088d9ad85a1156eaaa3d872 100755 --- a/dyn_devices/Makefile +++ b/dyn_devices/Makefile @@ -1,25 +1,51 @@ +ROBOTIS_C_DIR ?= + +ifdef ROBOTIS_C_DIR + ROBOTIS_C_DIR_FOUND=true +else + ROBOTIS_C_DIR_FOUND=false +endif + 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/ -LIB_DIR=./lib/ -COMM_DIR=../communications +SDIR=./src/ +IDIR=./include/ +BDIR=./build/ +LDIR=./lib/ CC=avr-gcc -OBJCOPY=avr-ar +OBJCOPY=avr-ar rsc MMCU=atmega2561 -INC_DIRS=-I./include/ -I$(COMM_DIR)/include - -LIBS=$(COMM_DIR)/lib/libcomm.a +ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/ +ROBOTIS_LIBRARY_DIR=$(ROBOTIS_C_DIR)/lib/ CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes -ARFLAGS=rsc +.PHONY: all check_robotis_c show_banner clean -.PHONY: all show_banner clean +all: check_robotis_c show_banner $(PROJECT).a -all: show_banner communications $(PROJECT).a +check_robotis_c: + @if ! $(ROBOTIS_C_DIR_FOUND); then \ + echo " IMPORTANT !!! "; \ + echo " ROBOTIS_C_DIR variable is not set! The software won't compile"; \ + echo " unless you specify the path to Robotis embedded C software."; \ + echo ""; \ + echo " You should:"; \ + echo " 1. Edit the makefile, at the begging set up ROBOTIS_C_DIR"; \ + echo " 2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \ + echo ""; \ + echo " Beware of:"; \ + echo " 1. Do not use '(' in the directory name (as it comes from robotis) "; \ + echo " 2. Absolute path is preferred to avoid problems with submake calls"; \ + exit 2; \ + fi + + @if [ ! -d $(ROBOTIS_C_DIR) ]; then \ + echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it."; \ + exit 2; \ + fi show_banner: @echo "------------------------------------------------------"; @@ -34,18 +60,11 @@ show_banner: @echo "------------------------------------------------------"; $(PROJECT).a: ${OBJS} - $(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS) + $(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS) %.o: %.c - $(CC) -c $(CFLAGS) $(INC_DIRS) -o $@ $< - -communications: - $(MAKE) -C $(COMM_DIR) - -examples: - $(MAKE) -C src/examples + $(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -L${ROBOTIS_LIBRARY_DIR} -ldynamixel -o $@ $< clean: - rm -f ${LIB_DIR}$(PROJECT).a + rm -f ${LDIR}$(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 f7b0ba738b0413ce43fe7890c203a9a701dc299a..f16bd92dfafe7b434a8bfcbbc7e6e1f351a17170 100755 --- a/dyn_devices/include/dyn_servos.h +++ b/dyn_devices/include/dyn_servos.h @@ -18,9 +18,6 @@ #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 @@ -40,6 +37,40 @@ #define IS_INPUT_VOLTAGE_ERROR_SET(A) (A&INPUT_VOLTAGE_ERROR) +// low level dynamixel functions +/** + * \brief function to synchronous write to multiple servos + * + * This function implements the synchronous write of the Dynamixel protocol. This + * write operation allows to write the same addresses of multiple servos with a + * single operation. This function uses the broadcast identifier (0xFE) and + * receives no status packet as an answer. + * + * This function does nothing is any of the parameters are incorrect. + * + * \param num_servos An integer with the number of servos that are accessed by + * this operation. This value is used to determine the size + * of the other parameters. + * \param servo_ids a vector with the identifier of all the servos that have to + * be accessed. The length of this vector must be equal to the + * num_servos value. + * \param start_addr the address from which to start writing the data on all + * servos. This value, together with the length parameter must + * be compatible with the memory map of each servo to avoid + * unecpected errors. + * \param data a vector with the data, for each servo, to be written. This vector + * must have a length equal to num_servos*length, and the information + * must be organized as follows: + * + * servo 1 data | servo 2 data | ··· | servo n data + * + * \param length the total length, in bytes, of the imformation to be written to + * each servo. + * + * \return 0 if the function completes correctly, -1 otherwise + */ +unsigned char dxl_sync_tx_packet(unsigned char num_servos,unsigned char *servo_ids,unsigned char start_addr, unsigned char *data, int length); + // servo interface functions EEPROM memory /** * \brief function to get the model number of a given servo @@ -68,7 +99,7 @@ * is returned in case of any error. * */ -uint16_t get_model_number(uint8_t servo_id); +unsigned short int get_model_number(unsigned char servo_id); /** * \brief function to get the firmware version of a given servo * @@ -81,7 +112,7 @@ uint16_t get_model_number(uint8_t servo_id); * \return the current firmware version of the servo or 0xFFFF in case of any * error. */ -uint8_t get_firmware_version(uint8_t servo_id); +unsigned char get_firmware_version(unsigned char servo_id); /** * \brief function to change the current identifier of a given servo * @@ -98,7 +129,7 @@ uint8_t get_firmware_version(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_servo_id(uint8_t old_id, uint8_t new_id); +unsigned char set_servo_id(unsigned char old_id, unsigned char new_id); /** * \brief Function to get the current baudrate of the bus * @@ -116,7 +147,7 @@ uint8_t set_servo_id(uint8_t old_id, uint8_t new_id); * * \return the baudrate code of the given servo or 0xFF in case of any error. */ -uint8_t get_baudrate(uint8_t servo_id); +unsigned char get_baudrate(unsigned char servo_id); /** * \brief Function to change the current baudrate of the given servo. * @@ -134,7 +165,7 @@ uint8_t get_baudrate(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate); +unsigned char set_baudrate(unsigned char servo_id, unsigned char baudrate); /** * \brief Function to get the current angle limits of a given servo. * @@ -157,7 +188,7 @@ uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw); +unsigned char get_angle_limits(unsigned char servo_id, unsigned short int *cw, unsigned short int *ccw); /** * \brief Function to set the angle limits of a given servo * @@ -179,7 +210,7 @@ uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw); +unsigned char set_angle_limits(unsigned char servo_id, unsigned short int cw, unsigned short int ccw); /** * \brief Function to get the temperature limit of a given servo * @@ -193,7 +224,7 @@ uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw); * * \return the current temperature limit or 0xFF in case of any error. */ -uint8_t get_temperature_limit(uint8_t servo_id); +unsigned char get_temperature_limit(unsigned char servo_id); /** * \brief Function to change the temperature limit of a given servo * @@ -210,7 +241,7 @@ uint8_t get_temperature_limit(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp); +unsigned char set_temperature_limit(unsigned char servo_id, unsigned char temp); /** * \brief Function to get the operating voltage range of a given servo * @@ -230,7 +261,7 @@ uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high); +unsigned char get_voltage_limits(unsigned char servo_id, unsigned char *low, unsigned char *high); /** * \brief Function to change the operating voltage range of a given servo * @@ -249,7 +280,7 @@ uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high); +unsigned char set_voltage_limits(unsigned char servo_id, unsigned char low, unsigned char high); /** * \brief Function to get the maximum servo torque * @@ -267,7 +298,7 @@ uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high); * * \return the current maximum torque allowed or 0xFFFF in case of any error. */ -uint16_t get_max_torque(uint8_t servo_id); +unsigned short int get_max_torque(unsigned char servo_id); /** * \brief Function to change the maximum allowed torque * @@ -288,7 +319,7 @@ uint16_t get_max_torque(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_max_torque(uint8_t servo_id, uint16_t torque); +unsigned char set_max_torque(unsigned char servo_id, unsigned short int torque); /** * \brief function to get the current led alarm configuration * @@ -312,7 +343,7 @@ uint8_t set_max_torque(uint8_t servo_id, uint16_t torque); * * \return the current led alarm mask or 0xFF in case of any error */ -uint8_t get_led_alarm(uint8_t servo_id); +unsigned char get_led_alarm(unsigned char servo_id); /** * \brief Function to set the led alarm configuration * @@ -339,7 +370,7 @@ uint8_t get_led_alarm(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms); +unsigned char set_led_alarm(unsigned char servo_id, unsigned char alarms); /** * \brief function to get the current shutdown alarm configuration * @@ -363,7 +394,7 @@ uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms); * * \return the current led alarm mask or 0xFF in case of any error */ -uint8_t get_shd_alarm(uint8_t servo_id); +unsigned char get_shd_alarm(unsigned char servo_id); /** * \brief Function to set the led alarm configuration * @@ -389,7 +420,7 @@ uint8_t get_shd_alarm(uint8_t servo_id); * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms); +unsigned char set_shd_alarm(unsigned char servo_id, unsigned char alarms); // servo interface functions RAM memory /** @@ -406,7 +437,7 @@ uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_wheel_mode(uint8_t servo_id); +unsigned char set_wheel_mode(unsigned char servo_id); /** * \brief Function to set the joint mode * @@ -421,7 +452,7 @@ uint8_t set_wheel_mode(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_joint_mode(uint8_t servo_id); +unsigned char set_joint_mode(unsigned char servo_id); /** * \brief Function to check the current servo mode of a given servo * @@ -435,7 +466,7 @@ uint8_t set_joint_mode(uint8_t servo_id); * \return 0 if the servo is in joint mode, 0 if the servo is in wheel mode and * 0xFF in case of any error. */ -uint8_t is_joint_mode(uint8_t servo_id); +unsigned char is_joint_mode(unsigned char servo_id); /** * \brief Function to enable the given servo * @@ -448,7 +479,7 @@ uint8_t is_joint_mode(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t enable_servo(uint8_t servo_id); +unsigned char enable_servo(unsigned char servo_id); /** * \brief function to disable the given servo * @@ -461,7 +492,7 @@ uint8_t enable_servo(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t disable_servo(uint8_t servo_id); +unsigned char disable_servo(unsigned char servo_id); /** * \brief function to check wether the servo is enabled or not * @@ -474,7 +505,7 @@ uint8_t disable_servo(uint8_t servo_id); * \return 0 if the servo is disabled, 1 if the servo is enabled and 0xFF in * case of any error. */ -uint8_t is_servo_enabled(uint8_t servo_id); +unsigned char is_servo_enabled(unsigned char servo_id); /** * \brief Function to turn the servo led on * @@ -487,7 +518,7 @@ uint8_t is_servo_enabled(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t turn_servo_led_on(uint8_t servo_id); +unsigned char turn_servo_led_on(unsigned char servo_id); /** * \brief Function to turn the servo led off * @@ -500,7 +531,7 @@ uint8_t turn_servo_led_on(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t turn_servo_led_off(uint8_t servo_id); +unsigned char turn_servo_led_off(unsigned char servo_id); /** * \brief Function to check wether the servo led is on or not * @@ -513,7 +544,7 @@ uint8_t turn_servo_led_off(uint8_t servo_id); * \return 0 if the LED is turnwed off, 1 if the led is turned on and 0xFF in case * of any error. */ -uint8_t is_servo_led_on(uint8_t servo_id); +unsigned char is_servo_led_on(unsigned char servo_id); /** * \brief Function to get the current compliance margin of a given servo * @@ -535,7 +566,7 @@ uint8_t is_servo_led_on(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw); +unsigned char get_compliance_margin(unsigned char servo_id, unsigned char *cw, unsigned char *ccw); /** * \brief Function to set the desired compliance margin of a given servo * @@ -557,7 +588,7 @@ uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw); +unsigned char set_compliance_margin(unsigned char servo_id, unsigned char cw, unsigned char ccw); /** * \brief Function to get the current compliance slope of a given servo * @@ -589,7 +620,7 @@ uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw); +unsigned char get_compliance_slope(unsigned char servo_id, unsigned char *cw, unsigned char *ccw); /** * \brief Function to set the desired compliance slope of a given servo * @@ -621,19 +652,19 @@ uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw); +unsigned char set_compliance_slope(unsigned char servo_id, unsigned char cw, unsigned char ccw); /** * \brief Function to get the PID controller parameters of a given servo * * Not yet implemented */ -uint8_t get_pid_params(uint8_t servo_id,uint8_t *P,uint8_t *I, uint8_t *D); +unsigned char get_pid_params(unsigned char servo_id,unsigned char *P,unsigned char *I, unsigned char *D); /** * \brief Function to set the PID controller parameters of a given servo * * Not yet implemented */ -uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D); +unsigned char set_pid_params(unsigned char servo_id,unsigned char P,unsigned char I, unsigned char D); /** * \brief Function to get the current target position * @@ -652,7 +683,7 @@ uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D); * \return the current target position of the servo between 0 and the maximum encoder * resolution or 0xFFFF in case of any error. */ -uint16_t get_target_position(uint8_t servo_id); +unsigned short int get_target_position(unsigned char servo_id); /** * \brief Function to set the desired target position * @@ -673,7 +704,7 @@ uint16_t get_target_position(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_target_position(uint8_t serov_id, uint16_t position); +unsigned char set_target_position(unsigned char serov_id, unsigned short int position); /** * \brief Function to get the current target speed * @@ -691,7 +722,7 @@ uint8_t set_target_position(uint8_t serov_id, uint16_t position); * * \return the current speed command and 0xFFFF in cas of any error. */ -uint16_t get_target_speed(uint8_t servo_id); +unsigned short int get_target_speed(unsigned char servo_id); /** * \brief Function to set teh desired target speed * @@ -712,7 +743,7 @@ uint16_t get_target_speed(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_target_speed(uint8_t servo_id, uint16_t speed); +unsigned char set_target_speed(unsigned char servo_id, unsigned short int speed); /** * \brief Function to set the maximum torque for a given servo * @@ -738,7 +769,7 @@ uint8_t set_target_speed(uint8_t servo_id, uint16_t speed); * * \return the maximum torque allowed or 0xFFFF in case of any error. */ -uint16_t get_torque_limit(uint8_t servo_id); +unsigned short int get_torque_limit(unsigned char servo_id); /** * \brief Function to set the maximum torque for a given servo * @@ -760,7 +791,7 @@ uint16_t get_torque_limit(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque); +unsigned char set_torque_limit(unsigned char servo_id, unsigned short int torque); /** * \brief Function to return the current position of the given servo * @@ -774,7 +805,7 @@ uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque); * * \return the current servo position and 0xFFFF in case of any error. */ -uint16_t get_current_position(uint8_t servo_id); +unsigned short int get_current_position(unsigned char servo_id); /** * \brief Function to return the current speed of the given servo * @@ -792,7 +823,7 @@ uint16_t get_current_position(uint8_t servo_id); * * \return the current servo speed and 0xFFFF in case of any error. */ -uint16_t get_current_speed(uint8_t servo_id); +unsigned short int get_current_speed(unsigned char servo_id); /** * \brief Function to return the current load of the given servo * @@ -806,7 +837,7 @@ uint16_t get_current_speed(uint8_t servo_id); * * \return the current torque ratio and 0xFFFF in case of any error. */ -uint16_t get_current_load(uint8_t servo_id); +unsigned short int get_current_load(unsigned char servo_id); /** * \brief Function to get the current supply voltage of a given servo * @@ -824,7 +855,7 @@ uint16_t get_current_load(uint8_t servo_id); * * \return the current supply voltage and 0xFF in case of any error. */ -uint8_t get_current_voltage(uint8_t servo_id); +unsigned char get_current_voltage(unsigned char servo_id); /** * \brief Function to get the current temperature of a given servo * @@ -842,7 +873,7 @@ uint8_t get_current_voltage(uint8_t servo_id); * * \return the current temperature and 0xFF in case of any error. */ -uint8_t get_current_temperature(uint8_t servo_id); +unsigned char get_current_temperature(unsigned char servo_id); /** * \brief Function to check whether the servo is moving or not * @@ -856,7 +887,7 @@ uint8_t get_current_temperature(uint8_t servo_id); * \return 0 if the servo is stopped, 1 if the servo is moving and 0xFF in * case of any error. */ -uint8_t is_moving(uint8_t servo_id); +unsigned char is_moving(unsigned char servo_id); /** * \brief Function to check whether the servo is locked or not * @@ -871,7 +902,7 @@ uint8_t is_moving(uint8_t servo_id); * \return 0 if the servo is unlocked, 1 if the servo is locked and 0xFF in * case of any error. */ -uint8_t is_locked(uint8_t servo_id); +unsigned char is_locked(unsigned char servo_id); /** * \brief Function to lock the servo * @@ -885,7 +916,7 @@ uint8_t is_locked(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t lock(uint8_t servo_id); +unsigned char lock(unsigned char servo_id); /** * \brief Function to get the current punch value of a given servo. * @@ -903,7 +934,7 @@ uint8_t lock(uint8_t servo_id); * * \return the current punch value and 0xFFFF in case of any error. */ -uint16_t get_punch(uint8_t servo_id); +unsigned short int get_punch(unsigned char servo_id); /** * \brief Function to set the desired punch value of a given servo. * @@ -923,6 +954,6 @@ uint16_t get_punch(uint8_t servo_id); * * \return 0 if the function completes correctly, -1 otherwise */ -uint8_t set_punch(uint8_t servo_id, uint16_t punch); +unsigned char set_punch(unsigned char servo_id, unsigned short int punch); #endif diff --git a/dyn_devices/src/dyn_servos/dyn_servos.c b/dyn_devices/src/dyn_servos/dyn_servos.c index abb37e94e2b2f867c60b9d1a58d7a713c430524c..6c368002e222f7cf9a8544462706286c4f75d7b2 100755 --- a/dyn_devices/src/dyn_servos/dyn_servos.c +++ b/dyn_devices/src/dyn_servos/dyn_servos.c @@ -15,63 +15,92 @@ * along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>. */ -#include <avr/io.h> -#include <avr/interrupt.h> #include "dyn_servos_reg.h" #include "dyn_servos.h" -#include "dynamixel_master.h" +#include "dynamixel.h" -uint16_t get_model_number(uint8_t servo_id) + unsigned char dxl_sync_tx_packet(unsigned char num_servos,unsigned char *servo_ids,unsigned char start_addr, unsigned char *data, int length) + { + unsigned char i,j,data_ptr=0; + + dxl_set_txpacket_id(BROADCAST_ID); + dxl_set_txpacket_instruction(INST_SYNC_WRITE); + dxl_set_txpacket_parameter(0, start_addr); + dxl_set_txpacket_parameter(1, length); + for( i=0; i<num_servos; i++ ) + { + dxl_set_txpacket_parameter(2+(length+1)*i, servo_ids[i]); + for( j=0; j<length ;j++) + { + dxl_set_txpacket_parameter(2+(length+1)*i+1+j,data[data_ptr]); + data_ptr++; + } + } + dxl_set_txpacket_length((length+1)*num_servos+4); + + dxl_txrx_packet(); + + return dxl_get_result(); + } + +unsigned short int get_model_number(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,MODEL_NUM,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,MODEL_NUM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t get_firmware_version(uint8_t servo_id) +unsigned char get_firmware_version(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,FIRMWARE_VER,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,FIRMWARE_VER); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t set_servo_id(uint8_t old_id, uint8_t new_id) +unsigned char set_servo_id(unsigned char old_id, unsigned char new_id) { - if(dyn_master_write_byte(old_id,ID,new_id)==DYN_SUCCESS) + dxl_write_byte(old_id,ID,new_id); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t get_baudrate(uint8_t servo_id) +unsigned char get_baudrate(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,BAUD_RATE,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,BAUD_RATE); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t set_baudrate(uint8_t servo_id, uint8_t baudrate) +unsigned char set_baudrate(unsigned char servo_id, unsigned char baudrate) { - if(dyn_master_write_byte(servo_id,BAUD_RATE,baudrate)==DYN_SUCCESS) + dxl_write_byte(servo_id,BAUD_RATE,baudrate); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw) +unsigned char get_angle_limits(unsigned char servo_id, unsigned short int *cw, unsigned short int *ccw) { - if(dyn_master_read_word(servo_id,CW_ANGLE_LIM,cw)==DYN_SUCCESS) + *cw=dxl_read_word(servo_id,CW_ANGLE_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_read_word(servo_id,CCW_ANGLE_LIM,ccw)==DYN_SUCCESS) + *ccw=dxl_read_word(servo_id,CCW_ANGLE_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -80,11 +109,13 @@ uint8_t get_angle_limits(uint8_t servo_id, uint16_t *cw, uint16_t *ccw) return 0xFF;//error } -uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw) +unsigned char set_angle_limits(unsigned char servo_id, unsigned short int cw, unsigned short int ccw) { - if(dyn_master_write_word(servo_id,CW_ANGLE_LIM,cw)==DYN_SUCCESS) + dxl_write_word(servo_id,CW_ANGLE_LIM,cw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_write_word(servo_id,CCW_ANGLE_LIM,ccw)==DYN_SUCCESS) + dxl_write_word(servo_id,CCW_ANGLE_LIM,ccw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -93,29 +124,33 @@ uint8_t set_angle_limits(uint8_t servo_id, uint16_t cw, uint16_t ccw) return 0xFF;//error } -uint8_t get_temperature_limit(uint8_t servo_id) +unsigned char get_temperature_limit(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,TEMP_LIM,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,TEMP_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t set_temperature_limit(uint8_t servo_id, uint8_t temp) +unsigned char set_temperature_limit(unsigned char servo_id, unsigned char temp) { - if(dyn_master_write_byte(servo_id,TEMP_LIM,temp)==DYN_SUCCESS) + dxl_write_byte(servo_id,TEMP_LIM,temp); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high) +unsigned char get_voltage_limits(unsigned char servo_id, unsigned char *low, unsigned char *high) { - if(dyn_master_read_byte(servo_id,LOW_VOLTAGE_LIM,low)==DYN_SUCCESS) + *low=dxl_read_byte(servo_id,LOW_VOLTAGE_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_read_byte(servo_id,HIGH_VOLTAGE_LIM,high)==DYN_SUCCESS) + *high=dxl_read_byte(servo_id,HIGH_VOLTAGE_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -124,11 +159,13 @@ uint8_t get_voltage_limits(uint8_t servo_id, uint8_t *low, uint8_t *high) return 0xFF;//error } -uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high) +unsigned char set_voltage_limits(unsigned char servo_id, unsigned char low, unsigned char high) { - if(dyn_master_write_byte(servo_id,LOW_VOLTAGE_LIM,low)==DYN_SUCCESS) + dxl_write_byte(servo_id,LOW_VOLTAGE_LIM,low); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_write_byte(servo_id,HIGH_VOLTAGE_LIM,high)==DYN_SUCCESS) + dxl_write_word(servo_id,HIGH_VOLTAGE_LIM,high); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -137,73 +174,79 @@ uint8_t set_voltage_limits(uint8_t servo_id, uint8_t low, uint8_t high) return 0xFF;//error } -uint16_t get_max_torque(uint8_t servo_id) +unsigned short int get_max_torque(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,MAX_TORQUE,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,MAX_TORQUE); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t set_max_torque(uint8_t servo_id, uint16_t torque) +unsigned char set_max_torque(unsigned char servo_id, unsigned short int torque) { - if(dyn_master_write_word(servo_id,MAX_TORQUE,torque)==DYN_SUCCESS) + dxl_write_word(servo_id,MAX_TORQUE,torque); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t get_led_alarm(uint8_t servo_id) +unsigned char get_led_alarm(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,LED_ALARM,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,LED_ALARM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t set_led_alarm(uint8_t servo_id, uint8_t alarms) +unsigned char set_led_alarm(unsigned char servo_id, unsigned char alarms) { - if(dyn_master_write_byte(servo_id,LED_ALARM,alarms)==DYN_SUCCESS) + dxl_write_byte(servo_id,LED_ALARM,alarms); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t get_shd_alarm(uint8_t servo_id) +unsigned char get_shd_alarm(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,SHD_ALARM,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,SHD_ALARM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t set_shd_alarm(uint8_t servo_id, uint8_t alarms) +unsigned char set_shd_alarm(unsigned char servo_id, unsigned char alarms) { - if(dyn_master_write_byte(servo_id,SHD_ALARM,alarms)==DYN_SUCCESS) + dxl_write_byte(servo_id,SHD_ALARM,alarms); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t set_wheel_mode(uint8_t servo_id) +unsigned char set_wheel_mode(unsigned char servo_id) { return set_angle_limits(servo_id,0,0); } -uint8_t set_joint_mode(uint8_t servo_id) +unsigned char set_joint_mode(unsigned char servo_id) { return set_angle_limits(servo_id,1024,0); } -uint8_t is_joint_mode(uint8_t servo_id) +unsigned char is_joint_mode(unsigned char servo_id) { - uint16_t cw,ccw; + unsigned short int cw,ccw; if(get_angle_limits(servo_id,&cw,&ccw)==0xFF) return 0xFF; @@ -216,63 +259,71 @@ uint8_t is_joint_mode(uint8_t servo_id) } } -uint8_t enable_servo(uint8_t servo_id) +unsigned char enable_servo(unsigned char servo_id) { - if(dyn_master_write_byte(servo_id,TORQUE_EN,0x01)==DYN_SUCCESS) + dxl_write_byte(servo_id,TORQUE_EN,0x01); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t disable_servo(uint8_t servo_id) +unsigned char disable_servo(unsigned char servo_id) { - if(dyn_master_write_byte(servo_id,TORQUE_EN,0x00)==DYN_SUCCESS) + dxl_write_byte(servo_id,TORQUE_EN,0x00); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t is_servo_enabled(uint8_t servo_id) +unsigned char is_servo_enabled(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,TORQUE_EN,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,TORQUE_EN); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t turn_servo_led_on(uint8_t servo_id) +unsigned char turn_servo_led_on(unsigned char servo_id) { - if(dyn_master_write_byte(servo_id,LED,0x01)==DYN_SUCCESS) + dxl_write_byte(servo_id,LED,0x01); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t turn_servo_led_off(uint8_t servo_id) +unsigned char turn_servo_led_off(unsigned char servo_id) { - if(dyn_master_write_byte(servo_id,LED,0x00)==DYN_SUCCESS) + dxl_write_byte(servo_id,LED,0x00); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint8_t is_servo_led_on(uint8_t servo_id) +unsigned char is_servo_led_on(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,LED,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,LED); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw) +unsigned char get_compliance_margin(unsigned char servo_id, unsigned char *cw, unsigned char *ccw) { - if(dyn_master_read_byte(servo_id,CW_COMP_MARGIN,cw)==DYN_SUCCESS) + *cw=dxl_read_byte(servo_id,CW_COMP_MARGIN); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_read_byte(servo_id,CCW_COMP_MARGIN,ccw)==DYN_SUCCESS) + *ccw=dxl_read_byte(servo_id,CCW_COMP_MARGIN); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -281,11 +332,13 @@ uint8_t get_compliance_margin(uint8_t servo_id, uint8_t *cw, uint8_t *ccw) return 0xFF;//error } -uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw) +unsigned char set_compliance_margin(unsigned char servo_id, unsigned char cw, unsigned char ccw) { - if(dyn_master_write_byte(servo_id,CW_COMP_MARGIN,cw)==DYN_SUCCESS) + dxl_write_byte(servo_id,CW_COMP_MARGIN,cw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_write_byte(servo_id,CCW_COMP_MARGIN,ccw)==DYN_SUCCESS) + dxl_write_word(servo_id,CCW_COMP_MARGIN,ccw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -294,11 +347,13 @@ uint8_t set_compliance_margin(uint8_t servo_id, uint8_t cw, uint8_t ccw) return 0xFF;//error } -uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw) +unsigned char get_compliance_slope(unsigned char servo_id, unsigned char *cw, unsigned char *ccw) { - if(dyn_master_read_byte(servo_id,CW_COMP_SLOPE,cw)==DYN_SUCCESS) + *cw=dxl_read_byte(servo_id,CW_COMP_SLOPE); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_read_byte(servo_id,CCW_COMP_SLOPE,ccw)==DYN_SUCCESS) + *ccw=dxl_read_byte(servo_id,CCW_COMP_SLOPE); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -307,11 +362,13 @@ uint8_t get_compliance_slope(uint8_t servo_id, uint8_t *cw, uint8_t *ccw) return 0xFF;//error } -uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw) +unsigned char set_compliance_slope(unsigned char servo_id, unsigned char cw, unsigned char ccw) { - if(dyn_master_write_byte(servo_id,CW_COMP_SLOPE,cw)==DYN_SUCCESS) + dxl_write_byte(servo_id,CW_COMP_SLOPE,cw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) { - if(dyn_master_write_byte(servo_id,CCW_COMP_SLOPE,ccw)==DYN_SUCCESS) + dxl_write_word(servo_id,CCW_COMP_SLOPE,ccw); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF; @@ -320,161 +377,177 @@ uint8_t set_compliance_slope(uint8_t servo_id, uint8_t cw, uint8_t ccw) return 0xFF;//error } -uint8_t get_pid_params(uint8_t servo_id,uint8_t *P,uint8_t *I, uint8_t *D) +unsigned char get_pid_params(unsigned char servo_id,unsigned char *P,unsigned char *I, unsigned char *D) { return 0xFF; } -uint8_t set_pid_params(uint8_t servo_id,uint8_t P,uint8_t I, uint8_t D) +unsigned char set_pid_params(unsigned char servo_id,unsigned char P,unsigned char I, unsigned char D) { return 0xFF; } -uint16_t get_target_position(uint8_t servo_id) +unsigned short int get_target_position(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,GOAL_POS,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,GOAL_POS); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t set_target_position(uint8_t servo_id, uint16_t position) +unsigned char set_target_position(unsigned char servo_id, unsigned short int position) { - if(dyn_master_write_word(servo_id,GOAL_POS,position)==DYN_SUCCESS) + dxl_write_word(servo_id,GOAL_POS,position); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint16_t get_target_speed(uint8_t servo_id) +unsigned short int get_target_speed(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,GOAL_SPEED,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,GOAL_SPEED); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t set_target_speed(uint8_t servo_id, uint16_t speed) +unsigned char set_target_speed(unsigned char servo_id, unsigned short int speed) { - if(dyn_master_write_word(servo_id,GOAL_SPEED,speed)==DYN_SUCCESS) + dxl_write_word(servo_id,GOAL_SPEED,speed); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint16_t get_torque_limit(uint8_t servo_id) +unsigned short int get_torque_limit(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,TORQUE_LIM,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,TORQUE_LIM); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t set_torque_limit(uint8_t servo_id, uint16_t torque) +unsigned char set_torque_limit(unsigned char servo_id, unsigned short int torque) { - if(dyn_master_write_word(servo_id,TORQUE_LIM,torque)==DYN_SUCCESS) + dxl_write_word(servo_id,TORQUE_LIM,torque); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint16_t get_current_position(uint8_t servo_id) +unsigned short int get_current_position(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,CURRENT_POS,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,CURRENT_POS); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint16_t get_current_speed(uint8_t servo_id) +unsigned short int get_current_speed(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,CURRENT_SPEED,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,CURRENT_SPEED); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint16_t get_current_load(uint8_t servo_id) +unsigned short int get_current_load(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,CURRENT_LOAD,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,CURRENT_LOAD); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t get_current_voltage(uint8_t servo_id) +unsigned char get_current_voltage(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,CURRENT_VOLTAGE,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,CURRENT_VOLTAGE); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t get_current_temperature(uint8_t servo_id) +unsigned char get_current_temperature(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,CURRENT_TEMP,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,CURRENT_TEMP); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t is_moving(uint8_t servo_id) +unsigned char is_moving(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,MOVING,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,MOVING); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t is_locked(uint8_t servo_id) +unsigned char is_locked(unsigned char servo_id) { - uint8_t value; + unsigned char value; - if(dyn_master_read_byte(servo_id,LOCK,&value)==DYN_SUCCESS) + value=dxl_read_byte(servo_id,LOCK); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFF;//error } -uint8_t lock(uint8_t servo_id) +unsigned char lock(unsigned char servo_id) { - if(dyn_master_write_byte(servo_id,LOCK,0x01)==DYN_SUCCESS) + dxl_write_byte(servo_id,LOCK,0x01); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error } -uint16_t get_punch(uint8_t servo_id) +unsigned short int get_punch(unsigned char servo_id) { - uint16_t value; + unsigned short int value; - if(dyn_master_read_word(servo_id,PUNCH,&value)==DYN_SUCCESS) + value=dxl_read_word(servo_id,PUNCH); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return value;// the actual value else return 0xFFFF;//error } -uint8_t set_punch(uint8_t servo_id, uint16_t punch) +unsigned char set_punch(unsigned char servo_id, unsigned short int punch) { - if(dyn_master_write_word(servo_id,PUNCH,punch)==DYN_SUCCESS) + dxl_write_word(servo_id,PUNCH,punch); + if(dxl_get_result()==COMM_TXSUCCESS || dxl_get_result()==COMM_RXSUCCESS) return 0x00;// the actual value else return 0xFF;//error diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile deleted file mode 100755 index e47489236aa81efc8ff2c3bd7ba6d599ae7006d5..0000000000000000000000000000000000000000 --- a/dyn_devices/src/examples/Makefile +++ /dev/null @@ -1,47 +0,0 @@ -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: communications dyn_devices $(PROJECT).hex - -$(PROJECT).hex: $(PROJECT).elf - $(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS) $< $@ -$(PROJECT).elf: $(OBJS) $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a - $(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf -%.o:%.c - $(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $< - -communications: - $(MAKE) -C $(COMM_DIR) - -dyn_devices: - $(MAKE) -C $(DEV_DIR) - -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 f9ad49f2833c93b6444ef382640f07026af36b93..71c5f61d8d6e5c3b53c01e5e1ca8edaccad61ef3 100755 --- a/dyn_devices/src/examples/main.c +++ b/dyn_devices/src/examples/main.c @@ -15,42 +15,22 @@ * along with iri-libbioloid. If not, see <http://www.gnu.org/licenses/>. */ -#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); -} +#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); +} diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c index af73baf40bd2f1baf60df456187de07af573fbb6..d11525e49666758c5ec893b10c389c9c2d1d3d6f 100755 --- a/dyn_devices/src/exp_board/exp_board.c +++ b/dyn_devices/src/exp_board/exp_board.c @@ -19,16 +19,8 @@ #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 a6372baf5b0addcfbf95247cdd79507d8e64509a..4aca4c28a131916f4846509f591471e03b54b161 100755 --- a/motion/Makefile +++ b/motion/Makefile @@ -1,27 +1,54 @@ +ROBOTIS_C_DIR = ../robotis_lib + +ifdef ROBOTIS_C_DIR + ROBOTIS_C_DIR_FOUND=true +else + ROBOTIS_C_DIR_FOUND=false +endif + PROJECT=libmotion_manager -SOURCES=src/motion_manager.c src/action.c src/motion_pages.c src/balance.c +SOURCES=src/motion_manager.c src/action.c src/motion_pages.c OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -BIN_DIR=./build/ -LIB_DIR=./lib/ -COMM_DIR=../communications/ -DEV_DIR=../dyn_devices/ -CONT_DIR=../controllers/ +SDIR=./src/ +IDIR=./include/ +BDIR=./build/ +LDIR=./lib/ CC=avr-gcc -OBJCOPY=avr-ar +OBJCOPY=avr-ar rsc MMCU=atmega2561 -INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/ -I$(CONT_DIR)include/ +ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/ -LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a +SERVOS_INCLUDE_DIR=../dyn_devices/include/ + +CONTROLLER_INCLUDE_DIR=../controllers/include/ CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes -ARFLAGS= rsc +.PHONY: all check_robotis_c show_banner clean + +all: check_robotis_c show_banner $(PROJECT).a -.PHONY: all show_banner clean +check_robotis_c: + @if ! $(ROBOTIS_C_DIR_FOUND); then \ + echo " IMPORTANT !!! "; \ + echo " ROBOTIS_C_DIR variable is not set! The software won't compile"; \ + echo " unless you specify the path to Robotis embedded C software."; \ + echo ""; \ + echo " You should:"; \ + echo " 1. Edit the makefile, at the begging set up ROBOTIS_C_DIR"; \ + echo " 2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \ + echo ""; \ + echo " Beware of:"; \ + echo " 1. Do not use '(' in the directory name (as it comes from robotis) "; \ + echo " 2. Absolute path is preferred to avoid problems with submake calls"; \ + exit 2; \ + fi -all: show_banner communications dyn_devices $(PROJECT).a + @if [ ! -d $(ROBOTIS_C_DIR) ]; then \ + echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it."; \ + exit 2; \ + fi show_banner: @echo "------------------------------------------------------"; @@ -37,21 +64,11 @@ show_banner: $(PROJECT).a: ${OBJS} - $(OBJCOPY) $(ARFLAGS) ${LIB_DIR}$(PROJECT).a $(OBJS) - -communications: - $(MAKE) -C $(COMM_DIR) - -dyn_devices: - $(MAKE) -C $(DEV_DIR) - -examples: - $(MAKE) -C src/examples + $(OBJCOPY) ${LDIR}$(PROJECT).a $(OBJS) %.o: %.c - $(CC) -c $(CFLAGS) ${INC_DIRS} -o $@ $< + $(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $< clean: - rm -f ${LIB_DIR}$(PROJECT).a + rm -f ${LDIR}$(PROJECT).a rm -f $(OBJS) - $(MAKE) -C src/examples clean diff --git a/motion/include/action.h b/motion/include/action.h index 929ad3fa569e85a042af698bd77a7e1e168f24ec..549728d6745d48b1e7eaeb32fab03d9045a59000 100644 --- a/motion/include/action.h +++ b/motion/include/action.h @@ -1,10 +1,18 @@ #ifndef _ACTION_H #define _ACTION_H +#include <inttypes.h> + +#define NUM_SERVOS 18 + // basic motion interface +void action_init(void); uint8_t action_set_page(uint8_t page_id); void action_start_page(void); void action_stop_page(void); uint8_t is_action_running(void); +// motion manager interface +void action_process(void); + #endif diff --git a/motion/include/balance.h b/motion/include/balance.h deleted file mode 100644 index 5c227eb00a2be42b38b8fcdd38541ed20935aced..0000000000000000000000000000000000000000 --- a/motion/include/balance.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef _BALANCE_H -#define _BALANCE_H - -typedef enum {robot_standing,robot_face_down,robot_face_up} fallen_t; - -// public functions -void balance_init(void); -/** - * \brief Function to check if the robot has fallen to the ground - * - * This function returns: - * 0x00 : if the robot is standing. - * 0x01 : if the robot has fallen face-to-ground - * 0x02 : if the robot has fallen back-to-ground - * After reading the value, the internal state is automatically set - * back to 0x00 - */ -fallen_t balance_robot_has_fallen(void); -uint8_t balance_calibrate_gyro(void); -/** - * \brief Function to enable the internal gyros - * - * If the gyroscopes have been detected and calibrated in the init_motion() function, - * this function enbales them to be used. When enabled they perform two tasks: first - * balance the robot while moving to make it more stable and also detect when the - * robot falls. - */ -void balance_enable_gyro(void); -/** - * \brief Function to disable the internal gyros - * - * If the gyroscopes have been detected and calibrated in the init_motion() function, - * this function disales them. When disabled it is no longer possible to detect when - * robot falls. gyro - * - * This function may be used to avoid cancelling the standing up action due to fast - * motions associated with this motion. After the robot is standing up, the gyros may - * be enabled again. - */ -void balance_disable_gyro(void); -/** - * \brief Function to check whether the gyros are enabled or not - * - * This function checks wether the gyros are currently enabled or not. - * - * \return 0x01 if the gyros are enabled and 0x00 otherwise. - */ -uint8_t balance_is_gyro_enabled(void); - -#endif diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h deleted file mode 100644 index f96994c4cfcd0624ab272636e478ffeceefb09dd..0000000000000000000000000000000000000000 --- a/motion/include/motion_cfg.h +++ /dev/null @@ -1,74 +0,0 @@ -#ifndef _MOTION_CFG_H -#define _MOTION_CFG_H - -#include "buzzer.h" - -#ifndef MANAGER_MAX_NUM_SERVOS - #define MANAGER_MAX_NUM_SERVOS 18 -#endif - -#ifndef MANAGER_MISSING_SERVOS_ALARM_NOTE - #define MANAGER_MISSING_SERVOS_ALARM_NOTE NOTE_SOL -#endif - -#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_ON - #define MANAGER_MISSING_SERVOS_ALARM_TIME_ON 10 -#endif - -#ifndef MANAGER_MISSING_SERVOS_ALARM_TIME_OFF - #define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF 100 -#endif - -#ifndef BALANCE_GYRO_CAL_NUM_SAMPLES - #define BALANCE_GYRO_CAL_NUM_SAMPLES 10 -#endif - -#ifndef BALANCE_GYRO_X_CHANNHEL - #define BALANCE_GYRO_X_CHANNEL 3 -#endif - -#ifndef BALANCE_GYRO_Y_CHANNEL - #define BALANCE_GYRO_Y_CHANNEL 4 -#endif - -#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_NOTE - #define BALANCE_GYRO_CAL_FAILED_ALARM_NOTE NOTE_SOL -#endif - -#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON - #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON 10 -#endif - -#ifndef BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF - #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF 100 -#endif - -#ifndef BALANCE_MAX_CAL_GYRO_ERROR - #define BALANCE_MAX_CAL_GYRO_ERROR 20.0 -#endif - -#ifndef BALANCE_FORWARD_FALL_THD_VALUE - #define BALANCE_FORWARD_FALL_THD_VALUE (-200) -#endif - -#ifndef BALANCE_BACKWARD_FALL_THD_VALUE - #define BALANCE_BACKWARD_FALL_THD_VALUE 200 -#endif - -#ifndef BALANCE_KNEE_GAIN - #define BALANCE_KNEE_GAIN (4.0/54.0) -#endif - -#ifndef BALANCE_ANKLE_ROLL_GAIN - #define BALANCE_ANKLE_ROLL_GAIN (4.0/18.0) -#endif - -#ifndef BALANCE_HIP_PITCH_GAIN - #define BALANCE_HIP_PITCH_GAIN (4.0/20.0) -#endif - -#ifndef BALANCE_ANKLE_PITCH_GAIN - #define BALANCE_ANKLE_PITCH_GAIN (4.0/40.0) -#endif - -#endif diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h index 29b0ee61b9c8905828b823f6c75135ba69be8682..1745d0855f444a847a2219c003100890aee2ed97 100644 --- a/motion/include/motion_manager.h +++ b/motion/include/motion_manager.h @@ -2,19 +2,109 @@ #define _MOTION_MANAGER_H #include <inttypes.h> +#include "motion_pages.h" + +#define MANAGER_MAX_NUM_SERVOS 20 +#define GYRO_CAL_NUM_SAMPLES 10 +#define X_GYRO_CHANNEL 3 +#define Y_GYRO_CHANNEL 4 + +extern TPage action_next_page; +extern TPage action_current_page; // servo information structure typedef struct{ uint8_t id; + uint16_t encoder_resolution; + uint8_t gear_ratio; uint16_t max_value; uint16_t min_value; uint16_t center_value; + uint16_t center_angle; + uint16_t max_speed; uint16_t current_value; + int16_t max_angle; + int16_t min_angle; + int16_t current_angle; + uint8_t module; uint8_t cc_slope; uint8_t ccw_slope; }TServoInfo; +int16_t servo_offsets[MANAGER_MAX_NUM_SERVOS]; + // public functions -uint8_t manager_init(uint8_t num_servos); +void manager_init(unsigned char 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); +uint16_t manager_get_servo_value(uint8_t servo_id); + +void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope); + +void manager_set_servo_value(uint8_t servo_id, uint16_t value); +void manager_set_index_value(uint8_t servo_id, uint16_t value); +uint16_t manager_get_servo_value(uint8_t servo_id); +uint16_t manager_get_index_value(uint8_t servo_id); + +/** + * \brief Function to check if the robot has fallen to the ground + * + * This function returns: + * 0x00 : if the robot is standing. + * 0x01 : if the robot has fallen face-to-ground + * 0x02 : if the robot has fallen back-to-ground + * After reading the value, the internal state is automatically set + * back to 0x00 + */ +unsigned char manager_check_robot_fallen(void); +// gyro interface +unsigned char manager_calibrate_gyro(void); +/** + * \brief Function to enable the internal gyros + * + * If the gyroscopes have been detected and calibrated in the init_motion() function, + * this function enbales them to be used. When enabled they perform two tasks: first + * balance the robot while moving to make it more stable and also detect when the + * robot falls. + */ +void manager_enable_gyro(void); +/** + * \brief Function to disable the internal gyros + * + * If the gyroscopes have been detected and calibrated in the init_motion() function, + * this function disales them. When disabled it is no longer possible to detect when + * robot falls. gyro + * + * This function may be used to avoid cancelling the standing up action due to fast + * motions associated with this motion. After the robot is standing up, the gyros may + * be enabled again. + */ +void manager_disable_gyro(void); +/** + * \brief Function to check whether the gyros are enabled or not + * + * This function checks wether the gyros are currently enabled or not. + * + * \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. + */ #endif diff --git a/motion/src/action.c b/motion/src/action.c index 300aa966c768d52e739d604dd6e8fdf6d2af55b0..4cd742e9d92ddf8dd892d85b7dff595a37388967 100644 --- a/motion/src/action.c +++ b/motion/src/action.c @@ -1,11 +1,12 @@ +#include "action.h" +#include "dyn_servos.h" +#include "motion_pages.h" +#include "motion_manager.h" #include <avr/io.h> #include <avr/interrupt.h> #include <util/delay.h> -#include "motion_cfg.h" -#include "action.h" -#include "motion_pages.h" -#include "serial_console.h" -#include <stdio.h> +#include "cm510.h" + /************************************** * Section /----\ @@ -22,12 +23,6 @@ typedef enum{ ZERO_FINISH, NONE_ZERO_FINISH} TStopMode; #define TIME_BASE_SCHEDULE 0x0a #define INVALID_BIT_MASK 0x4000 -// external functions -extern uint16_t manager_get_index_value(uint8_t servo_id); -extern uint16_t manager_get_servo_value(uint8_t servo_id); -extern void manager_set_servo_value(uint8_t servo_id, uint16_t value); -extern void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope); - // private variables uint8_t action_finished; uint8_t action_stop; @@ -76,7 +71,10 @@ uint8_t action_set_page(uint8_t page_id) error=load_page_info(page_id,&action_current_page); if(error==0) + { + //ram_write_byte(CM730_ACTION_NUMBER,page_id); action_current_index=page_id; + } return error; } @@ -92,20 +90,26 @@ void action_start_page(void) action_step_count = 0; bPlayRepeatCount = action_current_page.header.repeat; action_next_index = 0x00; + - for( i=1; i<=MANAGER_MAX_NUM_SERVOS; i++ ) + for( i=1; i<=NUM_SERVOS; i++ ) { - wpTargetAngle1024[i] = manager_get_index_value(i); - ipLastOutSpeed1024[i] = 0; - ipMovingAngle1024[i] = 0; - ipGoalSpeed1024[i] = 0; + wpTargetAngle1024[i] = manager_get_index_value(i); + ipLastOutSpeed1024[i] = 0; + ipMovingAngle1024[i] = 0; + ipGoalSpeed1024[i] = 0; } action_is_running=0x01; + //ram_write_byte(CM730_ACTION_STATUS,0x01); + //ram_set_bit(CM730_ACTION_CONTROL,0x00); + //ram_clear_bit(CM730_ACTION_CONTROL,0x01); } void action_stop_page(void) { action_stop=0x01; + //ram_clear_bit(CM730_ACTION_CONTROL,0x00); + //ram_set_bit(CM730_ACTION_CONTROL,0x01); } uint8_t is_action_running(void) @@ -128,6 +132,7 @@ void action_process(void) int64_t lStartSpeed1024_PreTime_256T; int64_t lMovingAngle_Speed1024Scale_256T_2T; int64_t lDivider1,lDivider2; + uint16_t wMaxAngle1024; uint16_t wMaxSpeed256; uint16_t wPrevTargetAngle; // Start position uint16_t wCurrentTargetAngle; // Target position @@ -142,7 +147,7 @@ void action_process(void) { wUnitTimeCount = 0; - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { wpStartAngle1024[bID] = manager_get_servo_value(bID); ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID]; @@ -153,9 +158,9 @@ void action_process(void) { // MAIN Section Ãغñ bSection = MAIN_SECTION; -// turn_led_off(LED_TxD); + turn_led_off(LED_TxD); wUnitTimeNum = wUnitTimeTotalNum - (wAccelStep << 1); - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { if( bpFinishType[bID] == NONE_ZERO_FINISH ) { @@ -172,10 +177,10 @@ void action_process(void) { // POST Section Ãغñ bSection = POST_SECTION; -// turn_led_off(LED_TxD); + turn_led_off(LED_TxD); wUnitTimeNum = wAccelStep; - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID]; } @@ -186,22 +191,22 @@ void action_process(void) if( wPauseTime ) { bSection = PAUSE_SECTION; -// turn_led_off(LED_TxD); + turn_led_off(LED_TxD); wUnitTimeNum = wPauseTime; } else { bSection = PRE_SECTION; -// turn_led_on(LED_TxD); + turn_led_on(LED_TxD); } } else if( bSection == PAUSE_SECTION ) { // PRE Section Ãغñ bSection = PRE_SECTION; -// turn_led_on(LED_TxD); + turn_led_on(LED_TxD); - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { ipLastOutSpeed1024[bID] = 0; } @@ -268,7 +273,7 @@ void action_process(void) // wMaxAngle1024 = 0; ////////// Jointº° ÆÄ¶ó¹ÌÅà °è»ê - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÃÀ¸·Î ±ËÀûÀ» °è»ê ipAccelAngle1024[bID] = 0; @@ -361,7 +366,7 @@ void action_process(void) if(lDivider2 == 0) lDivider2 = 1; - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; // *300/1024 * 1024/720 * 256 * 2 lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12; @@ -384,11 +389,11 @@ void action_process(void) wUnitTimeCount++; if( bSection == PAUSE_SECTION ) { -// toggle_led(LED_AUX); + toggle_led(LED_AUX); } else { - for(bID=1;bID<=MANAGER_MAX_NUM_SERVOS;bID++) + for(bID=1;bID<=NUM_SERVOS;bID++) { if( ipMovingAngle1024[bID] == 0 ) manager_set_servo_value(bID,wpStartAngle1024[bID]); diff --git a/motion/src/balance.c b/motion/src/balance.c deleted file mode 100644 index d7461ef0fca2bfb56269050ca5a558a5045ec857..0000000000000000000000000000000000000000 --- a/motion/src/balance.c +++ /dev/null @@ -1,141 +0,0 @@ -#include <avr/io.h> -#include <avr/interrupt.h> -#include <util/delay.h> -#include "motion_cfg.h" -#include "balance.h" -#include "adc.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 -uint16_t balance_x_gyro_center; -uint16_t balance_y_gyro_center; -uint8_t balance_gyro_calibrated; -uint8_t balance_enabled; -uint8_t balance_robot_fallen_state; -int16_t balance_offsets[MANAGER_MAX_NUM_SERVOS]; - -// private functions -void balance_loop(void) -{ - int16_t gyro_x=0; - int16_t gyro_y=0; - - if(balance_enabled == 0x01 && balance_gyro_calibrated == 0x01) - { - // get x,y gyroscope - gyro_x=get_adc_channel(BALANCE_GYRO_X_CHANNEL)-balance_x_gyro_center; - gyro_y=get_adc_channel(BALANCE_GYRO_Y_CHANNEL)-balance_y_gyro_center; - - if(gyro_x > BALANCE_BACKWARD_FALL_THD_VALUE) - balance_robot_fallen_state = robot_face_up; - else if(gyro_x < BALANCE_FORWARD_FALL_THD_VALUE) - balance_robot_fallen_state = robot_face_down; - - float x_error1,x_error2,y_error1,y_error2; - x_error1=gyro_x*BALANCE_KNEE_GAIN;//4.0/54.0; - x_error2=gyro_x*BALANCE_ANKLE_ROLL_GAIN;//4.0/18.0; - y_error1=gyro_y*BALANCE_HIP_PITCH_GAIN;//4.0/20.0; - y_error2=gyro_y*BALANCE_ANKLE_PITCH_GAIN;//4.0/40.0; - - balance_offsets[8] = (uint16_t)y_error1; - balance_offsets[9] = (uint16_t)y_error1; - balance_offsets[12] = (uint16_t)x_error1;//-1.0; - balance_offsets[13] = (uint16_t)-x_error1;//+1.0; - balance_offsets[14] = (uint16_t)+x_error2; - balance_offsets[15] = (uint16_t)-x_error2; - balance_offsets[16] = (uint16_t)-y_error2; - balance_offsets[17] = (uint16_t)-y_error2; - } -} - -int16_t balance_get_offset(uint8_t index) -{ - return balance_offsets[index]; -} - -void balance_get_all_offsets(int16_t **offsets) -{ - *offsets=&balance_offsets[0]; -} - -// public functions -void balance_init(void) -{ - uint8_t i; - - balance_x_gyro_center=0; - balance_y_gyro_center=0; - balance_gyro_calibrated=0x00; - balance_enabled=0x00; - balance_robot_fallen_state=robot_standing; - for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) - balance_offsets[i]=0; -} - -uint8_t balance_robot_has_fallen(void) -{ - uint8_t state = balance_robot_fallen_state; - balance_robot_fallen_state = robot_standing; - return state; -} - -uint8_t balance_calibrate_gyro(void) -{ - uint16_t x_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES]; - uint16_t y_gyro_values[BALANCE_GYRO_CAL_NUM_SAMPLES]; - float x_gyro_average=0.0,y_gyro_average=0.0,std_x=0.0,std_y=0.0; - uint8_t i=0; - - balance_x_gyro_center=0; - balance_y_gyro_center=0; - for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++) - { - _delay_ms(50); - x_gyro_values[i]=get_adc_channel(BALANCE_GYRO_X_CHANNEL); - y_gyro_values[i]=get_adc_channel(BALANCE_GYRO_Y_CHANNEL); - x_gyro_average+=x_gyro_values[i]; - y_gyro_average+=y_gyro_values[i]; - } - x_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES; - y_gyro_average/=(float)BALANCE_GYRO_CAL_NUM_SAMPLES; - // compute the standard deviation - for(i=0;i<BALANCE_GYRO_CAL_NUM_SAMPLES;i++) - { - std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average); - std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average); - } - std_x=sqrt(std_x/BALANCE_GYRO_CAL_NUM_SAMPLES); - std_y=sqrt(std_y/BALANCE_GYRO_CAL_NUM_SAMPLES); - if(std_x<BALANCE_MAX_CAL_GYRO_ERROR && std_y<BALANCE_MAX_CAL_GYRO_ERROR) - { - balance_gyro_calibrated=0x01; - balance_x_gyro_center=(uint16_t)x_gyro_average; - balance_y_gyro_center=(uint16_t)y_gyro_average; - } - else - { - buzzer_start_alarm(BALANCE_GYRO_CAL_FAILED_ALARM_NOTE,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON,BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF); - return 0x00; - } - buzzer_stop_alarm(); - return 0x01; -} - -void balance_enable(void) -{ - balance_enabled=0x01; -} - -void balance_disable(void) -{ - balance_enabled=0x00; -} - -uint8_t balance_is_enabled(void) -{ - return balance_enabled; -} diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile index 6b50b49effa4336102f6b86be4556fea9f5fa6af..63e7191ae722f8bba643d837c64e842202220a4c 100644 --- a/motion/src/examples/Makefile +++ b/motion/src/examples/Makefile @@ -1,55 +1,80 @@ -PROJECT=manager_ex -######################################################## -# afegir tots els fitxers que s'han de compilar aquà -######################################################## -SOURCES=main.c +ROBOTIS_C_DIR =../../../robotis_lib +ifdef ROBOTIS_C_DIR + ROBOTIS_C_DIR_FOUND=true +else + ROBOTIS_C_DIR_FOUND=false +endif + +PROJECT=example +SOURCES=main.c OBJS=$(SOURCES:.c=.o) -SRC_DIR=./src/ -DEV_DIR=../../../dyn_devices/ -COMM_DIR=../../../communications/ -CONT_DIR=../../../controllers/ -MAN_DIR=../../ +SDIR=../../src/ +IDIR=../../include/ +BDIR=../../build/ +LDIR=../../lib/ CC=avr-gcc -OBJCOPY=avr-objcopy +OBJCOPY=avr-ar rsc MMCU=atmega2561 +CONTLIB=../../../controllers/lib/ +SERVLIB=../../../dyn_devices/lib/ +ROBOTISLIB=$(ROBOTIS_C_DIR)/lib/ -LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a +ROBOTIS_INCLUDE_DIR=$(ROBOTIS_C_DIR)/include/ -INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include +SERVOS_INCLUDE_DIR=../../../dyn_devices/include/ -CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes +CONTROLLER_INCLUDE_DIR=../../../controllers/include/ -LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL +CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes -HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature +.PHONY: all check_robotis_c show_banner clean -.PHONY: all +all: check_robotis_c show_banner $(PROJECT).elf $(PROJECT).hex -all: communications dyn_devices controllers motion_manager $(PROJECT).hex +check_robotis_c: + @if ! $(ROBOTIS_C_DIR_FOUND); then \ + echo " IMPORTANT !!! "; \ + echo " ROBOTIS_C_DIR variable is not set! The software won't compile"; \ + echo " unless you specify the path to Robotis embedded C software."; \ + echo ""; \ + echo " You should:"; \ + echo " 1. Edit the makefile, at the begging set up ROBOTIS_C_DIR"; \ + echo " 2. Use the variable with make: make ROBOTIS_C_DIR=path-to-bioloid-c"; \ + echo ""; \ + echo " Beware of:"; \ + echo " 1. Do not use '(' in the directory name (as it comes from robotis) "; \ + echo " 2. Absolute path is preferred to avoid problems with submake calls"; \ + exit 2; \ + fi -$(PROJECT).hex: $(PROJECT).elf - $(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS) $< $@ -$(PROJECT).elf: $(OBJS) - $(CC) $(LDFLAGS) $(OBJS) $(LIBS) -o $(PROJECT).elf -%.o:%.c - $(CC) -c $(CFLAGS) $(INCLUDE_DIRS) -o $@ $< + @if [ ! -d $(ROBOTIS_C_DIR) ]; then \ + echo " ! ROBOTIS_C_DIR is pointing to a non existant directory. Please fix it."; \ + exit 2; \ + fi -communications: - $(MAKE) -C $(COMM_DIR) +show_banner: + @echo "------------------------------------------------------"; + @echo " _____ "; + @echo " / _ \ "; + @echo " | |_| | The Humanoid Lab "; + @echo " ____\_____/____ "; + @echo " / \ http://apollo.upc.es/humanoide/ "; + @echo "/ _ _ \ "; + @echo "| | | | | | $(PROJECT) "; + @echo "| | | | | | "; + @echo "------------------------------------------------------"; -dyn_devices: - $(MAKE) -C $(DEV_DIR) -controllers: - $(MAKE) -C $(CONT_DIR) +$(PROJECT).elf: ${OBJS} + $(CC) -g -mmcu=$(MMCU) -o ${LDIR}$(PROJECT).elf $(OBJS) -L $(LDIR) -l motion_manager -L $(CONTLIB) -l controllers -L $(SERVLIB) -l dyn_devices -L $(ROBOTISLIB) -l dynamixel -L $(ROBOTISLIB) -l serial -motion_manager: - $(MAKE) -C $(MAN_DIR) +$(PROJECT).hex: $(PROJECT).elf + avr-objcopy -O ihex $(LDIR)$(PROJECT).elf $(PROJECT).hex -download: $(MAIN_OUT_HEX) - fw_downloader -d /dev/ttyUSB0 -f ./src/examples/comm_ex.hex -p new +%.o: %.c + $(CC) -c $(CFLAGS) -I$(IDIR) -I${ROBOTIS_INCLUDE_DIR} -I${SERVOS_INCLUDE_DIR} -I${CONTROLLER_INCLUDE_DIR} -o $@ $< clean: - -rm $(PROJECT).* - -rm $(OBJS) + rm -f ${LDIR}$(PROJECT).a + rm -f $(OBJS) diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c index 93e0ce421b3f7aa7d16e5c3292f48b1b75be7497..3a50003e54f89fc451dccd8f42ae6746c1a085a7 100644 --- a/motion/src/examples/main.c +++ b/motion/src/examples/main.c @@ -1,35 +1,65 @@ +#include "motion_manager.h" +#include "action.h" +#include "cm510.h" #include <util/delay.h> #include <stdio.h> -#include "cm510.h" +#include "exp_board.h" +#include "exp_board_reg.h" +#include "serial.h" -void user_init(void) -{ - serial_console_init(57600); -} -void user_loop(void) +int main(void) { - if(is_button_falling_edge(BTN_UP)) - { - if(is_action_running()==0x00) - { - printf("start action\n"); - if(action_set_page(3)==0) - action_start_page(); - else - printf("Error loading page\n"); - } + int n_servos = 18; + init_cm510(ASYNC); + + manager_init(n_servos); + + int exp_board_ad_4, exp_board_ad_5, gyro_x, gyro_y; + + //serial_initialize(57600); + exp_board_init(192); + + + action_init(); + action_set_page(1); + action_start_page(); + _delay_ms(2000); + + if(manager_calibrate_gyro()) + { + manager_enable_gyro(); + action_set_page(3); + action_start_page(); } - else + + exp_board_ad_4 = get_adc_avg_channel(ADC4); + while(exp_board_ad_4 < 400) { - if(is_button_falling_edge(BTN_DOWN)) - { - if(is_action_running()) - { - printf("stop action\n"); - action_stop_page(); - } - } + exp_board_ad_4 = get_adc_avg_channel(ADC4); + //printf("exp_board_ad_4: %d\n",exp_board_ad_4); + _delay_ms(50); } -} + //printf("stop"); + action_stop_page(); + //while(is_action_running()) + action_set_page(1); + action_start_page(); + + //_delay_ms(1000); + //action_set_page(32); + //action_start_page(); + + while(1) { + //exp_board_ad_4 = get_adc_avg_channel(ADC4); + //printf("exp_board_ad_4: %d\n",exp_board_ad_4); + + //exp_board_ad_5 = get_adc_avg_channel(ADC5); + //printf("exp_board_ad_5: %d\n\n",exp_board_ad_5); + //turn_led_on(LED_PROGRAM); + //action_process(); + //turn_led_off(LED_PROGRAM); + //_delay_ms(500); + } +} diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c index 71232414b946f5d6c18a1da303dcc6710a015733..e70eae49157e2130aa21efe1e8738f1b7481c761 100644 --- a/motion/src/motion_manager.c +++ b/motion/src/motion_manager.c @@ -1,106 +1,120 @@ +#include "motion_manager.h" #include <avr/io.h> #include <avr/interrupt.h> #include <util/delay.h> -#include "motion_cfg.h" -#include "motion_manager.h" -#include "motion_pages.h" -#include "dynamixel_master.h" +#include "cm510.h" +#include "dynamixel.h" #include "dyn_servos.h" #include "dyn_servos_reg.h" #include "action.h" -#include "balance.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); -extern void balance_loop(void); -extern void balance_get_all_offsets(int16_t **offsets); -extern void action_init(void); -extern void action_process(void); +#include "cm510.h" // private variables uint8_t manager_num_servos; TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS]; -extern TPage action_next_page; -extern TPage action_current_page; +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; typedef struct { uint8_t cc_slope; uint8_t ccw_slope; uint16_t position; -}dyn_send_data; +}dxl_send_struct; -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_timer_init(void) -{ - // initialize timer4 for buzzer time control (7.8ms period) - TIMSK4=0x00;// disable all interrupts - TIFR4=0x2F;// clear any pending interrupt - TCNT4H=0x00; - TCNT4L=0x00; - // set PWM mode with ICR top-count (CTC mode) - TCCR4A=0x00; - TCCR4A&=~(1<<WGM40); - TCCR4A&=~(1<<WGM41); - TCCR4B=0x00; - TCCR4B|=(1<<WGM42); - TCCR4B&=~(1<<WGM43); - // clear output compare value A - OCR4AH=0x3D; - OCR4AL=0x08; - TCCR4B&=0xF8;// set the prescaler to 8 - TCCR4B|=0x02; -} - -uint8_t manager_period_done(void) -{ - if(TIFR4&(1<<OCF4A)) - { - TIFR4|=(1<<OCF4A);// clear any interrupt - TCNT4H=0x00; - TCNT4L=0x00; - return 0x01; - } - else - return 0x00; -} - -void manager_send_motion_command(void) -{ - uint8_t *pdata; +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 i; uint16_t target_pos; - int16_t *offsets; + uint8_t ccslope, ccwslope; - pdata = (uint8_t *)data; + ppacket = (uint8_t * )packet; - balance_get_all_offsets(&offsets); - for(i=0;i<manager_num_servos;i++) - { - target_pos = manager_servos[i].current_value + offsets[i]; - pdata[i*4+2] = target_pos&0xFF; - pdata[i*4+3] = (target_pos>>8)&0xFF; + for(i=0;i<manager_num_servos;i++) { + servo_ids[i]=manager_servos[i].id; + target_pos = manager_servos[i].current_value + servo_offsets[i]; if(manager_servos[i].cc_slope == 0) - pdata[i*4] = 32; - else - pdata[i*4] = 1<<(0x0F&manager_servos[i].cc_slope); + ccslope = 32; + else ccslope = 1<<(0x0F&manager_servos[i].cc_slope); if(manager_servos[i].ccw_slope == 0) - pdata[i*4+1] = 32; - else - pdata[i*4+1] = 1<<(0x0F&manager_servos[i].ccw_slope); + ccwslope = 32; + else ccslope = 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; + } + + dxl_sync_tx_packet(manager_num_servos,(unsigned char *)servo_ids,CW_COMP_SLOPE,(unsigned char *)packet,4); +} + +void manager_balance(void) +{ + int16_t gyro_x=0; + int16_t gyro_y=0; + + + 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; + + + if(gyro_x > 200) + robot_fallen_state = 0x02; + else if(gyro_x < -200) + robot_fallen_state = 0x01; + + float x_error1,x_error2,y_error1,y_error2; + x_error1=gyro_x*4.0/54.0; + x_error2=gyro_x*4.0/18.0; + y_error1=gyro_y*4.0/20.0; + y_error2=gyro_y*4.0/40.0; - packets[i].data_addr=(uint8_t *)&data[i]; + servo_offsets[8] = (uint16_t)y_error1; + servo_offsets[9] = (uint16_t)y_error1; + servo_offsets[12] = (uint16_t)x_error1;//-1.0; + servo_offsets[13] = (uint16_t)-x_error1;//+1.0; + servo_offsets[14] = (uint16_t)+x_error2; + servo_offsets[15] = (uint16_t)-x_error2; + servo_offsets[16] = (uint16_t)-y_error2; + servo_offsets[17] = (uint16_t)-y_error2; } - if(manager_num_servos>0) - dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets); +} + +inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle) +{ + return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle; +} + +inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value) +{ + return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle); +} + +inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed) +{ + if(speed>manager_servos[servo_id].max_speed) + speed=manager_servos[servo_id].max_speed; + return (speed*3)>>1; +} + +inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value) +{ + return (value*2)/3; } void manager_get_current_position(void) @@ -108,7 +122,10 @@ void manager_get_current_position(void) uint8_t i; for(i=0;i<manager_num_servos;i++) + { manager_servos[i].current_value=get_current_position(i+1); +// manager_servos[i].current_angle=manager_value_to_angle(i+1,manager_servos[i].current_value); + } } void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope) @@ -126,11 +143,6 @@ void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope) } } -void manager_set_index_value(uint8_t servo_id, uint16_t value) -{ - manager_servos[servo_id].current_value=value; -} - void manager_set_servo_value(uint8_t servo_id, uint16_t value) { uint8_t i; @@ -145,6 +157,13 @@ void manager_set_servo_value(uint8_t servo_id, uint16_t value) } } +void manager_set_index_value(uint8_t servo_id, uint16_t value) +{ + manager_servos[servo_id].current_value=value; + manager_servos[servo_id].current_angle=((value-manager_servos[servo_id].center_value)*(manager_servos[servo_id].max_angle-manager_servos[servo_id].min_angle))/manager_servos[servo_id].max_value; + //ram_write_word(CM730_SERVO_1_POS_L+servo_id*2,manager_servos[servo_id].current_angle); +} + uint16_t manager_get_index_value(uint8_t servo_id) { return manager_servos[servo_id-1].current_value; @@ -161,53 +180,176 @@ uint16_t manager_get_servo_value(uint8_t servo_id) return 2048; } -void manager_loop(void) +uint8_t manager_scan_servos(uint8_t n_servos) { - if(manager_period_done()==0x01) + uint8_t i; + uint8_t num = 0; + uint8_t error; + + for(i=1;i<=n_servos;i++) { - // call the action process - action_process(); //action_ready - // balance the robot - balance_loop(); - // send the motion commands to the servos - manager_send_motion_command(); + 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) +{ + 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 -uint8_t manager_init(uint8_t num_servos) +void manager_init(unsigned char num_servos) { uint8_t i; + uint8_t found_servos; + robot_fallen_state = 0x00; // enable power to the servos - dyn_master_init(); + dxl_initialize(0,1); _delay_ms(500); - /* scan the bus for all available servos */ - dyn_master_scan(&manager_num_servos,servo_ids); - if(num_servos != manager_num_servos) - buzzer_start_alarm(MANAGER_MISSING_SERVOS_ALARM_NOTE,MANAGER_MISSING_SERVOS_ALARM_TIME_ON,MANAGER_MISSING_SERVOS_ALARM_TIME_OFF); - else - buzzer_stop_alarm(); + 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); for(i=0;i<manager_num_servos;i++) { //set_target_speed(i+1,0); - enable_servo(servo_ids[i]); - manager_servos[i].id=servo_ids[i]; + servo_offsets[i] = 0; + + enable_servo(i+1); + manager_servos[i].id=i+1; + manager_servos[i].encoder_resolution=1023; + manager_servos[i].gear_ratio=254; + manager_servos[i].max_angle=300<<7; + manager_servos[i].center_angle=150<<7; + manager_servos[i].max_speed=354; 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(servo_ids[i]); + manager_servos[i].max_angle=1500; + manager_servos[i].min_angle=-1500; + manager_servos[i].current_value=get_current_position(i+1); + // manager_servos[i].current_angle=manager_value_to_angle(i+1,manager_servos[i].current_value); } - /* initialize the period timer */ - manager_timer_init(); - /* initialize the action module */ - action_init(); + timer_init(); + // initialize the controller + //init_cm510(ASYNC); + // initialize the timer interrupts + sei(); + //action_init(); + +} + +unsigned char manager_check_robot_fallen(void) +{ + unsigned char state = robot_fallen_state; + robot_fallen_state = 0x00; + return state; +} + +unsigned char manager_calibrate_gyro(void) +{ + unsigned short int x_gyro_values[GYRO_CAL_NUM_SAMPLES]; + unsigned short int 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; + x_gyro_center=0; + y_gyro_center=0; + while(!calibrated) + { + 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_average+=x_gyro_values[i]; + y_gyro_average+=y_gyro_values[i]; + } + x_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES; + y_gyro_average/=(float)GYRO_CAL_NUM_SAMPLES; + // compute the standard deviation + for(i=0;i<GYRO_CAL_NUM_SAMPLES;i++) + { + std_x+=(x_gyro_values[i]-x_gyro_average)*(x_gyro_values[i]-x_gyro_average); + std_y+=(y_gyro_values[i]-y_gyro_average)*(y_gyro_values[i]-y_gyro_average); + } + std_x=sqrt(std_x/GYRO_CAL_NUM_SAMPLES); + std_y=sqrt(std_y/GYRO_CAL_NUM_SAMPLES); + 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; + } + else + { + count++; + if(count==5) + return 0x00; + } + } return 0x01; } +void manager_enable_gyro(void) +{ + gyro_enabled=0x01; +} + +void manager_disable_gyro(void) +{ + gyro_enabled=0x00; +} + +unsigned char manager_is_gyro_enabled(void) +{ + return gyro_enabled; +} diff --git a/motion/src/motion_pages.c b/motion/src/motion_pages.c index ef28d68ea22f1f88f867559c985f1ed40e310420..c8641f9e46af6e56dca137118ca738206b6f0e7c 100644 --- a/motion/src/motion_pages.c +++ b/motion/src/motion_pages.c @@ -1,14 +1,14 @@ #include "motion_pages.h" +#include "cm510.h" -void pages_get_page(uint8_t page_id,TPage *page) -{ - uint8_t i=0; - uint8_t *ppage = (uint8_t *)page; - TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id]; - +void pages_get_page(uint8_t page_id,TPage *page) { + uint8_t i=0; + uint8_t *ppage = (uint8_t *)page; + TPage *base_page = &((TPage *)PAGE_BASE_ADDR)[page_id]; + for(i=0;i<sizeof(TPage);i++) { - ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i); - } + ppage[i]= pgm_read_byte_far((uint8_t *)base_page+i); + } } uint8_t pages_check_checksum(TPage *page) @@ -40,18 +40,24 @@ void pages_copy_page(TPage *src,TPage *dst) ((uint8_t *)dst)[i]=((volatile uint8_t *)src)[i]; } -uint8_t load_page_info(uint8_t page_id,TPage *page) -{ + +uint8_t load_page_info(uint8_t page_id,TPage *page) { +// uint8_t *start_address; +// uint16_t i; + +// start_address=(uint8_t *)POSE_BASE_ADR+(0x200*page_id); + +// for(i=0;i<0x200;i++) (((uint8_t *)page)[i])=pgm_read_byte(start_address+i); uint32_t i=0; uint8_t *ppage = (uint8_t *)page; +// TPage *base_page = (TPage *)PAGE_BASE_ADDR; - for(i=0;i<sizeof(TPage);i++) - ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+((uint32_t)sizeof(TPage)*(uint32_t)page_id)+i); - - if(pages_check_checksum(page)==0) - return 0; - else - return -1; + for(i=0;i<sizeof(TPage);i++) { + ppage[i]= pgm_read_byte_far((uint32_t)PAGE_BASE_ADDR+(uint32_t)(sizeof(TPage)*page_id)+i); +// ppage[i]= pgm_read_byte_far( &(base_page[page_id])+i); + } + if(pages_check_checksum(page)==0) return 0; + else return -1; } diff --git a/robotis_lib/src/Dynamixel/dxl_hal.c b/robotis_lib/src/Dynamixel/dxl_hal.c index 3c39f70bc45bf3eec167102affdd74d45b96486f..6a4817982ab91bc905d3c626164768140b4399f8 100755 --- a/robotis_lib/src/Dynamixel/dxl_hal.c +++ b/robotis_lib/src/Dynamixel/dxl_hal.c @@ -206,4 +206,4 @@ unsigned char dxl_hal_get_queue(void) SIGNAL(USART0_RX_vect) { dxl_hal_put_queue( UDR0 ); -} +} \ No newline at end of file