WebSVN - shark - Path Comparison - /shark/trunk/ports/ Rev 284 and /shark/trunk/ports/ Rev 285

Ignore whitespace Rev 284 → Rev 285

 /shark/trunk/ports/servo/servo.c
 ,47 → 103,172
     }
 }
+/* 1000.0100:bbbb.bbbb */
 int servo_set_RS232_baudrate(int baud)
 {
+  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 = 0x84;
+  err = com_send(SERVO_PORT, b);
+  err = com_receive(SERVO_PORT);
+  if (err != (int)(b)) timer_expired = 1;
-  return 0;
+  b = baud & 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.0101 */
 int servo_get_RS232_baudrate(void)
 {
+  struct timespec current_time;
+  unsigned char b;
+  int err,res;
+  timer_expired = 0;
+  kern_gettime(&current_time);
+  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
+  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
+  b = 0x85;
+  err = com_send(SERVO_PORT, b);
+  err = com_receive(SERVO_PORT);
+  if (err != (int)(b)) timer_expired = 1;
+  res = com_receive(SERVO_PORT);
+  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  if (!timer_expired)
+    return res;
+  else
+    return -1;
-  return 0;
 }
+/* 1000.0110 */
 int servo_store_RS232_buadrate(void)
 {
+  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 = 0x86;
+  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;
-  return 0;
 }
+/* 1000.1000:llll.llll */
 int servo_set_period(int period)
 {
+  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 = 0x88;
+  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);
+  if (err != (int)(b)) timer_expired = 1;
+  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  if (!timer_expired)
+    return 0;
+  else
+    return -1;
-  return 0;
 }
+/* 1000.1001 */
 int servo_get_period(void)
 {
+  struct timespec current_time;
+  unsigned char b;
+  int err,res;
+  timer_expired = 0;
+  kern_gettime(&current_time);
+  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
+  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
+  b = 0x89;
+  err = com_send(SERVO_PORT, b);
+  err = com_receive(SERVO_PORT);
+  if (err != (int)(b)) timer_expired = 1;
+  res = com_receive(SERVO_PORT);
+  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  if (!timer_expired)
+    return res*8;
+  else
+    return -1;
-  return 0;
 }
+/* 1000.1010 */
 int servo_store_period(void)
 {
+  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 = 0x8A;
+  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;
-  return 0;
 }
 /* 1000.1100 */
 ,10 → 323,138
 }
+/* 1000.0000:0000.0mmm */
+int servo_turn_off(int servo)
+{
+  struct timespec current_time;
+  unsigned char b;
+  int err;
+  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 = 0x80;
+  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);
+  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:1000.0mmm */
+int servo_turn_on(int servo)
+{
+  struct timespec current_time;
+  unsigned char b;
+  int err;
+  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 = 0x80;
+  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);
+  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:0001.0000 */
+int servo_turn_off_all(void)
+{                                                                                                                              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 = 0x10;
+  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:0001.0001 */
+int servo_turn_on_all(void)
+{
+  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 = 0x11;
+  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;
+}
 /* 0000.0ppp:0000.vvvv:vvvv.vvvv */
 int servo_set_angle(int servo, int angle)
 {
   struct timespec current_time;
   unsigned char b;
   int err;

Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 284 → Rev 285