Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 323 → Rev 316

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