diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index 0b2b730ac12bbc37fba3455238aed9fe85a1d636..54775760f91b5cbe79d46f74d6f9c4660d71a6be 100644
--- a/controllers/include/cm510_cfg.h
+++ b/controllers/include/cm510_cfg.h
@@ -20,7 +20,7 @@
#define SERIAL_CONSOLE_MAX_BUFFER_LEN 128
// motion configuration parameters
-#define MANAGER_MAX_NUM_SERVOS 18
+#define MANAGER_MAX_NUM_SERVOS 32
#define MANAGER_MISSING_SERVOS_ALARM_NOTE NOTE_SOL
#define MANAGER_MISSING_SERVOS_ALARM_TIME_ON 10
#define MANAGER_MISSING_SERVOS_ALARM_TIME_OFF 100
diff --git a/controllers/src/cm510.c b/controllers/src/cm510.c
index 91769ecf9738f9afe7052993a456bb43cf3fd200..16612ed2edf70c225e86a7808a6e29daf7121158 100755
--- a/controllers/src/cm510.c
+++ b/controllers/src/cm510.c
@@ -50,6 +50,7 @@ int16_t main(void)
uint8_t num_servos;
init_cm510();
+ turn_led_off(LED_BAT);
sei();
_delay_ms(2000);
/* call the user initialization function */
diff --git a/dyn_devices/src/exp_board/exp_board.c b/dyn_devices/src/exp_board/exp_board.c
index 5098889252e8bd11cf961db10a02c720031371f0..f4df041c21ed4b96d523966776fa3c6a79c795b0 100755
--- a/dyn_devices/src/exp_board/exp_board.c
+++ b/dyn_devices/src/exp_board/exp_board.c
@@ -81,6 +81,8 @@ uint8_t exp_board_init(uint8_t board_id)
exp_board_dac_present=0x00;
exp_board_uart_usb_present=0x01;
}
+ // read the number pf PWM servos
+ dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
return 0x00;
}
else
@@ -89,10 +91,10 @@ uint8_t exp_board_init(uint8_t board_id)
exp_board_uart_ttl_present=0x00;
exp_board_pwm_present=0x00;
exp_board_uart_usb_present=0x00;
+ // read the number pf PWM servos
+ dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
return 0xFF;
}
- // read the number pf PWM servos
- dyn_master_read_byte(exp_board_id,NUM_PWM_SERVOS,&exp_board_num_servos);
}
else
{
diff --git a/examples/pan_tilt/Makefile b/examples/pan_tilt/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..419086620880f1f6fae04368489a249b5993be73
--- /dev/null
+++ b/examples/pan_tilt/Makefile
@@ -0,0 +1,57 @@
+PROJECT=pan_tilt
+########################################################
+# afegir tots els fitxers que s'han de compilar aquÃ
+########################################################
+SOURCES=pan_tilt.c
+
+OBJS=$(SOURCES:.c=.o)
+SRC_DIR=./
+DEV_DIR=../../dyn_devices/
+COMM_DIR=../../communications/
+CONT_DIR=../../controllers/
+MAN_DIR=../../motion/
+CC=avr-gcc
+OBJCOPY=avr-objcopy
+MMCU=atmega2561
+
+LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
+
+INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include -I../movements
+
+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 controllers motion_manager $(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 $@ $<
+
+communications:
+ $(MAKE) -C $(COMM_DIR)
+
+dyn_devices:
+ $(MAKE) -C $(DEV_DIR)
+
+controllers:
+ $(MAKE) -C $(CONT_DIR)
+
+motion_manager:
+ $(MAKE) -C $(MAN_DIR)
+
+download: $(MAIN_OUT_HEX)
+ fw_downloader -d /dev/ttyUSB0 -f ./$(PROJECT).hex -p cm510
+
+clean:
+ -rm $(PROJECT).map
+ -rm $(PROJECT).hex
+ -rm $(PROJECT).elf
+ -rm $(OBJS)
diff --git a/examples/pan_tilt/pan_tilt.c b/examples/pan_tilt/pan_tilt.c
new file mode 100644
index 0000000000000000000000000000000000000000..8b3ec676826bace4fdcb945b2a3f3c96445eddec
--- /dev/null
+++ b/examples/pan_tilt/pan_tilt.c
@@ -0,0 +1,88 @@
+#include <util/delay.h>
+#include <stdio.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+#include "pan_tilt.h"
+
+typedef enum {wait_start,wait_cmd,wait_pan_left,wait_pan_right,wait_tilt_up,wait_tilt_down} main_states;
+
+#define PAN_SERVO_ID 19
+#define TILT_SERVO_ID 20
+
+void user_init(void)
+{
+ serial_console_init(57600);
+ balance_init();
+ balance_calibrate_gyro();
+ balance_enable_gyro();
+ user_time_set_period(100);
+ pan_tilt_init(PAN_SERVO_ID,TILT_SERVO_ID);
+}
+
+void user_loop(void)
+{
+ static main_states state=wait_start;
+
+ if(user_time_is_period_done())
+ {
+ switch(state)
+ {
+ case wait_start: if(is_button_rising_edge(BTN_START))
+ {
+ action_set_page(31);
+ action_start_page();
+ state=wait_cmd;
+ }
+ else
+ state=wait_start;
+ break;
+ case wait_cmd: if(is_button_rising_edge(BTN_LEFT))
+ {
+ pan_set_speed(10);
+ pan_move_angle(70);
+ state=wait_pan_left;
+ }
+ else if(is_button_rising_edge(BTN_RIGHT))
+ {
+ pan_set_speed(200);
+ pan_move_angle(-70);
+ state=wait_pan_right;
+ }
+ else if(is_button_rising_edge(BTN_UP))
+ {
+ tilt_set_speed(10);
+ tilt_move_angle(70);
+ state=wait_tilt_up;
+ }
+ else if(is_button_rising_edge(BTN_DOWN))
+ {
+ tilt_set_speed(200);
+ tilt_move_angle(-70);
+ state=wait_tilt_down;
+ }
+ break;
+ case wait_pan_left: if(pan_is_moving())
+ state=wait_pan_left;
+ else
+ state=wait_cmd;
+ break;
+ case wait_pan_right: if(pan_is_moving())
+ state=wait_pan_right;
+ else
+ state=wait_cmd;
+ break;
+ case wait_tilt_up: if(tilt_is_moving())
+ state=wait_tilt_up;
+ else
+ state=wait_cmd;
+ break;
+ case wait_tilt_down: if(tilt_is_moving())
+ state=wait_tilt_down;
+ else
+ state=wait_cmd;
+ break;
+ }
+ }
+}
+
diff --git a/motion/Makefile b/motion/Makefile
index 5b1a7eebcbb7df4fc7c92336a70a9c94bd683f90..2b667ac1a44b2ab3f87f261801e7016b9601c6d8 100755
--- a/motion/Makefile
+++ b/motion/Makefile
@@ -1,5 +1,5 @@
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 src/balance.c src/pan_tilt.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
BIN_DIR=./build/
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
index 399c1cfc097bb9e6204769b0af55e9dc6930495a..0440e64c64301544b029bb5b779867790189d5ce 100644
--- a/motion/include/motion_cfg.h
+++ b/motion/include/motion_cfg.h
@@ -4,7 +4,7 @@
#include "buzzer.h"
#ifndef MANAGER_MAX_NUM_SERVOS
- #define MANAGER_MAX_NUM_SERVOS 18
+ #define MANAGER_MAX_NUM_SERVOS 32
#endif
#ifndef MANAGER_MISSING_SERVOS_ALARM_NOTE
diff --git a/motion/include/motion_manager.h b/motion/include/motion_manager.h
index 32f7ff0e5e63eca3db6345e9582f319a9c3ee906..5f4e43172e70a64fa0995dd77ce72dc0cd17c454 100644
--- a/motion/include/motion_manager.h
+++ b/motion/include/motion_manager.h
@@ -11,12 +11,14 @@ typedef enum {MM_NONE=0,MM_ACTION=1,MM_JOINTS=2} mm_module_t;
// servo information structure
typedef struct{
+ uint16_t angle;
uint16_t max_value;
uint16_t min_value;
uint16_t center_value;
uint16_t current_value;
- uint8_t cc_slope;
- uint8_t ccw_slope;
+ uint16_t cw_limit;
+ uint16_t ccw_limit;
+ uint8_t slope;
mm_module_t module;
}TServoInfo;
diff --git a/motion/include/pan_tilt.h b/motion/include/pan_tilt.h
index 8ec19866066cc131b7fb70969c80f801cd5c43b4..8623da4d24d35ca6a290daa144e9e6465cbb492c 100644
--- a/motion/include/pan_tilt.h
+++ b/motion/include/pan_tilt.h
@@ -1,13 +1,28 @@
#ifndef PAN_TILT_H
#define PAN_TILT_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <avr/io.h>
+
// public functions
-void pan_tilt_init(void);
-void pan_tilt_set_angles(int8_t pan_angle,uint8_t titl_angle);
-void pan_set_angle(int8_t pan_angle);
+void pan_tilt_init(uint8_t pan_id,uint8_t tilt_id);
+void pan_tilt_move_angles(int8_t pan_angle,uint8_t titl_angle);
+void pan_move_angle(int8_t pan_angle);
void pan_set_speed(uint8_t pan_speed);
-void tilt_set_angle(int8_t tilt_angle);
-void tilt_set_speed(int8_t tilt_speed);
+void pan_stop(void);
+uint8_t pan_is_moving(void);
+void tilt_move_angle(int8_t tilt_angle);
+void tilt_set_speed(uint8_t tilt_speed);
+void tilt_stop(void);
+uint8_t tilt_is_moving(void);
+
void pan_tilt_process(void);
+#ifdef __cplusplus
+}
+#endif
+
#endif
diff --git a/motion/src/action.c b/motion/src/action.c
index 25c3ef1b2516093e040a5d50baf0bb7425c6ee2d..6029311220afbc1930d940bfbbb1ad8b71d54a0e 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -22,11 +22,9 @@ 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_servo_value(uint8_t servo_id);
-extern mm_module_t manager_get_servo_module(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);
+// external variables
+extern uint8_t manager_num_servos;
+extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// private variables
uint8_t action_finished;
@@ -61,7 +59,6 @@ uint8_t action_is_running;
// public functions
void action_init(void)
{
- //ram_write_byte(CM730_ACTION_STATUS,0x00);
action_finished=0x00;
action_stop=0x00;
action_next_index=0xFF;
@@ -93,11 +90,11 @@ void action_start_page(void)
bPlayRepeatCount = action_current_page.header.repeat;
action_next_index = 0x00;
- for(i=1;i<=MANAGER_MAX_NUM_SERVOS;i++)
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_get_servo_module(i)==MM_ACTION)
+ if(manager_servos[i].module==MM_ACTION)
{
- wpTargetAngle1024[i] = manager_get_servo_value(i);
+ wpTargetAngle1024[i] = manager_servos[i].current_value;
ipLastOutSpeed1024[i] = 0;
ipMovingAngle1024[i] = 0;
ipGoalSpeed1024[i] = 0;
@@ -147,9 +144,9 @@ void action_process(void)
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
{
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
{
- wpStartAngle1024[bID] = manager_get_servo_value(bID);
+ wpStartAngle1024[bID] = manager_servos[bID].current_value;
ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
}
}
@@ -162,7 +159,7 @@ void action_process(void)
wUnitTimeNum = wUnitTimeTotalNum - (wAccelStep << 1);
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
{
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
{
if( bpFinishType[bID] == NONE_ZERO_FINISH )
{
@@ -183,7 +180,7 @@ void action_process(void)
wUnitTimeNum = wAccelStep;
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
}
else if( bSection == POST_SECTION )
@@ -205,7 +202,7 @@ void action_process(void)
bSection = PRE_SECTION;
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
ipLastOutSpeed1024[bID] = 0;
}
@@ -270,7 +267,7 @@ void action_process(void)
////////// Jointº° ÆĶó¹ÌÅà °è»ê
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
{
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
{
// ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÃÀ¸·Î ±ËÀûÀ» °è»ê
ipAccelAngle1024[bID] = 0;
@@ -366,7 +363,7 @@ void action_process(void)
lDivider2 = 1;
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
{
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
{
lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; // *300/1024 * 1024/720 * 256 * 2
lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12;
@@ -395,10 +392,10 @@ void action_process(void)
{
for(bID=0;bID<MANAGER_MAX_NUM_SERVOS;bID++)
{
- if(manager_get_servo_module(bID)==MM_ACTION)
+ if(manager_servos[bID].module==MM_ACTION)
{
if( ipMovingAngle1024[bID] == 0 )
- manager_set_servo_value(bID,wpStartAngle1024[bID]);
+ manager_servos[bID].current_value=wpStartAngle1024[bID];
else
{
if( bSection == PRE_SECTION )
@@ -406,11 +403,11 @@ void action_process(void)
iSpeedN = (short)(((long)(ipMainSpeed1024[bID] - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
ipAccelAngle1024[bID] = (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN >> 1)) * wUnitTimeCount * 144) / 15) >> 9);
- manager_set_servo_value(bID,wpStartAngle1024[bID] + ipAccelAngle1024[bID]);
+ manager_servos[bID].current_value=wpStartAngle1024[bID] + ipAccelAngle1024[bID];
}
else if( bSection == MAIN_SECTION )
{
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID])*wUnitTimeCount) / wUnitTimeNum));
+ manager_servos[bID].current_value=wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID])*wUnitTimeCount) / wUnitTimeNum);
ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
}
else // POST_SECTION
@@ -418,7 +415,7 @@ void action_process(void)
if( wUnitTimeCount == (wUnitTimeNum-1) )
{
// ½ºÅÜ ¸¶Ãö¸· ¿ÀÂ÷¸¦ ÃÙÀ̱âÀ§ÇØ ±×³É ¸ñÇ¥ À§Ä¡ °ªÀ» »ç¿ë
- manager_set_servo_value(bID,wpTargetAngle1024[bID]);
+ manager_servos[bID].current_value=wpTargetAngle1024[bID];
}
else
{
@@ -426,20 +423,20 @@ void action_process(void)
{
iSpeedN = (short int)(((long)(0 - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9));
+ manager_servos[bID].current_value=wpStartAngle1024[bID] + (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9);
}
else // NONE_ZERO_FINISH
{
// MAIN Section°ú µ¿ÀÃÇÃ°Ô ÀÛµ¿-µ¿ÀÃ
// step¿¡¼ ¾î¶²¼Âº¸´Â °¡°à ¾î¶² ¼Âº¸´Â ¼Â¾ßÇô »óȲÀÌ ¹ß»ýÇÒ ¼ö ÀÖÀ¸¹Ç·Î ÀÌ·¸°Ô ÇÒ ¼ö¹Û¿¡ ¾øÀ½
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum));
+ manager_servos[bID].current_value=wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
}
}
}
}
//set slopes
- manager_set_servo_slopes(bID, action_current_page.header.slope[bID]);
+ manager_servos[bID].slope=action_current_page.header.slope[bID];
}
}
}
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index d9735b5d3258a112bac9f12c23f573c2a6d00554..53084cb906f797585bd28fe9863443c096252cac 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -31,7 +31,7 @@ extern TActionPage action_current_page;
typedef struct
{
- uint8_t cc_slope;
+ uint8_t cw_slope;
uint8_t ccw_slope;
uint16_t position;
}dyn_send_data;
@@ -93,15 +93,16 @@ void manager_send_motion_command(void)
pdata[num*4+2] = target_pos&0xFF;
pdata[num*4+3] = (target_pos>>8)&0xFF;
- if(manager_servos[i].cc_slope == 0)
+ if(manager_servos[i].slope == 0)
+ {
pdata[num*4] = 32;
- else
- pdata[num*4] = 1<<(0x0F&manager_servos[i].cc_slope);
-
- if(manager_servos[i].ccw_slope == 0)
pdata[num*4+1] = 32;
+ }
else
- pdata[num*4+1] = 1<<(0x0F&manager_servos[i].ccw_slope);
+ {
+ pdata[num*4] = 1<<(0x0F&manager_servos[i].slope);
+ pdata[num*4+1] = 1<<(0x0F&manager_servos[i].slope);
+ }
packets[num].data_addr=(uint8_t *)&data[num];
num++;
@@ -111,19 +112,14 @@ void manager_send_motion_command(void)
dyn_master_sync_write(manager_num_servos,servo_ids,CW_COMP_SLOPE,4,packets);
}
-void manager_get_current_position(void)
+void manager_set_servo_slope(uint8_t servo_id, uint8_t slope)
{
- uint8_t i;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- if(manager_servos[i].module!=MM_NONE)
- manager_servos[i].current_value=get_current_position(i);
+ manager_servos[servo_id].slope=slope;
}
-void manager_set_servo_slopes(uint8_t servo_id, uint8_t slope)
-{
- manager_servos[servo_id].cc_slope=slope;
- manager_servos[servo_id].ccw_slope=slope;
+uint8_t manager_get_servo_slope(uint8_t servo_id)
+{
+ return manager_servos[servo_id].slope;
}
void manager_set_servo_value(uint8_t servo_id, uint16_t value)
@@ -141,6 +137,15 @@ mm_module_t manager_get_servo_module(uint8_t servo_id)
return manager_servos[servo_id].module;
}
+uint8_t manager_get_cw_servo_limit(uint8_t servo_id)
+{
+ return manager_servos[servo_id].cw_limit;
+}
+
+uint8_t manager_get_ccw_servo_limit(uint8_t servo_id)
+{
+ return manager_servos[servo_id].ccw_limit;
+}
void manager_loop(void)
{
if(manager_period_done()==0x01)
@@ -174,11 +179,15 @@ uint8_t manager_init(uint8_t num_servos)
/* initialize by default the information for all servos */
for(i=0;i<num;i++)
{
+ manager_servos[i].angle=0;
manager_servos[i].max_value=0;
manager_servos[i].min_value=0;
manager_servos[i].center_value=0;
manager_servos[i].current_value=0;
manager_servos[i].module=MM_NONE;
+ manager_servos[i].slope=32;
+ manager_servos[i].cw_limit=0;
+ manager_servos[i].ccw_limit=0;
}
/* scan the bus for all available servos */
@@ -192,21 +201,27 @@ uint8_t manager_init(uint8_t num_servos)
if(model==0x000C || model==0x012C)// standard AX-12 or AX12W
{
enable_servo(id);
+ manager_servos[id].angle=300;
manager_servos[id].max_value=1023;
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);
manager_servos[id].module=MM_ACTION;
+ manager_servos[id].slope=32;
manager_num_servos++;
}
else if(model==0x0C00)// Emmulated servos from the expansion board pan&tilt servos
{
enable_servo(id);
+ manager_servos[id].angle=300;
manager_servos[id].max_value=1023;
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);
manager_servos[id].module=MM_JOINTS;
+ manager_servos[id].slope=32;
manager_num_servos++;
}
}
diff --git a/motion/src/pan_tilt.c b/motion/src/pan_tilt.c
index a79e2fe96732526234218130d5ef55c10de06ee1..66f1ab55340aaec90197d597d212c5e1403d2075 100644
--- a/motion/src/pan_tilt.c
+++ b/motion/src/pan_tilt.c
@@ -1,51 +1,209 @@
#include "pan_tilt.h"
+#include "motion_cfg.h"
+#include "motion_manager.h"
-// external functions
-extern uint16_t manager_get_servo_value(uint8_t servo_id);
-extern mm_module_t manager_get_servo_module(uint8_t servo_id);
-extern void manager_set_servo_value(uint8_t servo_id, uint16_t value);
+// external variables
+extern uint8_t manager_num_servos;
+extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// private variables
-uint16_t pan_angle;
-uint16_t pan_speed;
+// pan variables
+uint32_t pan_target_angle;
+uint32_t pan_target_speed;
+uint32_t pan_current_angle;
+uint8_t pan_stoping;
+uint8_t pan_moving;
uint8_t pan_servo_id;
-uint16_t tilt_angle;
-uint16_t tilt_speed;
+// tilt variables
+uint32_t tilt_target_angle;
+uint32_t tilt_target_speed;
+uint32_t tilt_current_angle;
+uint8_t tilt_stoping;
+uint8_t tilt_moving;
uint8_t tilt_servo_id;
// public functions
void pan_tilt_init(uint8_t pan_id,uint8_t tilt_id)
{
+ // set serov ID's
pan_servo_id=pan_id;
tilt_servo_id=tilt_id;
+ /* initialize pan variables */
+ pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_target_speed=0;
+ pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_stoping=0x00;
+ pan_moving=0x00;
+ /* initialize tilt variables */
+ tilt_target_angle=manager_servos[tilt_servo_id].current_value<<7;
+ tilt_target_speed=0;
+ tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+ tilt_stoping=0x00;
+ tilt_moving=0x00;
}
-void pan_tilt_set_angles(int8_t pan_angle,uint8_t titl_angle)
+void pan_tilt_move_angles(int8_t pan_angle,uint8_t tilt_angle)
{
-
+ pan_move_angle(pan_angle);
+ tilt_move_angle(tilt_angle);
}
-void pan_set_angle(int8_t pan_angle)
+void pan_move_angle(int8_t pan_angle)
{
-
+ if(pan_servo_id!=0xFF)
+ {
+ if(pan_target_speed==0)
+ {
+ pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_moving=0x00;
+ }
+ else
+ {
+ pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_target_angle=((uint32_t)(((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle))+(uint32_t)manager_servos[pan_servo_id].center_value);
+ if(pan_target_angle>manager_servos[pan_servo_id].ccw_limit)
+ pan_target_angle=manager_servos[pan_servo_id].ccw_limit;
+ else if(pan_target_angle<manager_servos[pan_servo_id].cw_limit)
+ pan_target_angle=manager_servos[pan_servo_id].cw_limit;
+ pan_target_angle=pan_target_angle<<7;
+ pan_moving=0x01;
+ }
+ }
}
void pan_set_speed(uint8_t pan_speed)
{
+ pan_target_speed=(((uint32_t)pan_speed*(uint32_t)manager_servos[pan_servo_id].max_value)/((uint32_t)manager_servos[pan_servo_id].angle));
+}
+void pan_stop(void)
+{
+ pan_stoping=0x01;
}
-void tilt_set_angle(int8_t tilt_angle)
+uint8_t pan_is_moving(void)
{
+ return pan_moving;
+}
+void tilt_move_angle(int8_t tilt_angle)
+{
+ if(tilt_servo_id!=0xFF)
+ {
+ if(tilt_target_speed==0)
+ {
+ tilt_target_angle=manager_servos[tilt_servo_id].current_value<<7;
+ tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+ tilt_moving=0x00;
+ }
+ else
+ {
+ tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
+ tilt_target_angle=(((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle))+manager_servos[tilt_servo_id].center_value;
+ if(tilt_target_angle>manager_servos[tilt_servo_id].ccw_limit)
+ tilt_target_angle=manager_servos[tilt_servo_id].ccw_limit;
+ else if(tilt_target_angle<manager_servos[tilt_servo_id].cw_limit)
+ tilt_target_angle=manager_servos[tilt_servo_id].cw_limit;
+ tilt_target_angle=tilt_target_angle<<7;
+ tilt_moving=0x01;
+ }
+ }
}
-void tilt_set_speed(int8_t tilt_speed)
+void tilt_set_speed(uint8_t tilt_speed)
{
+ tilt_target_speed=(((uint32_t)tilt_speed*(uint32_t)manager_servos[tilt_servo_id].max_value)/((uint32_t)manager_servos[tilt_servo_id].angle));
+}
+void tilt_stop(void)
+{
+ tilt_stoping=0x01;
+}
+
+uint8_t tilt_is_moving(void)
+{
+ return tilt_moving;
}
void pan_tilt_process(void)
{
-
+ // pan control
+ if(pan_stoping==0x01)
+ {
+ pan_target_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
+ pan_target_speed=0;
+ pan_moving=0x00;
+ pan_stoping=0x00;
+ }
+ else
+ {
+ if(pan_current_angle<pan_target_angle)
+ {
+ pan_current_angle+=pan_target_speed;
+ if(pan_current_angle>=pan_target_angle)
+ {
+ pan_current_angle=pan_target_angle;
+ pan_moving=0x00;
+ }
+ }
+ else if(pan_current_angle>pan_target_angle)
+ {
+ if(pan_current_angle<pan_target_speed)
+ {
+ pan_current_angle=pan_target_angle;
+ pan_moving=0x00;
+ }
+ else
+ {
+ pan_current_angle-=pan_target_speed;
+ if(pan_current_angle<=pan_target_angle)
+ {
+ pan_current_angle=pan_target_angle;
+ pan_moving=0x00;
+ }
+ }
+ }
+ }
+ manager_servos[pan_servo_id].current_value=(pan_current_angle>>7);
+ // tilt control
+ if(tilt_stoping==0x01)
+ {
+ tilt_target_angle=manager_servos[tilt_servo_id].current_value;
+ tilt_current_angle=manager_servos[tilt_servo_id].current_value;
+ tilt_target_speed=0;
+ tilt_moving=0x00;
+ tilt_stoping=0x00;
+ }
+ else
+ {
+ if(tilt_current_angle<tilt_target_angle)
+ {
+ tilt_current_angle+=tilt_target_speed;
+ if(tilt_current_angle>=tilt_target_angle)
+ {
+ tilt_current_angle=tilt_target_angle;
+ tilt_moving=0x00;
+ }
+ }
+ else if(tilt_current_angle>tilt_target_angle)
+ {
+ if(tilt_current_angle<tilt_target_speed)
+ {
+ tilt_current_angle=tilt_target_angle;
+ tilt_moving=0x00;
+ }
+ else
+ {
+ tilt_current_angle-=tilt_target_speed;
+ if(tilt_current_angle<=tilt_target_angle)
+ {
+ tilt_current_angle=tilt_target_angle;
+ tilt_moving=0x00;
+ }
+ }
+ }
+ }
+ manager_servos[tilt_servo_id].current_value=(tilt_current_angle>>7);
}