Controlling Multiple Servos with Teensy 3.6

Status
Not open for further replies.

KJS

New member
Hello all,

I've been working on a project for a while that uses a Teensy 3.6 to control four micro servos. See the code below for how I'm driving them, but I'm using the Servo library. When driving one servo at a time, it usually works fine (keyword usually), but sometimes when writing to multiple servos in sequence or sometimes to a single servo, the servos all stop moving and emit a high-pitch whine. Adding a delay between the servo.write commands makes it much more reliable, but this still fails on occasion. I'm using four HS55s, and a multimeter shows that they aren't overdrawing current. If anything, they're not drawing enough. A similar project worked fine on the SparkFun Pro Micro without issues, which runs at 5V. I'm not entirely sure what the issue is, but I was thinking either an issue with the interrupts in Servo causing issues with consecutive write commands, or perhaps that the servos need more the 3.3V for their PWM signal. PWMServo makes all sorts of awful things happen, with servos being driven to random positions and wiggling considerably before stalling. Any ideas on why the servos aren't moving reliably would be appreciated! I included the other libraries used, but have commented out most of the code that uses them for servo testing. I ommitted these comments from the code below to more easily read it.

Code:
#include <MPU9250.h>
#include <SPI.h>
#include <SD.h>
#include <SPIMemory.h>
#include <Servo.h>

#define servo0Pin 16
#define servo1Pin 17
#define servo2Pin 20
#define servo3Pin 21

//Declaring Servos
Servo servo0;
Servo servo1;
Servo servo2;
Servo servo3;

//Servo-associated constants
const float servo0Neutral = 70.00; // Neutral poing of servo0
const float servo1Neutral = 75.00; // Neutral poing of servo1
const float servo2Neutral = 100.00; // Neutral poing of servo2
const float servo3Neutral = 70.00; // Neutral poing of servo3
const float servoRange = 30.00; //Max and min servo range, used in constrain function
float rollRange = servoRange / 3;

void setup() {
  //Connect Servos
  //Attaching servos to their respective pins
  servo0.attach(servo0Pin);
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);

  //Test Servos
  ServoTest();
  }

void fins(float pitchIn, float yawIn, float rollIn) {

  float servo0Out = servo0Neutral - .707 * pitchIn + .707 * yawIn + rollIn;
  float servo1Out = servo1Neutral - .707 * pitchIn - .707 * yawIn + rollIn;
  float servo2Out = servo2Neutral + .707 * pitchIn - .707 * yawIn + rollIn;
  float servo3Out = servo3Neutral + .707 * pitchIn + .707 * yawIn + rollIn;

  servo0Out = constrain(servo0Out, servo0Neutral - servoRange, servo0Neutral + servoRange);
  servo1Out = constrain(servo1Out, servo1Neutral - servoRange, servo1Neutral + servoRange);
  servo2Out = constrain(servo2Out, servo2Neutral - servoRange, servo2Neutral + servoRange);
  servo3Out = constrain(servo3Out, servo3Neutral - servoRange, servo3Neutral + servoRange);

  servo0.write(servo0Out);
  //delay(20);
  servo1.write(servo1Out);
  //delay(20);
  servo2.write(servo2Out);
  //delay(20);
  servo3.write(servo3Out);
  //delay(20);

}

void ServoTest(){
  delay(5000);
  fins(0,0,0);
  servo0.write(servo0Neutral-servoRange);
  delay(500);
  servo0.write(servo0Neutral+servoRange);
  delay(500);
  servo0.write(servo0Neutral);
  delay(1000);
  
  servo1.write(servo1Neutral-servoRange);
  delay(500);
  servo1.write(servo1Neutral+servoRange);
  delay(500);
  servo1.write(servo1Neutral);
  delay(1000);
  

  servo2.write(servo2Neutral-servoRange);
  delay(500);
  servo2.write(servo2Neutral+servoRange);
  delay(500);
  servo2.write(servo2Neutral);
  delay(1000);


  servo3.write(servo3Neutral-servoRange);
  delay(500);
  servo3.write(servo3Neutral+servoRange);
  delay(500);
  servo3.write(servo3Neutral);
  delay(1000);

  
  fins(30,0,0);
  delay(500);
  fins(-30,0,0);
  delay(500);
  fins(0,0,0);
  delay(1000);
  
}
 
Status
Not open for further replies.
Back
Top