Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 282 → Rev 283

/shark/trunk/ports/servo/servo.c
57,13 → 57,13
#define SERVO_PORT 1
 
int timer_expired = 0;
int timeout_event;
unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
 
void set_timer_expired(void *arg)
{
 
timeout_event = NIL;
timer_expired = 1;
 
}
unsigned com_read(unsigned port,unsigned reg)
146,18 → 146,56
 
}
 
/* 1000.1100 */
int servo_get_setup_switch(void)
{
struct timespec current_time;
unsigned char b;
int err,res;
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
return 0;
b = 0x8C;
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(SERVO_PORT);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return res;
else
return -1;
 
}
 
/* 1000.111s */
int servo_set_RC5_switch(int data)
{
return 0;
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 = 0x8E | (data & 0x01);
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.0ppp:0000.vvvv:vvvv.vvvv */
166,7 → 204,7
 
struct timespec current_time;
unsigned char b;
int err,timeout_event;
int err;
 
if (servo > 7) return -1;
 
178,21 → 216,24
b = 0x00 | (servo & 0x07);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) return -1;
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | ((angle >> 8) & 0x0F);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) return -1;
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (angle & 0xFF);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) return -1;
if (err != (int)(b)) timer_expired = 1;
 
kern_event_delete(timeout_event);
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
return 0;
if (!timer_expired)
return 0;
else
return -1;
 
}
 
199,10 → 240,9
/* 0000.1ppp */
int servo_get_angle(int servo)
{
 
struct timespec current_time;
unsigned char b;
int err,res,timeout_event;
int err,res;
if (servo > 7) return -1;
214,10 → 254,11
b = 0x08 | (servo & 0x07);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) return -1;
res = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(SERVO_PORT) << 8;
res |= com_receive(SERVO_PORT);
 
kern_event_delete(timeout_event);
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return res;
232,7 → 273,7
 
struct timespec current_time;
unsigned char b;
int err,res,timeout_event;
int err,res;
if (port > 4) return -1;
244,11 → 285,11
b = 0x40 | (port & 7);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) return -1;
if (err != (int)(b)) timer_expired = 1;
res = com_receive(SERVO_PORT) << 8;
res |= com_receive(SERVO_PORT);
 
kern_event_delete(timeout_event);
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
if (!timer_expired)
return res;