Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 314 → Rev 315

/shark/trunk/ports/servo/servo.c
66,7 → 66,7
#define SERVO_LEN 8
#define SERVO_STOP 1
 
#define TICK_LEN 1600 /* us */
#define TICK_LEN 1600 /* ns */
 
struct servo_data {
int min_angle_sec;
273,13 → 273,13
diff = abs(Baud20MhzData[i+1] - baud);
if (diff < mindiff) {
mindiff = diff;
spbrg = (unsigned char)(Baud20MhzData[i+1]);
spbrg = (unsigned char)(Baud20MhzData[i]);
w = 0;
}
diff = abs(Baud20MhzData[i+2] - baud);
if (diff < mindiff) {
mindiff = diff;
spbrg = (unsigned char)(Baud20MhzData[i+2]);
spbrg = (unsigned char)(Baud20MhzData[i]);
w = 1;
}
i += 3;
386,7 → 386,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = period/TICK_LEN/8 & 0xFF;
b = (period*1000)/TICK_LEN/8 & 0xFF;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
421,7 → 421,7
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return (res*TICK_LEN*8);
return (res*TICK_LEN/1000*8);
else
return -1;
 
634,8 → 634,44
}
 
/* 1000.0000:0011.0000:mmmm.mmmm */
int servo_set_levels(int mask)
{
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 = 0x80;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = 0x30;
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = (unsigned char)(mask & 0xFF);
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;
}
 
/* 1000.0000:0100.0000 */
int servo_get_servo_levels(void)
int servo_get_levels(void)
{
struct timespec current_time;
unsigned char b;
/shark/trunk/ports/servo/include/servo.h
32,6 → 32,10
int servo_turn_off_all(void);
int servo_turn_on_all(void);
 
int servo_set_levels(int mask);
int servo_get_levels();
int servo_store_levels();
 
int servo_set_max_angle_sec(int servo, int angle_sec);
int servo_set_min_angle_sec(int servo, int angle_sec);