36,6 → 36,7 |
*/ |
|
#include <ll/sys/ll/ll-instr.h> |
#include "kernel/kern.h" |
#include "servo.h" |
|
#define THR 0 |
51,12 → 52,14 |
|
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
#define TIMEOUT 100000 /* us */ |
#define SERVO_TIMEOUT 100000 /* us */ |
|
#define SERVO_PORT 1 |
|
int timer_expired = 0; |
unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
void set_timer_expired(void) |
void set_timer_expired(void *arg) |
{ |
|
timer_expired = 1; |
157,25 → 160,100 |
|
} |
|
|
/* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle(int servo, int angle) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err,timeout_event; |
|
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x00 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
|
b = 0x00 | ((angle >> 8) & 0x0F); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
|
b = 0x00 | (angle & 0xFF); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
|
kern_event_delete(timeout_event); |
|
return 0; |
|
} |
|
/* 0000.1ppp */ |
int servo_get_angle(int servo) |
{ |
|
return 0; |
struct timespec current_time; |
unsigned char b; |
int err,res,timeout_event; |
|
if (servo > 7) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
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); |
|
kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|
/* 0100:0aaa */ |
int servo_get_analog(int port) |
{ |
|
return 0; |
struct timespec current_time; |
unsigned char b; |
int err,res,timeout_event; |
|
if (port > 4) return -1; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x40 | (port & 7); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
res = com_receive(SERVO_PORT) << 8; |
res |= com_receive(SERVO_PORT); |
|
kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|