Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 322 → Rev 323

/shark/trunk/ports/servo/servo.c
506,16 → 506,14
 
}
 
/* 1000.0000:0000.0mmm */
/* 1000.0000:0000.Mmmm */
int servo_turn_off(int servo)
{
 
struct timespec current_time;
unsigned char b;
int err;
if (servo > 7) return -1;
int err;
if (servo > 15) return -1;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
526,7 → 524,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (servo & 0x07);
b = 0x00 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
548,7 → 546,7
unsigned char b;
int err;
if (servo > 7) return -1;
if (servo > 15) return -1;
timer_expired = 0;
kern_gettime(&current_time);
560,7 → 558,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x10 | (servo & 0x07);
b = 0x10 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
576,7 → 574,8
 
/* 1000.0000:0010.0000 */
int servo_turn_off_all(void)
{ struct timespec current_time;
{
struct timespec current_time;
unsigned char b;
int err;
635,8 → 634,8
}
 
/* 1000.0000:0011.0000:mmmm.mmmm */
int servo_set_levels(int mask)
/* 1000.0000:0101.000M:mmmm.mmmm */
int servo_set_levels(int bank,int mask)
{
struct timespec current_time;
unsigned char b;
652,7 → 651,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x30;
b = 0x50 | (0x01 & bank);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
671,8 → 670,8
}
 
/* 1000.0000:0100.0000 */
int servo_get_levels(void)
/* 1000.0000:0100.000M */
int servo_get_levels(int bank)
{
struct timespec current_time;
unsigned char b;
688,7 → 687,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x40;
b = 0x40 | (0x01 & bank);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
750,7 → 749,7
 
}
 
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle_sec(int servo, int angle_sec)
{
struct timespec current_time;
757,7 → 756,7
unsigned char b;
int err, angle_tick;
 
if (servo > 7) return -1;
if (servo > 15) return -1;
 
if (angle_sec > servo_table[servo].max_angle_sec ||
angle_sec < servo_table[servo].min_angle_sec) return -1;
770,7 → 769,7
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x00 | (servo & 0x07);
b = 0x00 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
794,7 → 793,7
 
}
 
/* 0010.0ppp */
/* 0010.Pppp */
int servo_store_default_position(int servo)
{
struct timespec current_time;
801,7 → 800,7
unsigned char b;
int err;
 
if (servo > 7) return -1;
if (servo > 15) return -1;
timer_expired = 0;
kern_gettime(&current_time);
808,7 → 807,7
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x20 | (servo & 0x07);
b = 0x20 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
822,7 → 821,7
 
}
 
/* 0001.0ppp */
/* 0001.Pppp */
int servo_get_angle_sec(int servo)
{
struct timespec current_time;
829,7 → 828,7
unsigned char b;
int err,res,data;
if (servo > 7) return -1;
if (servo > 15) return -1;
timer_expired = 0;
kern_gettime(&current_time);
836,7 → 835,7
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x10 | (servo & 0x07);
b = 0x10 | (servo & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
864,7 → 863,7
unsigned char b;
int err,res;
if (port > 4) return -1;
if (port > 7) return -1;
timer_expired = 0;
kern_gettime(&current_time);
/shark/trunk/ports/servo/include/servo.h
32,7 → 32,7
int servo_turn_off_all(void);
int servo_turn_on_all(void);
 
int servo_set_levels(int mask);
int servo_set_levels(int bank, int mask);
int servo_get_levels();
int servo_store_levels();