Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 289 → Rev 288

/shark/trunk/ports/servo/servo.c
61,6 → 61,7
 
#define SERVO_TIMEOUT 100000 /* us */
 
#define SERVO_PORT 2
#define SERVO_SPEED 38400
#define SERVO_PARITY NONE
#define SERVO_LEN 8
68,7 → 69,6
 
int timer_expired = 0;
int timeout_event;
int servo_port;
 
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
 
195,12 → 195,11
 
}
 
int servo_open(int port)
int servo_open(void)
{
int err;
 
servo_port = (unsigned)(port);
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
return err;
 
210,7 → 209,7
{
int err;
 
err = com_close(servo_port);
err = com_close(SERVO_PORT);
 
return err;
 
261,19 → 260,19
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x86 | (w & 0x01);
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
 
b = spbrg;
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
com_close(servo_port);
com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
com_close(SERVO_PORT);
com_open(SERVO_PORT, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
if (!timer_expired)
return 0;
295,10 → 294,10
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x85;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port);
res = com_receive(SERVO_PORT);
if (timeout_event != NIL) kern_event_delete(timeout_event);
322,8 → 321,8
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x84;
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
335,7 → 334,7
 
}
 
/* 1000.1010:llll.llll */
/* 1000.1000:llll.llll */
int servo_set_period(int period)
{
struct timespec current_time;
347,14 → 346,14
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x8A;
err = com_send(servo_port, b);
err = com_receive(servo_port);
b = 0x88;
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
b = period/8 & 0xFF;
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
379,10 → 378,10
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x89;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port);
res = com_receive(SERVO_PORT);
if (timeout_event != NIL) kern_event_delete(timeout_event);
393,7 → 392,7
 
}
 
/* 1000.1000 */
/* 1000.1010 */
int servo_store_period(void)
{
struct timespec current_time;
405,9 → 404,9
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x88;
err = com_send(servo_port, b);
err = com_receive(servo_port);
b = 0x8A;
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);
431,10 → 430,10
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x8C;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port);
res = com_receive(SERVO_PORT);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
458,8 → 457,8
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);
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);
487,13 → 486,13
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (servo & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
521,13 → 520,13
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
b = 0x08 | (servo & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
551,13 → 550,13
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
b = 0x10;
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
582,13 → 581,13
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
b = 0x11;
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
615,18 → 614,18
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x00 | (servo & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | ((angle >> 8) & 0x0F);
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
 
b = 0x00 | (angle & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
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);
653,11 → 652,11
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x08 | (servo & 0x07);
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port) << 8;
res |= com_receive(servo_port);
res = com_receive(SERVO_PORT) << 8;
res |= com_receive(SERVO_PORT);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
684,11 → 683,11
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
 
b = 0x40 | (port & 7);
err = com_send(servo_port, b);
err = com_receive(servo_port);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
res = com_receive(servo_port) << 8;
res |= com_receive(servo_port);
res = com_receive(SERVO_PORT) << 8;
res |= com_receive(SERVO_PORT);
 
if (timeout_event != NIL) kern_event_delete(timeout_event);