CalebDueck
Member
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.
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;
}