// HardwareQuadratureEncoderScaleV500.ino
/*Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in the
Software without restriction, including without limitation the rights to use, copy,
modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
and to permit persons to whom the Software is furnished to do so, subject to the
following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/* From decode.h by TLB https://forum.pjrc.com/threads/26803-Hardware-Quadrature-Code-for-Teensy-3-x?p=56036&viewfull=1#post56036
*
* HARDWARE QUADRATURE DECODER DETAILS
*
* ENCXA 3 PTA12 28 Input FTM1 7
* ENCXB 4 PTA13 29 Input FTM1 7
* ENCYA 32 PTB18 41 Input FTM2 6
* ENCYB 25 PTB19 42 Input FTM2 6
*
*Pin Control Registers PORTx_PCRn
* Bit 10-8 MUX Pin Mux Control 1-7 alternative
* Alternative 1 is GPIO
* Bit 4 PFE Passive Filter Enable 1 to enable
* Bit 1 PE Pull Enable 1 to enable
* Bit 0 PS Pull Select 0 is pulldown 1 is pullup
*
* The following registers are set in pins_teensy.c
* and need to be reset.
* FTM1_SC
* FTM1_CNT
* FTM1_MOD
* FTM1_C0SC
* FTM1_C1SC
*
* FTM1_CNT Counter value
* Bit 15-0 is counter value
* Writing any value updates counter with CNTIN
* FTM1_MOD Modulo (Max value)
* Bit 15-0 is counter value - set to 0xFFFF
* Write to CNT first
* FTM1_MODE Features Mode Selection
* Bit0 FTMEN FTM Enable - [WPDIS must be 1 to write]
* Bit2 WPDIS Write Protect to Disable
* Set WPDIS, then set FTMEN - must be set to access FTM regs
* FTM1_FMS Fault Mode Status
* Bit 6 WPEN Write Protect Enable
* Write 1 to clear WPDIS
* FTM1_FILTER
* Filter out pulses shorter than CHxFVALx 4 system clocks
* Bit 7-4 CH1FVAL for PHB
* Bit 3-0 CH0FVAL for PHA
* FTM1_QDCTRL Quadrature Decoder Control and Status
* Bit 7 PHAFLTREN Phase A Filter Enable
* Bit 6 PHBFLTREN Phase B Filter Enable
* Bit 5 PHAPOL Phase A Polarity
* Bit 4 PHBPOL Phase B Polarity
* Bit 3 QUADMODE Quadrature Decoder Mode
* 0 for Quadrature
* Bit 2 QUADIR Counting Direction
* Bit 1 TOFDIR Timer Overflow Dir
* 0 was set on bottom of counting
* 1 was set on top of counting
* Bit 0 QUADEN Quadrature Mode Enable
* 1 is quadrature mode enabled [WPDIS to write]
* FTM1_SC FTM1 Status and Control
* Bit 7 TOF Timer Overflow Flag
* Bit 6 TOIE Timer Overflow Interrupt Enable
* Bit 5 CPWMS Center Aligned PWM Select
* Resets to 0 (Write when WPDIS is 1)
* Rest of bits are 0 (Write when WPDIS is 1)
* FTM1_C0SC FTM1 Channel 0 Status and Control
* Set WPDIS before writing control bits
* Bit 7 CHF Channel Flag
* Channel event occured
* Read and write 0 to clear
* Bit 6 CHIE Channel Interrupt Enable
* Set for compare interrupt
* Bit 5 MSB Channel Mode Select B
* Set to 0
* Bit 4 MSA Channel Mode Select A
* Set to 1
* Bit 3:2 ELS Edge or Level Select
* Set to 0:0
* FTM1_COMBINE
* Set WPDIS to 1 before writing
* DECAPEN (Dual Edge Capture Enable)
* COMBINE (Combine Channels)
* Resets to all zero
* FTM1_C0V Channel 0 Value
* Channel Compare Value
* Set to 0x80 - halfway thru count
* FTM1_STATUS
* Duplicate of CHnF bit for all channels
* Bit 0 CH0F
*/
/* Simplified for one encoder connected to pins 3 and 4.
* based on code provided https://forum.pjrc.com/threads/26803-Hardware-Quadrature-Code-for-Teensy-3-x/page3
* Pin Assignments, 3, 4
* for documentation see https://www.pjrc.com/teensy/K20P64M72SF1RM.pdf page 227
* for ports see https://github.com/russellbarnes/Teensy-3.1-C-Example/blob/master/core_pins.h
* (but did not yet get ports 8 and 7 working, nor pullups but the encoder required debounce sircuit anyway
* https://hifiduino.wordpress.com/2010/10/21/arduino-code-for-buffalo-ii-dac-rotary-encoder-connections/
* wozzy 10/09/2016 Tested with: Arduino 1.6.12 with Teensyduino 1.31 beta 1 Hardware Teensy 3.6 @ 240MHz */
#include <Bounce2.h>
#define BUTTON_PIN 2 // toggles count step size
#define LED_PIN 29 // Red LED indicates low speed
#define LED_PIN2 30 // Green LED indicates high speed
bool ledState = HIGH; // RED LED initial state
bool led2State = LOW; // GREEN LED initial state
float count1InitialValue = 0.0; // encoder value at startup
float count1StepSize = 0.001; // encouder count stepsize at startup
float count1StepSizeSlow = 0.001; // low speed encoder step size
float count1StepSizeFast = 1.0; // high speed encoder step size
float count1StepSizeTurboBoost = 1.0; // turbo boost step initial value
float count1StepSizeTurboBoostValue = 10.0; // turbo boost step multiplier (activates on rapid turn rate of encoder)
float encoder1 = count1InitialValue; // sets stepsize to initial value defined above
int16_t encoder1ShiftSpeed = 15; // milliseconds threshold to activate turbo boost
int16_t count1Min = -25000; // the maximum count value continuing to turn encoder holds here
int16_t count1Max = 25000; // the minimum count value continuing to turn encoder holds here
int16_t pulse1Time; // time between pulses to determine turbo boost
int16_t oldPulse1Time;
int16_t newPulse1Time;
int16_t rawChangeCounts1; // total number of counts since last read (almost always -1 or +1)
int16_t changeCounts1; // total number of counts since last read (almost always -1 or +1)
int16_t oldrawEncoder1Counts16;
int16_t rawEncoder1Counts16; // 16-BIT hardware encoder count register (-32767 to +32767)
int16_t overFlows1 = 0;
int32_t buttonDownTime; // length of time in ms that button was depressed to toggle output enable
int32_t oldEncoder1Counts32;
int32_t encoder1Counts32; // 32-BIT encoder counts (-2,147,483,647 to +2,147,483,647)
int32_t changeCounts32; // total number of counts since last read (almost always -1 or +1)
bool TOFDIR; // Timer Overflow Direction ( 0 = Counting DOWN, 1 = Counting UP)
// Instantiate a Bounce object :
Bounce debouncer = Bounce();
void setup() {
// Setup the button with an internal pull-up :
pinMode(BUTTON_PIN,INPUT_PULLUP);
// After setting up the button, setup the Bounce instance :
debouncer.attach(BUTTON_PIN);
debouncer.interval(20);
// Setup the LEDS :
pinMode(LED_PIN,OUTPUT);
digitalWrite(LED_PIN,ledState);
pinMode(LED_PIN2,OUTPUT);
digitalWrite(LED_PIN2,led2State);
PORTA_PCR12 = 0b0000011100010010; //Alt7-QD_FTM1,FilterEnable,Pulldown, 712h = 0b011100010010
PORTA_PCR13 = 0x00000712; //Alt7-QD_FTM1,FilterEnable,Pulldown
//Set FTMEN to be able to write registers
FTM1_MODE=0x04; // Write protect disable - reset value
FTM1_MODE=0x05; // Set FTM Enable
// Set registers written in pins_teensy.c back to default
FTM1_CNT = 0;
FTM1_MOD = 0;
FTM1_C0SC =0;
FTM1_C1SC =0;
FTM1_SC = 0;
// Set registers to count quadrature
FTM1_FILTER=0x22; // 2x4 clock filters on both channels
FTM1_CNTIN=0;
FTM1_MOD=0xFFFF; // Maximum value of counter
FTM1_CNT=0; // Updates counter with CNTIN
bitRead(FTM1_SC,7);
FTM1_SC = 0b00000010; // BITS 0,1,2 = "PS" Prescaler (010 = /4) - use /4 for 4 quadrature edges / detent type encoders
FTM1_QDCTRL=0b00000001; // Quadrature control BIT 0 ="QUAD ENABLE", BIT 1 = "TOFDIR" (Read Only)
// BIT 2 = "QUADDIR" (Read Only), BIT 3 = "QUADMODE" (0=quadrature, 1=Count & Direction)
// BITS 4&5 ="PHAPOL" (0 = normal polarity, 1 = reverse polarity), BITS 6&7 = "PHAFLTREN", "PHBFLTREN"
// Write Protect Enable
FTM1_FMS=0x40; // Write Protect, WPDIS=1
Serial.begin(9600);
}
void loop() {
rawEncoder1Counts16 = FTM1_CNT;
if (rawEncoder1Counts16 != oldrawEncoder1Counts16) {
newPulse1Time = millis();
rawChangeCounts1 = rawEncoder1Counts16 - oldrawEncoder1Counts16;
oldrawEncoder1Counts16 = rawEncoder1Counts16;
if (bitRead(FTM1_SC,7)==1){
TOFDIR = bitRead(FTM1_QDCTRL,1);
bitWrite(FTM1_SC,7,0); // Need to write 0 to TOF to clear it
}
encoder1Counts32 = rawEncoder1Counts16 + (overFlows1 * 256);
//encoder1Counts32 = rawEncoder1Counts16 + (overFlows1 * 32768);
changeCounts1 = encoder1Counts32 - oldEncoder1Counts32;
oldEncoder1Counts32 = encoder1Counts32;
updateEncoder1();
}
// Update the Bounce instance :
debouncer.update();
// Call code if Bounce fell (transition from HIGH to LOW) :
if ( debouncer.fell() ) shiftSpeed();
if ( debouncer.rose() ) toggleOutput();
}
void updateEncoder1 (void) {
pulse1Time = (int16_t)(newPulse1Time - oldPulse1Time);
oldPulse1Time = newPulse1Time;
if(pulse1Time < encoder1ShiftSpeed){
count1StepSizeTurboBoost = count1StepSizeTurboBoostValue;
}
else{
count1StepSizeTurboBoost = 1.0;
}
encoder1 = encoder1 + (changeCounts1 * count1StepSize * count1StepSizeTurboBoost);
if (encoder1 <= count1Min) encoder1 = count1Min;
if (encoder1 >= count1Max) encoder1 = count1Max;
Serial.print("ENCODER1 = ");
Serial.println(encoder1,3);
}
void shiftSpeed(void){
buttonDownTime=millis();
// Toggle LED state :
ledState = !ledState;
digitalWrite(LED_PIN,ledState);
digitalWrite(LED_PIN2,!ledState);
if(ledState){
count1StepSize = count1StepSizeSlow;
}
else{
count1StepSize = count1StepSizeFast;
}
}
void toggleOutput(void){
if (millis()- buttonDownTime >= 2000) {
Serial.println("TOGGLE STROBE OUTPUT");
}
}