diff options
Diffstat (limited to 'examples/test-servo.cpp')
-rw-r--r-- | examples/test-servo.cpp | 152 |
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; +} |