current_servo_angle implementation

Author: inesmaria08

examples/servo/main.c

@@ -1,18 +1,25 @@
+#include "../../libtock/interface/syscalls/servo_syscalls.h"
+#include <libtock-sync/services/alarm.h>
+#include <libtock/tock.h>
 #include <stdio.h>
 #include <stdlib.h>
 
-#include <libtock-sync/services/alarm.h>
-#include <libtock-sync/interface/servo.c>
-
 int main(void) {
   // Checks if the driver exists and, if not, returns -1.
-	if (!libtock_servo_exists()) {
+  if (!libtock_servo_exists()) {
     printf("There is no available servo\n");
     return -1;
   }
+
+  uint16_t angle;
+
   // Changes the angle of the servomotor from 0 to 180 degrees (waiting 0.1 ms between every change).
-  for (int i = 0; i<=180; i++) {
-    libtocksync_servo_angle(i);
+  for (int i = 0; i <= 180; i++) {
+    libtock_servo_angle(i);
     libtocksync_alarm_delay_ms(100);
+    // Verifies if the function successfully returned the current angle.
+    if (libtock_current_servo_angle(&angle) == RETURNCODE_SUCCESS) {
+      printf("the current angle is: %d", angle);
+    }
   }
 }

libtock-sync/interface/servo.c

@@ -9,3 +9,13 @@ returncode_t libtock_servo_angle(uint32_t angle) {
   syscall_return_t cval = command(DRIVER_NUM_SERVO, 1, angle, 0);
   return tock_command_return_novalue_to_returncode(cval);
 }
+
+returncode_t libtock_current_servo_angle(uint16_t* angle) {
+  uint32_t value     = 0;
+  syscall_return_t r = command(DRIVER_NUM_SERVO, 2, 0, 0);
+  // Converts the value returned by the servo, stored in `r`,
+  // and assigns it to `ret`.
+  returncode_t ret = tock_command_return_u32_to_returncode(r, &value);
+  *angle = (uint16_t)value;
+  return ret;
+}

libtock-sync/interface/servo.h

@@ -13,6 +13,8 @@ bool libtock_servo_exists(void);
 
 // Change the angle.
 returncode_t libtock_servo_angle(uint32_t angle);
+// Requests the current angle from the servo.
+returncode_t libtock_current_servo_angle(uint16_t* angle);
 
 #ifdef __cplusplus
 }