Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 315 → Rev 316

/shark/trunk/ports/servo/servo.c
67,6 → 67,7
#define SERVO_STOP 1
 
#define TICK_LEN 1600 /* ns */
#define TICK_LEN_PERIOD 51200 /* ns */
 
struct servo_data {
int min_angle_sec;
76,14 → 77,14
};
 
struct servo_data servo_table[] = {
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600},
{-324000, 324000, 1800, 1600}};
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600}};
 
int timer_expired = 0;
int timeout_event;
386,7 → 387,7
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
b = (period*1000)/TICK_LEN/8 & 0xFF;
b = 0xFF - ((period*1000)/TICK_LEN_PERIOD/8 & 0xFF);
err = com_send(servo_port, b);
err = com_receive(servo_port);
if (err != (int)(b)) timer_expired = 1;
421,7 → 422,7
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return (res*TICK_LEN/1000*8);
return ((0xFF - (unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
else
return -1;
 
696,7 → 697,7
if (timeout_event != NIL) kern_event_delete(timeout_event);
if (!timer_expired)
return 0;
return err;
else
return -1;
761,8 → 762,8
if (angle_sec > servo_table[servo].max_angle_sec ||
angle_sec < servo_table[servo].min_angle_sec) return -1;
 
angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec)) * 1000 / TICK_LEN;
 
timer_expired = 0;
kern_gettime(&current_time);
844,7 → 845,7
 
if (timeout_event != NIL) kern_event_delete(timeout_event);
 
data = (res - servo_table[servo].zero_tick) *
data = ((res*TICK_LEN/1000) - servo_table[servo].zero_tick) *
(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
servo_table[servo].delta_tick;
 
/shark/trunk/ports/servo/include/servo.h
46,6 → 46,8
 
/* Analog control */
 
#define MAX_ANALOG 0x03FF
 
int servo_get_analog(int port);
 
#endif