aboutsummaryrefslogtreecommitdiffstats
path: root/examples/test-servo.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'examples/test-servo.cpp')
-rw-r--r--examples/test-servo.cpp152
1 files changed, 152 insertions, 0 deletions
diff --git a/examples/test-servo.cpp b/examples/test-servo.cpp
new file mode 100644
index 0000000..b6b8cd5
--- /dev/null
+++ b/examples/test-servo.cpp
@@ -0,0 +1,152 @@
+/*
+ * Basic Servo library test program.
+ *
+ * Setup:
+ *
+ * - Connect a potentiometer to POT_PIN (default pin 15)
+ * - Connect an oscilloscope to SERVO_PIN1 (default pin 5) and
+ * SERVO_PIN2 (default pin 6).
+ * - Connect a serial monitor to SerialUSB
+ *
+ * The potentiometer controls the target angle for each of two Servo
+ * objects, one with angles in [-90, 90], and another in [0, 180].
+ * Servo pulse width range is [1000, 2000].
+ *
+ * Serial2 will tell you what inputs it's giving to each servo object,
+ * and some information it gets back. Pressing the button
+ * detaches/reattaches the Servo objects.
+ *
+ * Tests you should perform:
+ *
+ * - Check calculated pulse widths for each servo's target angle
+ * - Check that calculated pulse widths match actual pulse widths
+ * - Check that the period of the pulse train is roughly 20 ms
+ * - Check that the pulses stop when detached, and resume when reattached
+ * - Check that Servo::write() and Servo::read() round-trip properly
+ *
+ * This file is released into the public domain.
+ */
+
+#include <stdio.h>
+
+#include "wirish.h"
+
+#include "libraries/Servo/Servo.h"
+
+#define POT_PIN 15
+
+#define MIN_PW 1000
+#define MAX_PW 2000
+
+#define SERVO_PIN1 5
+#define MIN_ANGLE1 0
+#define MAX_ANGLE1 180
+
+#define SERVO_PIN2 6
+#define MIN_ANGLE2 (-90)
+#define MAX_ANGLE2 90
+
+Servo servo1;
+Servo servo2;
+
+#define BUF_SIZE 100
+char buf[BUF_SIZE];
+
+#define print_buf(fmt, ...) do { \
+ snprintf(buf, BUF_SIZE, fmt, __VA_ARGS__); \
+ Serial2.println(buf); } while (0)
+
+int averageAnalogReads(int);
+void attach();
+void detach();
+
+void setup() {
+ pinMode(POT_PIN, INPUT_ANALOG);
+ pinMode(BOARD_BUTTON_PIN, INPUT);
+ pinMode(BOARD_LED_PIN, OUTPUT);
+
+ Serial2.begin(9600);
+
+ servo1.attach(SERVO_PIN1, MIN_PW, MAX_PW, MIN_ANGLE1, MAX_ANGLE1);
+ servo2.attach(SERVO_PIN2, MIN_PW, MAX_PW, MIN_ANGLE2, MAX_ANGLE2);
+
+ ASSERT(servo1.attachedPin() == SERVO_PIN1);
+ ASSERT(servo2.attachedPin() == SERVO_PIN2);
+}
+
+void loop() {
+ delay(250);
+ toggleLED();
+
+ if (isButtonPressed()) {
+ if (servo1.attached()) detach();
+ else attach();
+ }
+
+ if (!servo1.attached()) return;
+
+ int32 average = averageAnalogReads(250);
+ int16 angle1 = (int16)map(average, 0, 4095, MIN_ANGLE1, MAX_ANGLE1);
+ int16 angle2 = (int16)map(average, 0, 4095, MIN_ANGLE2, MAX_ANGLE2);
+
+ print_buf("pot reading = %d, angle 1 = %d, angle 2 = %d.",
+ average, angle1, angle2);
+
+ servo1.write(angle1);
+ servo2.write(angle2);
+
+ int16 read1 = servo1.read();
+ int16 read2 = servo2.read();
+
+ print_buf("write/read angle 1: %d/%d, angle 2: %d/%d",
+ angle1, read1, angle2, read2);
+
+ ASSERT(abs(angle1 - read1) <= 1);
+ ASSERT(abs(angle2 - read2) <= 1);
+
+ print_buf("pulse width 1: %d, pulse width 2: %d",
+ servo1.readMicroseconds(), servo2.readMicroseconds());
+
+ Serial2.println("\n--------------------------\n");
+}
+
+int32 averageAnalogReads(int n) {
+ uint64 total = 0;
+
+ for (int i = 0; i < n; i++) {
+ total += analogRead(POT_PIN);
+ }
+
+ return (int32)(total / n);
+}
+
+void attach() {
+ Serial2.println("attaching");
+ servo1.attach(SERVO_PIN1);
+ servo2.attach(SERVO_PIN2);
+ ASSERT(servo1.attachedPin() == SERVO_PIN1);
+ ASSERT(servo2.attachedPin() == SERVO_PIN2);
+}
+
+void detach() {
+ Serial2.println("detaching");
+ servo1.detach();
+ servo2.detach();
+ ASSERT(!servo1.attached());
+ ASSERT(!servo2.attached());
+}
+
+// Force init to be called *first*, i.e. before static object allocation.
+// Otherwise, statically allocated objects that need libmaple may fail.
+__attribute__((constructor)) void premain() {
+ init();
+}
+
+int main(void) {
+ setup();
+
+ while (true) {
+ loop();
+ }
+ return 0;
+}