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

Thread: Servo Motor Won't Activate - Can You Help? Animatronics

  1. #1
    Junior Member
    Join Date
    May 2020
    Posts
    12

    Servo Motor Won't Activate - Can You Help? Animatronics

    Hi. Here is the source code. Teensy 3.5.
    Basic servo test sketch (not included here) does work. Also note that I have a mosfet switch before the servo which is known to be working (indicator LED)
    The serial monitor produces good results for what I am trying to do.

    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. Can you help?

    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 https://www.pjrc.com/teensy/gui/

    // 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_CS_PIN BUILTIN_SDCARD
    #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);
    delayMicroseconds(2);
    digitalWrite(PING_PIN, HIGH);
    delayMicroseconds(10);
    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.begin(9600);
    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");
    jaw.attach(SERVO_PIN);
    pinMode(PING_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    pinMode(MOSFETPin, OUTPUT);
    delay1.delay(0, 0);
    delay1.delay(1, SERVO_DELAY);
    delay1.disable(2);
    delay1.disable(3);
    delay1.disable(4);
    delay1.disable(5);
    delay1.disable(6);
    delay1.disable(7);

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

    Serial.println("initializing SD access");
    SPI.setMOSI(SDCARD_MOSI_PIN);
    SPI.setSCK(SDCARD_SCK_PIN);
    if (!(SD.begin(SDCARD_CS_PIN))) {
    // stop here, but print a message repetitively
    while (1) {
    Serial.println(SD.begin(SDCARD_CS_PIN));
    Serial.println("Unable to access the SD card");
    delay(500);
    }
    }
    }

    void loop_state_waiting() {
    if (mosfet_state == HIGH) {
    Serial.println("playback ended; returning to sleep.");
    }
    set_mosfet(LOW);
    if (ping() == HIGH) {
    Serial.println("sonar ping! Starting sound/animation.");
    set_mosfet(HIGH);
    playSdWav1.play(SAMPLE_FILE);
    delay(5);
    }
    }

    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(level);
    Serial.print("; servo: ");
    Serial.println(new_servo_pos);
    jaw.write(new_servo_pos);
    }
    }

    void loop() {
    // resets the counter
    timing = 0;
    if (!playSdWav1.isPlaying()) {
    loop_state_waiting();
    } else {
    loop_state_playing();
    }
    // Only delay for the remaining time in the sample.
    delay(SAMPLE_TIME - timing);
    }

  2. #2
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,472
    Try changing your SERVO_PIN number to something like pin 3. Just a guess here but looking at the audio adapter board pin out: https://www.pjrc.com/store/teensy3_audio.html pin 10 is also a default pin for the Audio board SD card. May be getting confused.

  3. #3
    Senior Member+ KurtE's Avatar
    Join Date
    Jan 2014
    Posts
    7,153
    Sorry I am not sure...

    Do you get the Serial prints to your debug monitor showing the servo should have it's position set? Do they look reasonable?

    I am guessing you have Audio board connected to T3.5? Audio board has pin 10 going to CS of the SD Card, which has built-in 3.3v PU resistor. Not sure
    if that is interfering.

    Might try different pin.

    If it were me, I would run some simple test like a modified version of the servo sweep program to see if it works. Would need to modify probably to enable power to servo, plus change pin, and maybe range if servo is in something that has limited range, but at least then you know wiring and power and servo are working properly


    But here is a formatted version of your program which makes it easier to follow (use the # button in toolbar)
    Code:
    #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 https://www.pjrc.com/teensy/gui/
    
    // 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_CS_PIN BUILTIN_SDCARD
    #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);
      delayMicroseconds(2);
      digitalWrite(PING_PIN, HIGH);
      delayMicroseconds(10);
      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.begin(9600);
      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");
      jaw.attach(SERVO_PIN);
      pinMode(PING_PIN, OUTPUT);
      pinMode(ECHO_PIN, INPUT);
      pinMode(MOSFETPin, OUTPUT);
      delay1.delay(0, 0);
      delay1.delay(1, SERVO_DELAY);
      delay1.disable(2);
      delay1.disable(3);
      delay1.disable(4);
      delay1.disable(5);
      delay1.disable(6);
      delay1.disable(7);
    
      mixer1.gain(0, 1.0);
      mixer1.gain(1, 1.0);
      mixer1.gain(2, 0);
      mixer1.gain(3, 0);
    
      Serial.println("initializing SD access");
      SPI.setMOSI(SDCARD_MOSI_PIN);
      SPI.setSCK(SDCARD_SCK_PIN);
      if (!(SD.begin(SDCARD_CS_PIN))) {
        // stop here, but print a message repetitively
        while (1) {
          Serial.println(SD.begin(SDCARD_CS_PIN));
          Serial.println("Unable to access the SD card");
          delay(500);
        }
      }
    }
    
    void loop_state_waiting() {
      if (mosfet_state == HIGH) {
        Serial.println("playback ended; returning to sleep.");
      }
      set_mosfet(LOW);
      if (ping() == HIGH) {
        Serial.println("sonar ping! Starting sound/animation.");
        set_mosfet(HIGH);
        playSdWav1.play(SAMPLE_FILE);
        delay(5);
      }
    }
    
    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(level);
        Serial.print("; servo: ");
        Serial.println(new_servo_pos);
        jaw.write(new_servo_pos);
      }
    }
    
    void loop() {
      // resets the counter
      timing = 0;
      if (!playSdWav1.isPlaying()) {
        loop_state_waiting();
      } else {
        loop_state_playing();
      }
      // Only delay for the remaining time in the sample.
      delay(SAMPLE_TIME - timing);
    }

  4. #4
    Senior Member PaulStoffregen's Avatar
    Join Date
    Nov 2012
    Posts
    22,294
    Servo.h and AudioOutputAnalog don't play nice with each other on Teensy 3.x. They both want the PDB timer.

    The simplest solution is to use PWMServo.h rather than Servo.h. But PWMServo only supports certain pins. Details here:

    https://www.pjrc.com/teensy/td_libs_Servo.html

    The downside to PWMServo is other PWM pins controlled by the same FTM timer are forced to 50 Hz. I don't see any use of analogWrite() in your code, so that's probably not an issue for you. But if it is, the other not-as-simple solution is to edit Servo. It can use the LPTMR timer rather than the PDB timer, if the change the right defines in the code. Some libraries depend on LPTMR (FreqCount & Entropy are the two I remember) so editing Servo this way would make it incompatible with those, but it will then work together with AudioOutputAnalog which must use the PDB timer.

  5. #5
    Junior Member
    Join Date
    May 2020
    Posts
    12
    Thank you Paul! This is extremely useful moving forwards. I will have a look at the options; would like to avoid cutting traces on my custom pcb; will reconvene with my programmer buddies.

    Additionally, once the servo is worked out, will want to incorporate FastLED, adding a new layer of timing circuit traffic.

    I have heard that it is possible to "partition" on the Cortex Core - is this useage of separate timers what is meant by that?

    EE tech learning to program here; next time I make with Teensy 4.0 and above..

    Thanks for your time

Posting Permissions

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