Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 281 → Rev 282

/shark/trunk/ports/servo/servo.c
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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
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);
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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
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);
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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
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);
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;
 
}