adding option to change the angle of multiple servomotors

Author: inesmaria08

examples/servo/main.c

@@ -10,21 +10,32 @@ int main(void) {
     printf("There is no available servo\n");
     return -1;
   }
-
+  returncode_t result = RETURNCODE_EOFF;
   uint16_t angle = 0;
+  uint16_t index = 0; // the first index available.
+
+  if (libtock_current_servo_angle(index, &angle) == RETURNCODE_ENODEVICE) {
+    printf("\n The index number is bigger than the available servomotors\n");
+    return -1;
+  }
 
   // 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++) {
-    if (libtock_servo_angle(i) == RETURNCODE_SUCCESS) {
+    // `result` stores the returned code received from the driver.
+    result = libtock_servo_angle(index, i);
+    if (result == RETURNCODE_SUCCESS) {
       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\n", angle);
+      if (libtock_current_servo_angle(index, &angle) == RETURNCODE_SUCCESS) {
+        printf("\nThe current angle is: %d", angle);
       } else {
-        printf("\nThe servomotor is OFF\n");
+        printf("\n This servo cannot return it's angle.\n");
       }
-    } else {
-      printf("\nThe angle could not be changed\n");
+    } else if (result == RETURNCODE_EINVAL){
+      printf("\nThe angle you provided exceeds 360 degrees\n");
+      return -1;
+    } else if (result == RETURNCODE_FAIL){
+      printf("\nThe angle could not be changed. The provided angle exceeds the servo's limit\n");
       return -1;
     }
   }

libtock/interface/syscalls/servo_syscalls.c

@@ -5,14 +5,14 @@ bool libtock_servo_exists(void) {
   return driver_exists(DRIVER_NUM_SERVO);
 }
 
-returncode_t libtock_servo_angle(uint32_t angle) {
-  syscall_return_t cval = command(DRIVER_NUM_SERVO, 1, angle, 0);
+returncode_t libtock_servo_angle(uint16_t index, uint16_t angle) {
+  syscall_return_t cval = command(DRIVER_NUM_SERVO, 1, index, angle);
   return tock_command_return_novalue_to_returncode(cval);
 }
 
-returncode_t libtock_current_servo_angle(uint16_t* angle) {
+returncode_t libtock_current_servo_angle(uint16_t index, uint16_t* angle) {
   uint32_t value     = 0;
-  syscall_return_t r = command(DRIVER_NUM_SERVO, 2, 0, 0);
+  syscall_return_t r = command(DRIVER_NUM_SERVO, 2, index, 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);

libtock/interface/syscalls/servo_syscalls.h

@@ -12,10 +12,10 @@ extern "C" {
 bool libtock_servo_exists(void);
 
 // Change the angle.
-returncode_t libtock_servo_angle(uint32_t angle);
+returncode_t libtock_servo_angle(uint16_t index, uint16_t angle );
 // Requests the current angle from the servo.
-returncode_t libtock_current_servo_angle(uint16_t* angle);
+returncode_t libtock_current_servo_angle(uint16_t index, uint16_t* angle);
 
 #ifdef __cplusplus
 }
-#endif
+#endif