Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 288 → Rev 289

/shark/trunk/ports/servo/servo.c
61,7 → 61,6
 
#define SERVO_TIMEOUT 100000 /* us */
 
#define SERVO_PORT 2
#define SERVO_SPEED 38400
#define SERVO_PARITY NONE
#define SERVO_LEN 8
69,6 → 68,7
 
int timer_expired = 0;
int timeout_event;
int servo_port;
 
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
 
195,11 → 195,12
 
}
 
int servo_open(void)
int servo_open(int port)
{
int err;
 
err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
servo_port = (unsigned)(port);
err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
return err;
 
209,7 → 210,7
{
int err;
 
err = com_close(SERVO_PORT);
err = com_close(servo_port);
 
return err;
 
260,19 → 261,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;
294,10 → 295,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);
321,8 → 322,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);
334,7 → 335,7
 
}
 
/* 1000.1000:llll.llll */
/* 1000.1010:llll.llll */
int servo_set_period(int period)
{
struct timespec current_time;
346,14 → 347,14
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;
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);
378,10 → 379,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);
392,7 → 393,7
 
}
 
/* 1000.1010 */
/* 1000.1000 */
int servo_store_period(void)
{
struct timespec current_time;
404,9 → 405,9
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;
if (timeout_event != NIL) kern_event_delete(timeout_event);
430,10 → 431,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);
 
457,8 → 458,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);
486,13 → 487,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);
520,13 → 521,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);
550,13 → 551,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);
581,13 → 582,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);
614,18 → 615,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);
652,11 → 653,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);
 
683,11 → 684,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);