diff --git a/communications/include/comm_cfg.h b/communications/include/comm_cfg.h
index 454edd881749c3ab5c5fa892726b6ca083a13006..05a8a6d2aa6ba00ecdb10fb0be5a1d5079d91d72 100644
--- a/communications/include/comm_cfg.h
+++ b/communications/include/comm_cfg.h
@@ -14,7 +14,7 @@
#endif
#ifndef DYN_MASTER_DEFAULT_TIMEOUT_US
- #define DYN_MASTER_DEFAULT_TIMEOUT_US 10000
+ #define DYN_MASTER_DEFAULT_TIMEOUT_US 2000
#endif
#ifndef SERIAL_CONSOLE_MAX_BUFFER_LEN
diff --git a/communications/include/dynamixel_master.h b/communications/include/dynamixel_master.h
index 01f3d947065aa3a7c5b8dba78c94dc049154a6a7..bce7c4a5408d2e7269e75571ce55aa00eaa4be0d 100644
--- a/communications/include/dynamixel_master.h
+++ b/communications/include/dynamixel_master.h
@@ -19,6 +19,8 @@ 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_start_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
+uint8_t dyn_master_wait_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_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);
diff --git a/communications/src/dynamixel_master.c b/communications/src/dynamixel_master.c
index 95865d9704c57df97e4ceb7d210ddbd7b6d4469f..fa36b904ef61e63b8adb8071b5304433cdac737f 100644
--- a/communications/src/dynamixel_master.c
+++ b/communications/src/dynamixel_master.c
@@ -113,7 +113,17 @@ ISR(USART0_RX_vect)
uint8_t dyn_master_wait_transmission(void)
{
- while(dyn_master_sent_done==0);
+ dyn_master_start_timeout();
+ while(dyn_master_sent_done==0)
+ {
+ if(dyn_master_timeout())
+ {
+ dyn_master_cancel_timeout();
+ dyn_master_sent_done=0x01;
+ return DYN_TIMEOUT;
+ }
+ }
+ dyn_master_cancel_timeout();
return DYN_SUCCESS;
}
@@ -274,7 +284,8 @@ uint8_t dyn_master_ping(uint8_t id)
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// generate the ping packet for the desired device
dyn_init_ping_packet(dyn_master_tx_buffer,id);
dyn_master_rx_num_packets=0x01;
@@ -310,12 +321,56 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
return error;
}
+
+uint8_t dyn_master_start_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+ uint8_t error;
+
+ // wait until any previous transmissions ends
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
+ // 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;
+ }
+ return error;
+}
+
+uint8_t dyn_master_wait_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+ uint8_t error=DYN_SUCCESS;
+
+ // 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_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
{
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// 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;
@@ -363,7 +418,8 @@ uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, ui
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// 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;
@@ -391,7 +447,8 @@ uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// 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;
@@ -419,7 +476,8 @@ uint8_t dyn_master_action(void)
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// generate the action packet for the desired device
dyn_init_action_packet(dyn_master_tx_buffer);
dyn_master_rx_num_packets=0x01;
@@ -441,7 +499,8 @@ uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_
uint8_t error;
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// 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;
@@ -463,7 +522,8 @@ uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_
uint8_t error,i,ver1_address[255],ver1_length[255];
// wait until any previous transmissions ends
- dyn_master_wait_transmission();
+ if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+ return DYN_TIMEOUT;
// generate the read packet for the desired device
for(i=0;i<num;i++)
{
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 9f53a7a3e2d012eb17032cfa56d80eb1e9bd22b0..af72a027b39da22565406ddc29c855ccda09e069 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -18,17 +18,14 @@ static FILE *device;
/* interrupt handlers */
SIGNAL(USART1_RX_vect)
{
- cli();
serial_console_rx_buffer[serial_console_rx_write_ptr]=UDR1;
serial_console_rx_write_ptr=(serial_console_rx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
if(serial_console_rx_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
serial_console_rx_num_data++;
- sei();
}
SIGNAL(USART1_TX_vect)
{
- cli();
if(serial_console_tx_num_data>0)
{
UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
@@ -37,7 +34,6 @@ SIGNAL(USART1_TX_vect)
}
else
serial_console_sending=0x00;
- sei();
}
/* private functions */
@@ -53,7 +49,6 @@ void serial_console_set_baudrate(uint32_t baudrate)
int serial_console_putchar(char c,FILE *dev)
{
- cli();
if(c=='\n')
{
serial_console_tx_buffer[serial_console_tx_write_ptr]='\r';
@@ -70,7 +65,6 @@ int serial_console_putchar(char c,FILE *dev)
serial_console_tx_num_data--;
serial_console_sending=0x01;
}
- sei();
return 0;
}
@@ -79,7 +73,6 @@ int serial_console_getchar(FILE *dev)
{
int8_t rx;
- cli();
if(serial_console_rx_num_data>0)
{
rx=serial_console_rx_buffer[serial_console_rx_read_ptr];
@@ -87,14 +80,10 @@ int serial_console_getchar(FILE *dev)
serial_console_rx_num_data--;
if(rx=='\r')
rx='\n';
- sei();
return rx;
}
else
- {
- sei();
return -1;
- }
}
/* pubic functions */
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index 8ebb4b7790e12abedfc47ceecc8435de06276c88..a6c5b79cf954c135ec2e479b26a784e77594040d 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -16,7 +16,7 @@
#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 1000
+#define DYN_MASTER_DEFAULT_TIMEOUT_US 2000
#define SERIAL_CONSOLE_MAX_BUFFER_LEN 128
// motion configuration parameters
@@ -31,8 +31,8 @@
#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_FORWARD_FALL_THD_VALUE 200
+#define BALANCE_BACKWARD_FALL_THD_VALUE (-200)
#define BALANCE_KNEE_GAIN (4.0/54.0)
#define BALANCE_ANKLE_PITCH_GAIN (4.0/18.0)
#define BALANCE_ANKLE_ROLL_GAIN (4.0/20.0)
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index 71ddd298767dd535c434e18f1d820d0db70c66a3..2d913e447ec677acba807529f676ae90dbce056e 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -41,7 +41,17 @@ uint8_t exp_board_int_data[BLOCK_LENGTH];
/* private functions */
#if EXP_BOARD_USE_LOOP==1
+void exp_board_loop_start(void)
+{
+ dyn_master_start_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
+}
+
+void exp_board_loop_read(void)
+{
+ dyn_master_wait_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
+}
void exp_board_loop(void)
+
{
dyn_master_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
}
diff --git a/examples/movements/Makefile b/examples/movements/Makefile
index 84f98f106644726863bcbd568b0d45bd91e566d5..295b2a205fe97a8ea7639763a9bb8aab4e6002f6 100644
--- a/examples/movements/Makefile
+++ b/examples/movements/Makefile
@@ -2,7 +2,7 @@ PROJECT=movements
########################################################
# afegir tots els fitxers que s'han de compilar aquÃ
########################################################
-SOURCES=movements.c mtn_library.c
+SOURCES=movements.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./
diff --git a/examples/movements/movements.c b/examples/movements/movements.c
index af81f3ffe70a7ab555e1d6437621e22fecf784b1..67ad75d6be36a02085e66d56d9cded41a25fe9c7 100644
--- a/examples/movements/movements.c
+++ b/examples/movements/movements.c
@@ -14,13 +14,14 @@ void user_init(void)
balance_init();
balance_calibrate_gyro();
balance_enable_gyro();
+ user_time_set_period(100);
mtn_lib_init();
}
typedef uint8_t (*fnct_ptr)(void);
-fnct_ptr fnct1=fast_walk_forward;
-//fnct_ptr fnct1=walk_forward;
+//fnct_ptr fnct1=fast_walk_forward;
+fnct_ptr fnct1=walk_forward;
//fnct_ptr fnct1=walk_right;
//fnct_ptr fnct1=walk_backward_turn_left;
@@ -31,6 +32,7 @@ fnct_ptr fnct2=turn_left;
void user_loop(void)
{
static main_states state=wait_start;
+ static unsigned char i=0;
switch(state)
{
@@ -66,7 +68,14 @@ void user_loop(void)
if(fnct1()==0x01)
state=wait_cmd;
else
+ {
state=walk;
+// if(user_time_is_period_done())
+// {
+ printf("%d\n",i);
+ i++;
+// }
+ }
break;
case walk2: if(is_button_rising_edge(BTN_RIGHT))
mtn_lib_stop_mtn();
diff --git a/examples/vision/cm510_vision.c b/examples/vision/cm510_vision.c
index 9c86ccaede8ce9d3f43b552e4cd3a008ee55b3b5..f77a0da988f81bf8808bc742ef80457b6e4c4da5 100644
--- a/examples/vision/cm510_vision.c
+++ b/examples/vision/cm510_vision.c
@@ -40,9 +40,10 @@ void user_loop(void)
break;
case wait_cmd: if(serial_console_get_num_data()>0)
{
- printf("New data received\n");
+// printf("New data received\n");
cm510_scanf("%c",&cmd);
- switch(cmd)
+ printf("%c",cmd);
+/* switch(cmd)
{
case 'l': pan_set_speed(200);
pan_move_angle(-70);
@@ -56,7 +57,8 @@ void user_loop(void)
break;
default: state=wait_cmd;
break;
- }
+ }*/
+ state=wait_cmd;
}
else
state=wait_cmd;
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
index 0440e64c64301544b029bb5b779867790189d5ce..a4d211884c84f21573f590a2c2cc1650812af696 100644
--- a/motion/include/motion_cfg.h
+++ b/motion/include/motion_cfg.h
@@ -48,11 +48,11 @@
#endif
#ifndef BALANCE_FORWARD_FALL_THD_VALUE
- #define BALANCE_FORWARD_FALL_THD_VALUE (-200)
+ #define BALANCE_FORWARD_FALL_THD_VALUE 200
#endif
#ifndef BALANCE_BACKWARD_FALL_THD_VALUE
- #define BALANCE_BACKWARD_FALL_THD_VALUE 200
+ #define BALANCE_BACKWARD_FALL_THD_VALUE (-200)
#endif
#ifndef BALANCE_KNEE_GAIN
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index dc3dc786209368eace9d7874b79ff223660fb169..8ff1da5766b372890ec8033c5fdc42241474d919 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -22,6 +22,8 @@ extern void balance_get_all_offsets(int16_t **offsets);
extern void action_init(void);
extern void action_process(void);
extern void exp_board_loop(void);
+extern void exp_board_loop_start(void);
+extern void exp_board_loop_read(void);
// private variables
uint8_t manager_num_servos;
@@ -150,9 +152,13 @@ void manager_loop(void)
{
if(manager_period_done()==0x01)
{
- #if EXP_BOARD_USE_LOOP==1
+/* #if EXP_BOARD_USE_LOOP==1
if(exp_board_is_present())
exp_board_loop();
+ #endif*/
+ #if EXP_BOARD_USE_LOOP==1
+ if(exp_board_is_present())
+ exp_board_loop_start();
#endif
// call the action process
action_process(); //action_ready
@@ -160,6 +166,10 @@ void manager_loop(void)
balance_loop();
// update the pan&tilt angles
pan_tilt_process();
+ #if EXP_BOARD_USE_LOOP==1
+ if(exp_board_is_present())
+ exp_board_loop_read();
+ #endif
// send the motion commands to the servos
manager_send_motion_command();
}
@@ -205,7 +215,7 @@ uint8_t manager_init(uint8_t num_servos)
manager_servos[id].min_value=0;
manager_servos[id].center_value=512;
manager_servos[id].current_value=get_current_position(id);
- get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+ get_angle_limits(id,&manager_servos[id].cw_limit,&manager_servos[id].ccw_limit);
manager_servos[id].module=MM_ACTION;
manager_servos[id].slope=5;
manager_num_servos++;
@@ -218,7 +228,7 @@ uint8_t manager_init(uint8_t num_servos)
manager_servos[id].min_value=0;
manager_servos[id].center_value=512;
manager_servos[id].current_value=get_current_position(id);
- get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+ get_angle_limits(id,&manager_servos[id].cw_limit,&manager_servos[id].ccw_limit);
manager_servos[id].module=MM_JOINTS;
manager_servos[id].slope=5;
manager_num_servos++;