Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 2 of 2

Thread: How Come My Servo Won't Activate? RMS to Servo Map for Animatronics.

  1. #1
    Junior Member
    Join Date
    May 2020

    How Come My Servo Won't Activate? RMS to Servo Map for Animatronics.

    Hi. Here is the source code. Teensy 3.5. Note that my basic servo test sketch (not included here) does work. Also note that I have a mosfet switch before the servo. Finally, note that the serial monitor produces good results for what I am trying to do.

    So: The sonar, audio, mosfet, and serial monitor numbers are all dandy. The servo does nothing at all.

    I have tried messing with the sample time, its not that either.

    Must be a line of code which is missing something. i'm not a great coder.

    Thank you! SirEdward

    #include <Audio.h>
    #include <Wire.h>
    #include <SPI.h>
    #include <SD.h>
    #include <SerialFlash.h>
    #include <Servo.h>

    // Configuration
    // Connections
    #define SERVO_PIN 10 // Jaw servo
    #define PING_PIN 5 // ping / echo for sonar distance sensor
    #define ECHO_PIN 6
    #define MOSFETPin 9 // mostfet for servo power control

    // How long until the ping returns to consider it a "trigger"
    #define SONAR_THRESHOLD 900 // microseconds.
    // Speed of sound in air is about 343 m/s, so this is about 15 cm (~6 in), or a 30cm round trip.
    // Quick calculators:
    // const SONAR_THRESHOLD (148 * 6) // ~inches
    // const SONAR_THRESHOLD (58 * 15) // ~cm

    // Servo range configuration
    #define JAW_OPEN 70
    #define JAW_CLOSED 0

    // Length of time for `loop()` to run. 1000/25 Hz (40ms) should be pretty fluid
    #define SAMPLE_TIME 300 // ms

    // Sound file to play when sonar triggers
    #define SAMPLE_FILE "01.wav"

    // Delay playback to compensate for the jaw servo's lag.
    #define SERVO_DELAY 50 // ms

    // To play with this, import the below block into

    // playSdWav1 will use the built-in sd card slot, assumed to have a fat32 filesystem.
    // If you're using some other way to attach SD, or you'd like to use in-memory or
    // whatever, there are other options.

    #include <Audio.h>
    #include <Wire.h>
    #include <SPI.h>
    #include <SD.h>
    #include <SerialFlash.h>

    // GUItool: begin automatically generated code
    AudioPlaySdWav playSdWav1; //xy=329,371
    AudioMixer4 mixer1; //xy=548,386
    AudioEffectDelay delay1; //xy=749,424
    AudioOutputAnalog dac1; //xy=951,421
    AudioAnalyzePeak peak1; //xy=973,360
    AudioConnection patchCord1(playSdWav1, 0, mixer1, 0);
    AudioConnection patchCord2(playSdWav1, 1, mixer1, 1);
    AudioConnection patchCord3(mixer1, delay1);
    AudioConnection patchCord4(delay1, 0, peak1, 0);
    AudioConnection patchCord5(delay1, 1, dac1, 0);
    // GUItool: end automatically generated code

    #define SDCARD_MOSI_PIN 11 // not actually used
    #define SDCARD_SCK_PIN 13

    Servo jaw;

    // Entirely optional. For stable sample management. `elapsedMillis` is a teensy type that
    // automatically counts up, so it's useful for making sure if you want 25Hz, you _get_ 25Hz.
    elapsedMillis timing;

    int ping() {
    digitalWrite(PING_PIN, LOW);
    digitalWrite(PING_PIN, HIGH);
    digitalWrite(PING_PIN, LOW);
    if (pulseIn(ECHO_PIN, HIGH, SONAR_THRESHOLD) != 0) {
    return HIGH;
    return LOW;

    uint8_t mosfet_state = LOW;
    void set_mosfet(uint8_t state) {
    if (state != mosfet_state) {
    Serial.print("Setting MOSFET to ");
    Serial.println(state == LOW ? "LOW" : "HIGH");
    mosfet_state = state;
    digitalWrite(MOSFETPin, mosfet_state);

    void setup() {
    Serial.println("initializing audio memory");

    // Set up audio memory. You need at least 1 block for each 2.9 ms of delay, plus
    // a minimum of 8 blocks for the player.
    AudioMemory((int) (ceil(SERVO_DELAY / 2.9) + 8));
    Serial.println("initializing pins");
    pinMode(PING_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    pinMode(MOSFETPin, OUTPUT);
    delay1.delay(0, 0);
    delay1.delay(1, SERVO_DELAY);

    mixer1.gain(0, 1.0);
    mixer1.gain(1, 1.0);
    mixer1.gain(2, 0);
    mixer1.gain(3, 0);

    Serial.println("initializing SD access");
    if (!(SD.begin(SDCARD_CS_PIN))) {
    // stop here, but print a message repetitively
    while (1) {
    Serial.println("Unable to access the SD card");

    void loop_state_waiting() {
    if (mosfet_state == HIGH) {
    Serial.println("playback ended; returning to sleep.");
    if (ping() == HIGH) {
    Serial.println("sonar ping! Starting sound/animation.");

    void loop_state_playing() {
    if (peak1.available()) {
    float level = peak1.readPeakToPeak();
    int new_servo_pos = (int) (level * (JAW_OPEN - JAW_CLOSED) / 2.0 + JAW_CLOSED);
    Serial.print("Level: ");
    Serial.print("; servo: ");

    void loop() {
    // resets the counter
    timing = 0;
    if (!playSdWav1.isPlaying()) {
    } else {
    // Only delay for the remaining time in the sample.
    delay(SAMPLE_TIME - timing);
    Last edited by siredward; 06-01-2020 at 05:25 AM. Reason: forgot to specify model number

  2. #2
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    New York
    This looks like a duplicate post to: I answered on that one

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts