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

Status
Not open for further replies.

siredward

Member
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);
}
 
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);
}
 
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.
 
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
 
Status
Not open for further replies.
Back
Top