Teensy Freezing when Connected to Vesc & Motor

Hello,
I am using a Teensy to drive a ESC through PWM signal.
It works fine when the Teensy is disconnected or connected just to the VESC with no motor attached.
However when I attach the motor it causes the teensy to freeze, sometimes turning on random outputs.

The motor system is on a seperate power supply they only share a ground and a signal line from the teensy.

I am wondering if there is a software or hardware fix that is recommended or if I should be doing anything differently?

Here is the main code.

Code:
#include "Nunchuk.h"
#include <Servo.h>
#include "Constants.h"
#include "HelperFunctions.h"

using namespace std;


/***    Control Options    ***/
bool stopBeforeClose = false;


/***    Global Variables    ***/
bool valveOpened = false;
bool estopPressed = true;

Nunchuk redNun(SDAPIN, SCLPIN, 1); //1 = debugMode, 0 = no debug
Servo vescPPM; //not actually PPM, it is PWM



void setup() {
  Serial.begin(19200);
  //***** OUTPUTS ******//
  vescPPM.attach(VESCPIN);
  vescPPM.writeMicroseconds(MOTOR_STOP);
  pinMode(GREENPIN, OUTPUT);
  pinMode(REDPIN, OUTPUT);
  
  pinMode(SHOULDER_EXTEND, OUTPUT);
  pinMode(SHOULDER_RETRACT, OUTPUT);
  pinMode(ELBOW_EXTEND, OUTPUT);
  pinMode(ELBOW_RETRACT, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  //pinMode(SPARE_RELAY, OUTPUT);
  
  //pinMode(SPARE, OUTPUT);

  //***** INPUTS *****//
  redNun.startup();
  //ANALOG INPUT PRESSURE_SENSOR
  pinMode(BUTTON1, INPUT);
  pinMode(BUTTON2, INPUT);
  
  pinMode(ENABLE_BUTTON, INPUT);

  pinMode(ESTOP_STATUS, INPUT);
  attachInterrupt(ESTOP_STATUS, estopPress, RISING);

  blinkLED(200, GREENPIN);
  blinkLED(200, REDPIN);
  blinkLED(200, GREENPIN);
  Serial.println("Setup Complete");
}

void loop() {
  //Read Nunchuk Values
  if(estopPressed){
    estopRecover();
  }
  redNun.refresh();
  motorJoyAndButtonControl();
  valveControl();

  //heartbeat
  digitalWrite(LED_BUILTIN, (millis()/250)%2);

  delay(10);
}

/***    ISR    ***/
void estopPress(){
  estopPressed = true;
}

/***    Program Functions    ***/
void estopRecover(){
  Serial.println("STOPPED");
  allOff(vescPPM);
  digitalWrite(REDPIN, HIGH);
  //waits till estop is low and enable button is low
  while(digitalRead(ESTOP_STATUS)==HIGH || digitalRead(ENABLE_BUTTON)==HIGH){
    //wait till estop released (active low)
    while(digitalRead(ESTOP_STATUS)==HIGH);
  }
  digitalWrite(REDPIN, LOW);
  Serial.println("ENABLED");
  for(int i = 0; i<3; ++i){
    blinkLED(200, GREENPIN);
    betterDelay(100);
  }
  estopPressed = false;
  betterDelay(200);
}

void motorJoyAndButtonControl(){
  double distance = sqrt(sq(redNun.analogX - redNun.xZero())+sq(redNun.analogY - redNun.yZero()));
  int motorSpeed = MOTOR_STOP;
  int joySpeed = MOTOR_STOP;
  
  if(psiRead()>3000){
    Serial.println("Pressure > 3000");
    vescPPM.writeMicroseconds(MOTOR_LOW);
  }
  else{
    if(distance > MOTOR_ON_THRESHOLD){
      joySpeed = (MOTOR_MAX-MOTOR_STOP)*(distance) / MAX_JOY + MOTOR_STOP; 
    }
    if(redNun.zButton == HIGH){
      if(redNun.cButton == LOW){
        motorSpeed = max(joySpeed, MOTOR_LOW);
      } else{
        motorSpeed = max(joySpeed, MOTOR_HIGH);
      }
    }else{
      motorSpeed = joySpeed;
    }
    vescPPM.writeMicroseconds(motorSpeed);
    Serial.println("PPM: ");
    Serial.println(motorSpeed);
  }
  
}

void motorJoyControl() {
  if(psiRead()>3000){
    Serial.println("Pressure > 3000");
    vescPPM.writeMicroseconds(MOTOR_LOW);
  }
  else{
    double distance = sqrt(sq(redNun.analogX - redNun.xZero())+sq(redNun.analogY - redNun.yZero()));
    if(distance > MOTOR_ON_THRESHOLD){
      int motorSpeed = (MOTOR_MAX-MOTOR_STOP)*(distance) / MAX_JOY + MOTOR_STOP;
      vescPPM.writeMicroseconds(motorSpeed);  
    }
    else{
      vescPPM.writeMicroseconds(MOTOR_STOP);
    }
    Serial.println("Distance: ");
    Serial.println(distance);
  }
}

void motorButtonControl() {
  if(redNun.zButton==HIGH){
    if(redNun.cButton==LOW){
      //z pressed, c not pressed
      vescPPM.writeMicroseconds(MOTOR_LOW);
    }
    else if(valveOpened && redNun.cButton==HIGH){
      //z and c pressed
      vescPPM.writeMicroseconds(MOTOR_HIGH);
    }
  }
  else{
    vescPPM.writeMicroseconds(MOTOR_STOP);
  }
}

void valveControl() {
  bool shoulderOpen = false;
  bool elbowOpen = false;
  
  //Turn on Valves if Joystick beyond threshold
  if(redNun.analogX < redNun.xZero() - VALVE_ON_THRESHOLD){
    if(shoulderOpen = false){
      Serial.print("SHOULDER_RETRACT ");
    }
    digitalWrite(SHOULDER_EXTEND, LOW); //ensures only one valve is open
    digitalWrite(SHOULDER_RETRACT, HIGH);
    shoulderOpen = true;
  } 
  else if(redNun.analogX > redNun.xZero() + VALVE_ON_THRESHOLD){
    if(shoulderOpen = false){
      Serial.print("SHOULDER_EXTEND ");
    }
    digitalWrite(SHOULDER_RETRACT, LOW);
    digitalWrite(SHOULDER_EXTEND, HIGH);
    shoulderOpen = true;
  }
  else{
    if(vescPPM.readMicroseconds()>MOTOR_STOP && valveOpened && shoulderOpen && !elbowOpen && stopBeforeClose){
      vescPPM.writeMicroseconds(MOTOR_STOP);  
      betterDelay(50);
    }
    digitalWrite(SHOULDER_EXTEND, LOW);
    digitalWrite(SHOULDER_RETRACT, LOW);
    shoulderOpen = false;
  }
  //ELBOW
  if(redNun.analogY < redNun.yZero() - VALVE_ON_THRESHOLD){
    if(elbowOpen = false){
      Serial.print("ELBOW_RETRACT ");
    }
    digitalWrite(ELBOW_EXTEND, LOW);
    digitalWrite(ELBOW_RETRACT, HIGH);
    elbowOpen = true;
  } 
  else if(redNun.analogY > redNun.yZero() + VALVE_ON_THRESHOLD){
    if(elbowOpen = false){
      Serial.print("ELBOW_-EXTEND ");
    }
    digitalWrite(ELBOW_RETRACT, LOW);
    digitalWrite(ELBOW_EXTEND, HIGH);
    elbowOpen = true;
  }
  else{
    if(vescPPM.readMicroseconds()>MOTOR_STOP && valveOpened && elbowOpen && !shoulderOpen && stopBeforeClose){
      vescPPM.writeMicroseconds(MOTOR_STOP);  
      betterDelay(50);
    }
    digitalWrite(ELBOW_EXTEND, LOW);
    digitalWrite(ELBOW_RETRACT, LOW);
    elbowOpen = false;
  }

  valveOpened = shoulderOpen || elbowOpen;
}
 

Attachments

  • HelperFunctions.cpp
    1.3 KB · Views: 31
  • ironSpiderControls.ino
    5.6 KB · Views: 27
  • Nunchuk.h
    660 bytes · Views: 31
Which Teensy? i.e. what voltage is the PWM? For instance is the ESC 5V and the Teensy 3.3V?

I suspect there will be some heavy interference involved due to the high currents involved - cable
layout is very important for something like this.
 
Teensy Used: Teensy 3.5
Vesc Used: https://www.makerx-tech.com/products/makerx-go-foc-hi100-75v-100a-base-on-75300-hardware-pre-sale
Motor Used: https://hobbyking.com/en_us/turnigy...me=hbk_live_products_analytics&___store=en_us

PWM Signal: 3V3 from teensy, level shifted to 5V for VESC (I had the system working for about an hour with just 3V3 out from the teensy but then had issues and added the level shifter which didn't solve anything)

Here are two pictures of how I have it set up.
https://photos.app.goo.gl/9dmu2cTY6rc6SghV9
https://photos.app.goo.gl/ttqzLX2T8dbR4yGK7

Currently I have an arduino nano as a middleman. The Nano can successfully drive the motor with PWM. It can also read the PWM from the teensy. However when I try to use both together the teensy freezes.
 
Here are two pictures of how I have it set up.

In the first picture I don't think I can see the ground-return for the PWM signal to the ESC - ideally
this would be on the same shielded cable, or twisted pair, to reduce EMI pickup on the sensitive logic signals.
Also the shorter the signal cable the better.

I suspect there are some voltage spikes flying around, either on the signal cable or the power supply
wiring connected to the Teensy that's responsible for the freezes. I'd put that 'scope on the various
connections and look for these. ESCs put out a _lot_ of EMI as I said.
 
I've narrowed it down to an issue with I2C.
I am able to output a sweep function no problem but I think the program is hanging somewhere in the I2C reading function.
I am using a Wii Nunchuk as input.
I am putting a bunch of serial.prints throughout to see where it is getting stuck
 
Have you used it with teensy?
I am using my own library and it is getting hung up at the request data portion.
Code:
#include "Nunchuk.h"
#include <Arduino.h>
using namespace std;

Nunchuk::Nunchuk (int sdaPin, int sclPin) {
  this->sdaPin = sdaPin;
  this->sclPin = sclPin;
}


Nunchuk::Nunchuk (int sdaPin, int sclPin, bool debug) {
  this->sdaPin = sdaPin;
  this->sclPin = sclPin;
  this->debugMode = debug;
}

void Nunchuk::startup() {
  Wire.begin(I2C_MASTER, 0x00, sclPin, sdaPin); //SCL 33 (yellow), SDA 34 (black)
  Wire.beginTransmission(0x52); // Wii Classic 7-bit I2C address is 0x52
  Wire.write(0xF0);
  Wire.write(0x55);
  Wire.endTransmission();
  Wire.beginTransmission(0x52);
  Wire.write(0xFB);
  Wire.write(0x00);
  Wire.endTransmission();

  //zero x and y
  int tempZero = -100;
  int lastZero = -200;
  while(tempZero != lastZero || (tempZero < 100 || tempZero > 150)){
    lastZero = tempZero;
    refresh();
    tempZero = analogX;
  }
  xZero_ = tempZero;
  
  tempZero = -100;
  lastZero = -200;
  while(tempZero != lastZero){
    lastZero = tempZero;
    refresh();
    tempZero = analogY;
  }
  yZero_ = tempZero;
  
  //if(debugMode){
    Serial.print("xZero: ");
    Serial.print(xZero_);
    Serial.print("yZero: ");
    Serial.print(yZero_);
  //}
  Serial.println("Nunchuk setup done");  
}

void Nunchuk::refresh() {
  // restart at the beginning of the address space
  delay(20);
  Wire.beginTransmission(0x52);
  Serial.println("transmited");
  Wire.write(0x00);
  Serial.println("wrote");
  Wire.endTransmission();
  Serial.println("transmit ended");
  delay(20); //if this delay not here then it dies
  // read values
  Serial.println("Going to request");
  Wire.requestFrom(0x52, 6);
  Serial.println("requested");
  while (Wire.available()) {
    for (int i = 0; i < 6; i++) {
      values[i] = Wire.receive();
    }
    
  Serial.println("received");


    // parse the controller data
    // analog controls
    analogX = values[0] & B11111111; 
    analogY = values[1] & B11111111; 
    acceanalogX = (values[2] << 2) | ((values[5] >> 2) & B00000011); 
    acceanalogY = (values[3] << 2) | ((values[5] >> 4) & B00000011); 
    acceanalogZ = (values[4] << 2) | ((values[5] >> 6) & B00000011);
    zButton = !((values[5] >> 0) & 1);
    cButton = !((values[5] >> 1) & 1);
    

  }
  if(debugMode){
    Serial.print("analogX ");
    Serial.print(analogX, DEC);
    Serial.print("; analogY ");
    Serial.print(analogY, DEC);
    Serial.print("; acceanalogX ");
    Serial.print(acceanalogX, DEC);
    Serial.print("; acceanalogY ");
    Serial.print(acceanalogY, DEC);
    Serial.print("; acceanalogZ ");
    Serial.print(acceanalogZ, DEC);
    Serial.print("; zButton ");
    Serial.print(zButton, DEC);
    Serial.print("cButton ");
    Serial.print(cButton, DEC);
    Serial.println(' ');
  } else{
    delay(1); //not sure why this is req'd
  }
}

int Nunchuk::xZero(){
  return xZero_;
}

int Nunchuk::yZero(){
  return yZero_;
}

int Nunchuk::returnX() {
  refresh();
  return analogX;
}
 
Code:
#ifndef NUNCHUK_H
#define NUNCHUK_H

#include "i2c_t3.h"

class Nunchuk {
  
  private:
  
    int values[6]; // store values read from nunchuk
    byte address = 52;
    int sdaPin;
    int sclPin;
    bool debugMode = 0;
    int xZero_;
    int yZero_;
    
  public:
    // the analog controls
    int analogX;
    int analogY;
    int acceanalogX;
    int acceanalogY;
    int acceanalogZ;
    int zButton;
    int cButton;

    Nunchuk(int sdaPin, int sclPin);
    Nunchuk(int sdaPin, int sclPin, bool debug);
    void startup();
    void refresh();
    int returnX();
    int xZero();
    int yZero();
};
#endif
 
Back
Top