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

Thread: Teensy Freezing when Connected to Vesc & Motor

  1. #1
    Junior Member
    Join Date
    Nov 2021
    Posts
    5

    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;
    }
    Attached Files Attached Files

  2. #2
    Senior Member
    Join Date
    Jul 2020
    Posts
    1,450
    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.

  3. #3
    Junior Member
    Join Date
    Nov 2021
    Posts
    5
    Teensy Used: Teensy 3.5
    Vesc Used: https://www.makerx-tech.com/products...dware-pre-sale
    Motor Used: https://hobbyking.com/en_us/turnigy-...___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.

  4. #4
    Senior Member
    Join Date
    Jul 2020
    Posts
    1,450
    Quote Originally Posted by CalebDueck View Post
    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.

  5. #5
    Junior Member
    Join Date
    Nov 2021
    Posts
    5
    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

  6. #6
    Senior Member
    Join Date
    Apr 2014
    Location
    Germany
    Posts
    9,569
    There is a great arduino nunchuck library somewhere. Used it often.

  7. #7
    Junior Member
    Join Date
    Nov 2021
    Posts
    5
    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;
    }

  8. #8
    Junior Member
    Join Date
    Nov 2021
    Posts
    5
    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

  9. #9
    Senior Member
    Join Date
    Apr 2014
    Location
    Germany
    Posts
    9,569
    Yup, with teensy.

  10. #10
    Senior Member BriComp's Avatar
    Join Date
    Apr 2014
    Location
    Cheltenham, UK
    Posts
    497

  11. #11
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    7,848

Posting Permissions

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