Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 289 → Rev 290

/shark/trunk/ports/servo/servo.c
66,6 → 66,25
#define SERVO_LEN 8
#define SERVO_STOP 1
 
#define TICK_LEN 1600 /* us */
 
struct servo_data {
int min_angle_sec;
int max_angle_sec;
int delta_tick;
int zero_tick;
};
 
struct servo_data servo_table[] = {
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600}};
 
int timer_expired = 0;
int timeout_event;
int servo_port;
352,7 → 371,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = period/8 & 0xFF;
b = period/TICK_LEN/8 & 0xFF;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
387,7 → 406,7
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return res*8;
return (res*TICK_LEN*8);
else
return -1;
 
600,15 → 619,37
}
 
int servo_set_max_angle(int servo, int angle_sec)
{
 
servo_table[servo].max_angle_sec = angle_sec;
return 0;
 
}
 
int servo_set_min_angle(int servo, int angle_sec)
{
 
servo_table[servo].min_angle_sec = angle_sec;
return 0;
 
}
 
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle(int servo, int angle)
int servo_set_angle_sec(int servo, int angle_sec)
{
struct timespec current_time;
unsigned char b;
int err;
int err, angle_tick;
 
if (servo > 7) return -1;
 
if (angle_sec > servo_table[servo].max_angle_sec ||
angle_sec < servo_table[servo].min_angle_sec) return -1;
 
angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
 
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
619,12 → 660,12
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | ((angle >> 8) & 0x0F);
b = 0x00 | ((angle_tick >> 8) & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (angle & 0xFF);
b = 0x00 | (angle_tick & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
638,12 → 679,40
 
}
 
/* 0001.0ppp */
int servo_store_default_position(int servo)
{
struct timespec current_time;
unsigned char b;
int err;
 
if (servo > 7) return -1;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x10 | (servo & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return 0;
else
return -1;
 
}
 
/* 0000.1ppp */
int servo_get_angle(int servo)
int servo_get_angle_sec(int servo)
{
struct timespec current_time;
unsigned char b;
int err,res;
int err,res,data;
if (servo > 7) return -1;
661,8 → 730,12
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
data = (res - servo_table[servo].zero_tick) *
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
servo_table[servo].delta_tick;
 
if (!timer_expired)
return res;
return data;
else
return -1;
 
/shark/trunk/ports/servo/include/servo.h
16,8 → 16,8
 
/* Servo control */
 
#define DEG180 0xFFFFFF
#define SERVO_ANGLE(deg,min,sec) (((deg)*3600 + (min)*60 + (sec)) * DEG180 / 648000)
/* Convert angle (degree, minute, second -> second) */
#define ANGLE2SEC(deg,min,sec) ((deg)*3600 + (min)*60 + (sec))
 
int servo_turn_off(int servo);
int servo_turn_on(int servo);
24,9 → 24,14
int servo_turn_off_all(void);
int servo_turn_on_all(void);
 
int servo_set_angle(int servo, int angle);
int servo_get_angle(int servo);
int servo_set_max_angle_sec(int servo, int angle_sec);
int servo_set_min_angle_sec(int servo, int angle_sec);
 
int servo_set_angle_sec(int servo, int angle_sec);
int servo_get_angle_sec(int servo);
 
int servo_store_default_position(int servo);
 
/* Analog control */
 
int servo_get_analog(int port);