Hello,
I am using the Teensy 3.6 which comes with the MK66FX1M0. I use it to drive a piezo motor. The motor is driven in a pulsed state by using the internal Quadrature Decoder. Speed Control is given by a time interval and the motor turns off after the Encoder has read the needed pulses, and the FTM fires its ISR.
It works fine with the given MOD it is initialized with, but unfortunately I am completely unable to change this MOD once the uC is running. I tried many different found code bits, software triggers, even tried to re-initialize all registers but it is not working. I'm sure there is just one register which needs to be set or read, I am grasping at every straw available and would really appreciate some help.
The code for the Decoder-subclass:
#ifndef QUADDECODE_H
#define QUADDECODE_H
#include <stdint.h>
#include "mk20dx128.h"
#include "core_pins.h"
/*
* - MAGIE -
* https://forum.pjrc.com/threads/26803-Hardware-Quadrature-Code-for-Teensy-3-x?p=90774&viewfull=1#post...
*/
class QuadDecode_t {
public:
QuadDecode_t(int cts) {
// Pin Assignments
// FTM2 Pins
// K20 pin 41,42
// Bit 8-10 is Alt Assignment
PORTB_PCR18 = 0x00000612; //Alt6-QD_FTM2,FilterEnable,Pulldown
PORTB_PCR19 = 0x00000612; //Alt6-QD_FTM2,FilterEnable,Pulldown
//Set FTMEN to be able to write registers
FTM2_MODE=0x04; // Write protect disable - reset value
FTM2_MODE=0x05; // Set FTM Enable
// Set registers written in pins_teensy.c back to default
FTM2_CNT = 0;
FTM2_MOD = 0;
FTM2_C0SC =0;
FTM2_C1SC =0;
FTM2_SC = 0x40; //enable hardware trigger
FTM2_FILTER=0x22; // 2x4 clock filters on both channels
FTM2_CNTIN=0;
FTM2_MOD=cts; // Maximum value of counter
FTM2_CNT=0; // Updates counter with CNTIN
FTM2_QDCTRL=0b11000001; // Quadrature control
// Filter enabled, QUADEN set
// Write Protect Enable
FTM2_FMS=0x40; // Write Protect, WPDIS=1
NVIC_ENABLE_IRQ(IRQ_FTM2);
}
void setCounter2( int16_t c ) {
FTM2_CNT = c;
}
int16_t getCounter2( ) {
int16_t c = FTM2_CNT;
return c;
}
void call_changeCTS(int count){
FTM2_MOD = count;
Serial.println(count);
FTM2_SYNC = 0x80;
}
};
#endif
And the Main Code:
#include "QuadDecode.h"
#include "AD9837.h"
#define pin_motor_direction 9 // for toggling the direction (left and right) of the motor
#define pin_enable_pushpull_converter 17 // to turn the push pull converters on and off
#define pin_test 20 // test pin for various debugging
const long interval = 200000; //Intervall in MIKROSEKUNDEN!
const int cts = 49; //cts pro Intervall - 1 weniger als angegeben! (!!!Schon vorher prüfen?!!!)
volatile bool motorTurning = false;
bool dutyCycleReached = true;
const double minfrequency = 49000;
double frequency = 54000;
double initialfrequency;
float dutyCycle = 0.25;
AD9837 ad9837(frequency*4);
QuadDecode_t QuadDecode(cts);
IntervalTimer motorTimer; //Intervaltimer des Motors
IntervalTimer dutyCycleTimer; //Hilfsintervall zur Überprüfung vom eingestellten Duty Cycle
elapsedMillis elapsedTime;
bool switched = true;
void setup() {
tone(2, 2000, 200);
// TURN OFF PUSH PULL CONVERTER
pinMode(pin_enable_pushpull_converter,OUTPUT);
digitalWrite(pin_enable_pushpull_converter, LOW);
pinMode(pin_test, OUTPUT);
digitalWrite(pin_test, LOW);
// SETUP MOTOR DIRECTION
pinMode(pin_motor_direction,OUTPUT); // set TOGGLE to OUTPUT
digitalWrite(pin_motor_direction, LOW); // set high for one motor direction
Serial.begin(115200);
delay(1000);
ad9837.setfrequency(frequency * 4);
// TURN ON PUSH PULL CONVERTER
digitalWrite(pin_enable_pushpull_converter, HIGH);
motorTurning = false;
while(!motorTurning){
if(frequency < minfrequency){
frequency = 49000;
}
frequency = frequency - 100;
ad9837.setfrequency(frequency * 4);
delay(300);
Serial.println(frequency);
}
digitalWrite(pin_enable_pushpull_converter, LOW);
frequency = frequency - 1000;
Serial.println(frequency);
ad9837.setfrequency(frequency * 4);
initialfrequency = frequency;
tone(2, 400, 200);
delay(200);
tone(2, 800, 200);
delay(200);
tone(2, 2000, 200);
Serial.println("Start");
digitalWrite(pin_enable_pushpull_converter, HIGH);
motorTimer.begin(motorLoop, interval);
dutyCycleTimer.begin(dutyCycleLoop, interval*dutyCycle);
elapsedTime = 0;
}
void loop() {
if(elapsedTime > 10000){
if(switched){
QuadDecode.call_changeCTS(49);
}
else{
QuadDecode.call_changeCTS(99);
}
switched = !switched;
elapsedTime = 0;
}
}
//ISR FUNKTIONEN
//ISR Counter-Overflow: Erforderliche Anzahl Flanken erreicht
void ftm2_isr(void) {
digitalWrite(pin_enable_pushpull_converter, LOW);
motorTurning = true;
// clear overflow flag
FTM2_SC&=0x7F;
digitalWrite(pin_test, LOW);
}
//Duty Cycle Überprüfung
void dutyCycleLoop(){
if(motorTurning) {
dutyCycleReached = true;
}
dutyCycleTimer.end();
}
//Motor Cycle Loop
void motorLoop(){
//falls Flanken NICHT erreicht!
if(!motorTurning){
Serial.println("Dreht nicht! Dreht zu langsam! Erreicht erforderliche Flanken nicht! Frequenz herabsetzen.");
frequency -= 250;
ad9837.setfrequency(frequency * 4);
}
//falls Duty Cycle nicht erreicht!
else if(!dutyCycleReached){
Serial.println("Dreht, aber Duty Cycle wurde nicht erreicht! Frequenz herabsetzen.");
frequency -= 250;
ad9837.setfrequency(frequency * 4);
}
//falls unter die Mindestfrequenz gerutscht - setze Duty Cycle rauf, nochmal von vorne!
if(frequency <= minfrequency){
Serial.println("Innerhalb des vorgegebenen Duty Cycle nicht möglich! Erlaube höheren Duty Cycle.");
frequency = initialfrequency;
dutyCycle += 0.1;
ad9837.setfrequency(frequency * 4);
if(dutyCycle > 0.95){
while(1){
tone(2, 4500, 100);
delay(200);
}
}
}
//ansonsten: "ganz normaler" Durchlauf, setze Variablen zurück, "starte" Motor erneut
motorTurning = false;
dutyCycleReached = false;
dutyCycleTimer.begin(dutyCycleLoop, interval * dutyCycle);
digitalWrite(pin_enable_pushpull_converter, HIGH);
digitalWrite(pin_test, HIGH);
}
Apparently one needs to look into the PWM registers to update although no PWM is used...
setting the LDOK bit in FTM_PWMLOAD after writing to MOD does the job.
the updated ISR routine
void ftm2_isr(void) {
if((FTM2_SC & FTM_SC_TOF) != 0){
FTM2_SC &= ~FTM_SC_TOF;
}
digitalWrite(pin_enable_pushpull_converter, LOW);
motorTurning = true;
digitalWrite(pin_test, LOW);
if(newMOD != 0){
Serial.println(newMOD);
FTM2_CNTIN = 0;
FTM2_MOD = newMOD;
FTM2_CNT = 0;
FTM2_PWMLOAD = 0x200;
newMOD = 0;
Serial.println("MOD should be changed.");
}
}
Hello Christopher,
Thank you for updating the answer. Do you have further questions?
Best Regards,
Sabina