ServoDAC.ino

/****

Code for controlling Brushless DC motor with controller
DBLS-01S via Teensy 4.1.

Uses AMT-102V motor encoder to monitor shaft.  AB signals are sent to quadrature decoder LS7084N.
Decoder signals (CLK* and UP/DOWN*) are passed through a level shifter (SN74LVC245AN) and to the Teensy.

The CLK from the decoder triggers an interrupt on the teensy to count pulses, up or down as indicated.

The Teensy interacts with an Adafruit MCP4725 DAC breakout over I2C.  Its output voltage is connected to SV.

Commands:
set voltage VOLTAGE (sets voltage to specified value)
dir X (x = 0 or 1; sets FR to logic level high or low)
count (displays current count)
reset (resets count to 0)
runseq XXX YYY ZZZ (loop count, duty cycle, time; send given number of pulses where pulse length = time and spends duty cycle % high.)


set voltage 3.3
dir1
g010

*****/

#include <avr/io.h>
#include <avr/interrupt.h>

const int upDown = 32;
const int dirPin = 31;
const int enPin = 30;

volatile long counter = 0;
volatile long counter_buffer = 0;
volatile bool lockBuffer = false;

int ledPin = 13;



#include <Wire.h>

int previous = 0;

void setup() {
  pinMode(dirPin, OUTPUT);
  digitalWrite(dirPin, LOW);
  
  pinMode(enPin, OUTPUT);
  digitalWrite(enPin, LOW);
  
  pinMode(ledPin, OUTPUT);
  
  attachInterrupt(digitalPinToInterrupt(33), myInterrupt, FALLING);
  pinMode(upDown, INPUT);
  
  Wire.begin(); 
  Wire.beginTransmission(0x62);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();
    
  // Debugging output
  Serial.begin(9600);
  Serial.setTimeout(10);
  
}

uint8_t i=0;

void loop() {
  //int incomingByte;
  
  digitalWrite(ledPin, HIGH);   // set the LED on
  delay(10);                  // wait for a second
  digitalWrite(ledPin, LOW);    // set the LED off
  delay(10);                  // wait for a second
  //cli();
  //count_copy = overflow_count;
  //sei();
  
  lockBuffer = true;
  
  if (previous != counter_buffer) {
    Serial.println(counter_buffer);
    previous = counter_buffer;
    }
    
  //HWSERIAL.print(counter_buffer);
  lockBuffer = false;
  delay(10);

  lockBuffer = true;
  
  if (Serial.available() > 0) {
    //incomingByte = Serial.read();
    //Serial.print("USB received: ");
    //Serial.println(incomingByte, DEC);
    String teststr = Serial.readString();

    if (teststr.substring(0,1) == "g") {
      int cv;
      int msb;
      int lsb;
      String _counter = teststr.substring(1, teststr.length());
      _counter.trim(); 
      int _count = _counter.toInt() * 1000;
      Serial.println(_count);
      
      if (counter < _count){
        
        digitalWrite(dirPin, HIGH);
        //Start at 3.3V
        
        cv = int(3.0/3.3 * 4096);     //float dacfVal = (hvoltage / 10000.0) * 3950.0;
        msb = (cv & 0xF00) >> 8;
        lsb = cv & 255;
        
        Wire.beginTransmission(0x62);
        Wire.write(msb);
        Wire.write(lsb);
        Wire.endTransmission();
        digitalWrite(enPin, HIGH);
        
        while (counter < _count - 30000) {
          delay(0.1);
        }
        //Slow down to 0.5V
        cv = int(0.5/3.3 * 4096); 
        msb = (cv & 0xF00) >> 8;
        lsb = cv & 255;
        Wire.beginTransmission(0x62);
        Wire.write(msb);
        Wire.write(lsb);
        Wire.endTransmission(); 
        
        while (counter < _count - 1000) {
          delay(0.1);
        }
        digitalWrite(enPin, LOW);
      }

      else {
        //EXAMPLE g-15000 with counter at 0.
        digitalWrite(dirPin, 0);
        cv = int(3.0/3.3 * 4096);     //float dacfVal = (hvoltage / 10000.0) * 3950.0;
        msb = (cv & 0xF00) >> 8;
        lsb = cv & 255;
        Wire.beginTransmission(0x62);
        Wire.write(msb);
        Wire.write(lsb);
        Wire.endTransmission(); 
        digitalWrite(enPin, HIGH);
        //while 0 > -15000 + 10000; 0 still > -5000, so need to keep motor 3.3V
        while (counter > _count + 30000) {
          delay(0.1);
        }

        //Slow down to 0.5V
        cv = int(0.6/3.3 * 4096); 
        msb = (cv & 0xF00) >> 8;
        lsb = cv & 255;
        Wire.beginTransmission(0x62);
        Wire.write(msb);
        Wire.write(lsb);
        Wire.endTransmission(); 
        while (counter > _count + 1000) {
          delay(0.1);
        }
        digitalWrite(enPin, LOW);
        
      }
    }

    if (teststr.substring(0,6) == "runseq") {

      String _loopcount = teststr.substring(7, 10);
      String _dutycycle = teststr.substring(11, 14);
      String _time = teststr.substring(15,18);
      _loopcount.trim();
      _dutycycle.trim(); 
      _time.trim();
      Serial.println(_loopcount);
      Serial.println(_dutycycle);
      Serial.println(_time);
      int loopcount = _loopcount.toInt();
      int timer = _time.toInt();
      float dutycycle = _dutycycle.toFloat();
      Serial.println(loopcount);
      Serial.println(timer);
      Serial.println(dutycycle);
      Serial.println("Running sequence....");

      for (int i = 0; i <= loopcount; i++) {
        float hi = timer * dutycycle;
        float lo = timer - hi;
        digitalWrite(enPin, HIGH);
        delay(hi);
        digitalWrite(enPin, LOW);
        delay(lo);
        }
    }

    if (teststr.substring(0,1) == "z") {

      String _delay = teststr.substring(1, 8);
      
      _delay.trim();
      Serial.println(_delay);
      float dlay = _delay.toFloat();
      Serial.println(dlay, 5);
      Serial.println("Running Z....");
      digitalWrite(enPin, HIGH);
      delay(dlay);
      digitalWrite(enPin, LOW);
    }

    if (teststr.substring(0,1) == "y") {

      String _delay = teststr.substring(1, 8);
      
      _delay.trim();
      Serial.println(_delay);
      float dlay = _delay.toFloat();
      Serial.println(dlay, 5);
      Serial.println("Running y....");
      digitalWrite(enPin, HIGH);
      delay(dlay*10);
      digitalWrite(enPin, LOW);
    }


    if (teststr.substring(0,5) == "test1") {
      int current_counter = counter;

      for (int i = 0; i <= 100; i++) {
        float dutycycle = float(i)/100;
        float hi = 20/2 * dutycycle;
        float lo = 20 - hi;
        digitalWrite(enPin, HIGH);

        for (int j = 0; j <= 10; j++) {
          
          delay(hi/10.0);

          if (counter != current_counter){
            digitalWrite(enPin, LOW);
            break;
          }
        }
       
        if (counter != current_counter){
            digitalWrite(enPin, LOW);
            break;
          }

        
        digitalWrite(enPin, LOW);
        delay(lo);
        }

    }

/*
 Seq 2 sets enPin low
 */

    if (teststr.substring(0,4) == "seq2") {
      
      Serial.println("Running seq2");
      digitalWrite(enPin, LOW);
      
    }

/*
 Seq 3 sets enPin high
 
 */

    if (teststr.substring(0,4) == "seq3") {
      
      Serial.println("Running seq3");
      digitalWrite(enPin, HIGH);
      
    }

/*
 Seq 1 sends 10 pulses, 10 ms high, 10 ms low (50% duty cycle, 20 ms pulse) 
 
 */

    if (teststr.substring(0,4) == "seq1") {
      
      Serial.println("Running seq1");

      for (int i = 0; i <= 10; i++) {
        digitalWrite(enPin, HIGH);
        delay(10);
        digitalWrite(enPin, LOW);
        delay(10);
        }
    }

    if (teststr.substring(0,5) == "reset") {
      
      Serial.println("Reset");
      counter = 0;
      counter_buffer = 0;
    }

    if (teststr.substring(0,5) == "count") {
      
      Serial.println("counter");
      Serial.println(counter);
      
    }

    /*
    
    dir X where x = 0 or 1
    Sets motor signal FR hi or lo
     
     */

    if (teststr.substring(0,3) == "dir") {
      String dstring = teststr.substring(3, 4);
      dstring.trim(); 
      Serial.println(dstring);
      int dir = dstring.toInt();
      digitalWrite(dirPin, dir); 
    }


    /*
     
     Format set voltage VOLTAGE
     Reads from after space to end of string and convents to float 
     Value is sent to MCP4725 over I2C (Address 0x62).
      
     */

    if (teststr.substring(0,11) == "set voltage") {

      String vstring = teststr.substring(12, teststr.length());
      vstring.trim(); 
      Serial.println(vstring);
      float voltage = vstring.toFloat();
      Serial.println(voltage);
      
      int cv = int(voltage/3.3 * 4096);     //float dacfVal = (hvoltage / 10000.0) * 3950.0;
  
      Serial.println(cv);
  
      int msb = (cv & 0xF00) >> 8;
       
      Serial.println(msb);
      //int dacVal = int(dacfVal);
      //apply_hv = true;
  
      int lsb = cv & 255;
      Serial.println(lsb);
  
      Wire.beginTransmission(0x62);
      Wire.write(msb);
      Wire.write(lsb);
      Wire.endTransmission();
      }
  }
  lockBuffer = false;
}

void myInterrupt() {
  int val = 0;
  val = digitalRead(upDown);
  if (val == 1) {
    counter += 1;
  }
  else {
    counter -= 1;
  }
  
  if (lockBuffer == false) {
    counter_buffer = counter;  
  }
  
}