Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 296 → Rev 297

/shark/trunk/ports/servo/servo.c
634,6 → 634,69
}
 
/* 1000.0000:0100.0000 */
int servo_get_servo_levels(void)
{
struct timespec current_time;
unsigned char b;
int err;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x40;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
err = com_receive(servo_port);
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return 0;
else
return -1;
}
 
/* 1000.0000:1000.0000 */
int servo_store_levels(void)
{
struct timespec current_time;
unsigned char b;
int err;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x80;
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;
}
 
int servo_set_max_angle(int servo, int angle_sec)
{