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 5154b967cffd8ede038d1c5d6c2272ad28c68a50..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
@@ -25,8 +25,8 @@
#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_X_CHANNEL 4
+#define BALANCE_GYRO_Y_CHANNEL 3
#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
diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c
index 16612ed2edf70c225e86a7808a6e29daf7121158..881ee13fa7cb5b116e1b5e76741641940500cf4d 100755
--- a/controllers/src/cm510.c
+++ b/controllers/src/cm510.c
@@ -52,7 +52,7 @@ int16_t main(void)
init_cm510();
turn_led_off(LED_BAT);
sei();
- _delay_ms(2000);
+ _delay_ms(3000);
/* call the user initialization function */
dyn_master_init();
exp_board_init(EXP_BOARD_ID);
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..71961032da5dc002276178a674d4a16ac319d479 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/movements/mtn_library.c b/examples/movements/mtn_library.c
deleted file mode 100644
index 63cb4cba312f48d827de8b82ffeedd92d687bfde..0000000000000000000000000000000000000000
--- a/examples/movements/mtn_library.c
+++ /dev/null
@@ -1,445 +0,0 @@
-#include <avr/io.h>
-#include "mtn_library.h"
-#include "action_id.h"
-#include "action.h"
-#include <stdio.h>
-
-// private variables
-typedef enum {mtn_fwd=0,mtn_bwd=1,mtn_turn_left=2,mtn_turn_right=3,mtn_left=4,mtn_right=5,mtn_fwd_turn_left=6,mtn_fwd_turn_right=7,
- mtn_bwd_turn_left=8,mtn_bwd_turn_right=9,mtn_fwd_left=10,mtn_fwd_right=11,mtn_bwd_left=12,mtn_bwd_right=13,
- mtn_fast_fwd=14,mtn_fast_bwd=15} mtn_t;
-typedef enum {idle,wait_start,wait_middle,wait_end} full_states;
-
-typedef struct{
- uint8_t start_l;
- uint8_t start_r;
- uint8_t middle_l;
- uint8_t middle_r;
- uint8_t end_l;
- uint8_t end_r;
-}TPages;
-
-uint8_t mtn_lib_stop;
-foot_t mtn_lib_start_foot;
-foot_t mtn_lib_current_foot;
-TPages mtn_pages[]={{F_S_L,F_S_R,F_M_L,F_M_R,F_E_L,F_E_R},
- {B_S_L,B_S_R,B_M_L,B_M_R,B_E_L,B_E_R},
- {LT_S_L,LT_S_R,LT_M_L,LT_M_R,LT_E_L,LT_E_R},
- {RT_S_L,RT_S_R,RT_M_L,RT_M_R,RT_E_L,RT_E_R},
- {L_S_L,L_S_R,L_M_L,L_M_R,L_E_L,L_E_R},
- {R_S_L,R_S_R,R_M_L,R_M_R,R_E_L,R_E_R},
- {FLT_S_L,FLT_S_R,FLT_M_L,FLT_M_R,FLT_E_L,FLT_E_R},
- {FRT_S_L,FRT_S_R,FRT_M_L,FRT_M_R,FRT_E_L,FRT_E_R},
- {BLT_S_L,BLT_S_R,BLT_M_L,BLT_M_R,BLT_E_L,BLT_E_R},
- {BRT_S_L,BRT_S_R,BRT_M_L,BRT_M_R,BRT_E_L,BRT_E_R},
- {FL_S_L,FL_S_R,FL_M_L,FL_M_R,FL_E_L,FL_E_R},
- {FR_S_L,FR_S_R,FR_M_L,FR_M_R,FR_E_L,FR_E_R},
- {BL_S_L,BL_S_R,BL_M_L,BL_M_R,BL_E_L,BL_E_R},
- {BR_S_L,BR_S_R,BR_M_L,BR_M_R,BR_E_L,BR_E_R},
- {fst_F_L_S,fst_F_R_S,fst_F_R_L_M,fst_F_L_R_M,fst_F_R_E,fst_F_L_E},
- {fst_B_L_S,fst_B_R_S,fst_B_L_M,fst_B_R_M,fst_B_L_E,fst_B_R_E}};
-
-/* private functions */
-uint8_t mtn_lib_full(TPages *pages)
-{
- static full_states state=idle;
- uint8_t done=0x00;
-
- switch(state)
- {
- case idle: if(mtn_lib_start_foot==left_foot)
- action_set_page(pages->start_l);
- else
- action_set_page(pages->start_r);
- action_start_page();
- mtn_lib_current_foot=mtn_lib_start_foot;
- state=wait_start;
- break;
- case wait_start: if(is_action_running())
- state=wait_start;
- else
- {
- if(mtn_lib_stop==0x01)
- {
- if(mtn_lib_current_foot==left_foot)
- {
- action_set_page(pages->end_r);
- mtn_lib_current_foot=right_foot;
- }
- else
- {
- action_set_page(pages->end_l);
- mtn_lib_current_foot=left_foot;
- }
- action_start_page();
- state=wait_end;
- }
- else
- {
- if(mtn_lib_current_foot==left_foot)
- {
- action_set_page(pages->middle_r);
- mtn_lib_current_foot=right_foot;
- }
- else
- {
- action_set_page(pages->middle_l);
- mtn_lib_current_foot=left_foot;
- }
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_middle: if(is_action_running())
- state=wait_middle;
- else
- {
- if(mtn_lib_stop==0x01)
- {
- if(mtn_lib_current_foot==left_foot)
- {
- action_set_page(pages->end_r);
- mtn_lib_current_foot=right_foot;
- }
- else
- {
- action_set_page(pages->end_l);
- mtn_lib_current_foot=left_foot;
- }
- action_start_page();
- state=wait_end;
- }
- else
- {
- if(mtn_lib_current_foot==left_foot)
- {
- action_set_page(pages->middle_r);
- mtn_lib_current_foot=right_foot;
- }
- else
- {
- action_set_page(pages->middle_l);
- mtn_lib_current_foot=left_foot;
- }
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_end: if(is_action_running())
- state=wait_end;
- else
- {
- mtn_lib_stop=0x00;
- state=idle;
- done=0x01;
- }
- break;
- }
-
- return done;
-}
-
-uint8_t mtn_lib_left(TPages *pages)
-{
- static full_states state=idle;
- uint8_t done=0x00;
-
- switch(state)
- {
- case idle: if(mtn_lib_start_foot==left_foot)
- action_set_page(pages->start_l);
- else
- action_set_page(pages->start_r);
- action_start_page();
- mtn_lib_current_foot=mtn_lib_start_foot;
- state=wait_start;
- break;
- case wait_start: if(is_action_running())
- state=wait_start;
- else
- {
- if(mtn_lib_current_foot==left_foot)
- {
- if(mtn_lib_stop==0x01)
- {
- action_set_page(pages->end_r);
- state=wait_end;
- }
- else
- {
- action_set_page(pages->middle_r);
- state=wait_middle;
- }
- mtn_lib_current_foot=right_foot;
- action_start_page();
- }
- else
- {
- action_set_page(pages->middle_l);
- mtn_lib_current_foot=left_foot;
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_middle: if(is_action_running())
- state=wait_middle;
- else
- {
- if(mtn_lib_current_foot==left_foot)
- {
- if(mtn_lib_stop==0x01)
- {
- action_set_page(pages->end_r);
- state=wait_end;
- }
- else
- {
- action_set_page(pages->middle_r);
- state=wait_middle;
- }
- mtn_lib_current_foot=right_foot;
- action_start_page();
- }
- else
- {
- action_set_page(pages->middle_l);
- mtn_lib_current_foot=left_foot;
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_end: if(is_action_running())
- state=wait_end;
- else
- {
- mtn_lib_stop=0x00;
- state=idle;
- done=0x01;
- }
- break;
- }
-
- return done;
-}
-
-uint8_t mtn_lib_right(TPages *pages)
-{
- static full_states state=idle;
- uint8_t done=0x00;
-
- switch(state)
- {
- case idle: if(mtn_lib_start_foot==left_foot)
- action_set_page(pages->start_l);
- else
- action_set_page(pages->start_r);
- action_start_page();
- mtn_lib_current_foot=mtn_lib_start_foot;
- state=wait_start;
- break;
- case wait_start: if(is_action_running())
- state=wait_start;
- else
- {
- if(mtn_lib_current_foot==right_foot)
- {
- if(mtn_lib_stop==0x01)
- {
- action_set_page(pages->end_l);
- state=wait_end;
- }
- else
- {
- action_set_page(pages->middle_l);
- state=wait_middle;
- }
- mtn_lib_current_foot=left_foot;
- action_start_page();
- }
- else
- {
- action_set_page(pages->middle_r);
- mtn_lib_current_foot=right_foot;
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_middle: if(is_action_running())
- state=wait_middle;
- else
- {
- if(mtn_lib_current_foot==right_foot)
- {
- if(mtn_lib_stop==0x01)
- {
- action_set_page(pages->end_l);
- state=wait_end;
- }
- else
- {
- action_set_page(pages->middle_l);
- state=wait_middle;
- }
- mtn_lib_current_foot=left_foot;
- action_start_page();
- }
- else
- {
- action_set_page(pages->middle_r);
- mtn_lib_current_foot=right_foot;
- action_start_page();
- state=wait_middle;
- }
- }
- break;
- case wait_end: if(is_action_running())
- state=wait_end;
- else
- {
- mtn_lib_stop=0x00;
- state=idle;
- done=0x01;
- }
- break;
- }
-
- return done;
-}
-
-/* public functions */
-void mtn_lib_init(void)
-{
- mtn_lib_stop=0x00;
- mtn_lib_start_foot=left_foot;
- mtn_lib_current_foot=left_foot;
-}
-
-void mtn_lib_stop_mtn(void)
-{
- mtn_lib_stop=0x01;
-}
-
-void mtn_lib_set_start_foot(foot_t foot)
-{
- mtn_lib_start_foot=foot;
-}
-
-uint8_t walk_forward(void)
-{
- return mtn_lib_full(&mtn_pages[mtn_fwd]);
-}
-
-uint8_t walk_backward(void)
-{
- return mtn_lib_full(&mtn_pages[mtn_bwd]);
-}
-
-uint8_t turn_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_turn_left]);
-}
-
-uint8_t turn_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_turn_right]);
-}
-
-uint8_t walk_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_left]);
-}
-
-uint8_t walk_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_right]);
-}
-
-uint8_t walk_forward_turn_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_fwd_turn_left]);
-}
-
-uint8_t walk_forward_turn_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_fwd_turn_right]);
-}
-
-uint8_t walk_backward_turn_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_bwd_turn_left]);
-}
-
-uint8_t walk_backward_turn_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_bwd_turn_right]);
-}
-
-uint8_t walk_forward_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_fwd_left]);
-}
-
-uint8_t walk_forward_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_fwd_right]);
-}
-
-uint8_t walk_backward_left(void)
-{
- return mtn_lib_left(&mtn_pages[mtn_bwd_left]);
-}
-
-uint8_t walk_backward_right(void)
-{
- return mtn_lib_right(&mtn_pages[mtn_bwd_right]);
-}
-
-
-uint8_t fast_walk_forward(void)
-{
- return mtn_lib_full(&mtn_pages[mtn_fast_fwd]);
-}
-
-uint8_t fast_walk_backward(void)
-{
- return mtn_lib_full(&mtn_pages[mtn_fast_bwd]);
-}
-
-uint8_t fast_turn_left(void)
-{
-}
-
-uint8_t fast_turn_right(void)
-{
-}
-
-uint8_t fast_walk_left(void)
-{
-}
-
-uint8_t fast_walk_right(void)
-{
-}
-
-uint8_t fast_walk_forward_turn_left(void)
-{
-}
-
-uint8_t fast_walk_forward_turn_right(void)
-{
-}
-
-uint8_t fast_walk_backward_turn_left(void)
-{
-}
-
-uint8_t fast_walk_backward_turn_right(void)
-{
-}
-
-
diff --git a/examples/movements/mtn_library.h b/examples/movements/mtn_library.h
deleted file mode 100644
index c23f1609c6e48e5b9e88137345569a3620a7d54e..0000000000000000000000000000000000000000
--- a/examples/movements/mtn_library.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef _MTN_LIBRARY_H
-#define _MTN_LIBRARY_H
-
-typedef enum {left_foot,right_foot} foot_t;
-
-void mtn_lib_init(void);
-void mtn_lib_stop_mtn(void);
-void mtn_lib_set_start_foot(foot_t foot);
-
-uint8_t walk_forward(void);
-uint8_t walk_backward(void);
-uint8_t turn_left(void);
-uint8_t turn_right(void);
-uint8_t walk_left(void);
-uint8_t walk_right(void);
-uint8_t walk_forward_turn_left(void);
-uint8_t walk_forward_turn_right(void);
-uint8_t walk_backward_turn_left(void);
-uint8_t walk_backward_turn_right(void);
-uint8_t walk_forward_left(void);
-uint8_t walk_forward_right(void);
-uint8_t walk_backward_left(void);
-uint8_t walk_backward_right(void);
-
-uint8_t fast_walk_forward(void);
-uint8_t fast_walk_backward(void);
-uint8_t fast_turn_left(void);
-uint8_t fast_turn_right(void);
-uint8_t fast_walk_left(void);
-uint8_t fast_walk_right(void);
-uint8_t fast_walk_forward_turn_left(void);
-uint8_t fast_walk_forward_turn_right(void);
-uint8_t fast_walk_backward_turn_left(void);
-uint8_t fast_walk_backward_turn_right(void);
-
-#endif
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/balance.c b/motion/src/balance.c
index dd5290656c969e42b5efdf6b78a373b6926fc2b8..cdbd6da17a197f57bdb833457176c46ff3a4c58e 100644
--- a/motion/src/balance.c
+++ b/motion/src/balance.c
@@ -35,9 +35,9 @@ void balance_loop(void)
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)
+ if(gyro_x < BALANCE_BACKWARD_FALL_THD_VALUE)
balance_robot_fallen_state = robot_face_up;
- else if(gyro_x < BALANCE_FORWARD_FALL_THD_VALUE)
+ 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;
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index dc3dc786209368eace9d7874b79ff223660fb169..4dbb21d47f3134367e4c6f591150ce3477587820 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;
@@ -95,8 +97,8 @@ void manager_send_motion_command(void)
if(manager_servos[i].slope == 0)
{
- pdata[num*4] = 32;
- pdata[num*4+1] = 32;
+ pdata[num*4] = 5;
+ pdata[num*4+1] = 5;
}
else
{
@@ -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++;