Improved shake, added debug mode

duck/duck.c

@@ -5,19 +5,26 @@
 #include <stdlib.h>
 #include "../libduck/libduck.h"
 
-#define SHAKE_ANGLE 45
+#define SHAKE_ANGLE 90
 #define SHAKE_T 300
 
-void shake(int motor, int number)
+void shake(int motor, int number, int min, int max, int delay)
 {
-     for (int j=0; j < number; j++) {
-	  duck_set_position(motor, -SHAKE_ANGLE);
-	  duck_delay(SHAKE_T);
-	  duck_set_position(motor, SHAKE_ANGLE);
-	  duck_delay(2 * SHAKE_T);
-	  duck_set_position(motor, 0);
-	  duck_delay(SHAKE_T);
+     int delay1 = (delay * (-min)) / (max - min);
+     int delay2 = delay - delay1;
+     
+     duck_set_position(motor, min);
+     duck_delay(delay1);
+     
+     for (int j=0; j < number-1; j++) {
+	  duck_set_position(motor, max);
+	  duck_delay(delay);
+	  duck_set_position(motor, min);
+	  duck_delay(delay);
      }
+
+     duck_set_position(motor, 0);
+     duck_delay(delay2);
 }
 
 int main(int argc, char **argv)
@@ -26,8 +33,11 @@ int main(int argc, char **argv)
      const char *fname = DEFAULT_DUCK_FNAME;
      char buf[100];
      
-     while ((option=getopt(argc, argv, "t:")) != -1) {
+     while ((option=getopt(argc, argv, "t:d")) != -1) {
 	  switch (option) {
+	  case 'd':
+	       duck_debug_mode = 1;
+	       break;
 	  case 't':
 	       strncpy(buf, optarg, 100);
 	       fname = buf;
@@ -60,13 +70,19 @@ int main(int argc, char **argv)
 	  } else if (strcmp(argv[i], "headshake") == 0) {
 	       if (argv[i+1]) {
 		    int numshakes = atoi(argv[i+1]);
-		    shake(MOTOR_1, numshakes);
+		    shake(MOTOR_3, numshakes, -90, 90, 250);
 		    i++;
 	       }
 	  } else if (strcmp(argv[i], "nod") == 0) {
 	       if (argv[i+1]) {
 		    int numnods = atoi(argv[i+1]);
-		    shake(MOTOR_2, numnods);
+		    shake(MOTOR_2, numnods, -90, 0, 250);
+		    i++;
+	       }
+	  } else if (strcmp(argv[i], "facepalm") == 0) {
+	       if (argv[i+1]) {
+		    int numnods = atoi(argv[i+1]);
+		    shake(MOTOR_2, numnods, -90, 0, 250);
 		    i++;
 	       }
 	  }