A clock pendulum with parametric drive

1#include "Arduino.h"
2
3//Timing parameters for TC6.
4const uint32_t tC6RA = 27750, tC6RC = 55500;
5
6void TC0Setup() {
7  pmc_set_writeprotect(false);
8  //Enable the clock in the PMC  
9  REG_PMC_PCER0 |= PMC_PCER0_PID27;   //Enable the clock (peripheral ID = PID27).   
10  //Disable PIO control over the output IO Line.
11  REG_PIOB_ABSR |= PIO_ABSR_P25;       //This is a B peripheral on PIOB.
12  REG_PIOB_PDR  |= PIO_PDR_P25;        //Disable PIO control over the output pin.
13  //REG_PIOB_IDR  |= PIO_IDR_P25;        //Disable PIO interrupts (seems not necessary?)
14  //Set the timer characteristics                         
15  REG_TC0_CMR0 = TC_CMR_WAVE |                      // Enable wave mode
16                 TC_CMR_WAVSEL_UP_RC |              // Count up with automatic trigger on RC compare
17                 TC_CMR_TCCLKS_TIMER_CLOCK1 |       // Set the timer clock to TCLK1 (MCK/2 = 84MHz/2 = 42MHz)                   
18                 TC_CMR_ACPA_SET |                  // Set TIOA on counter match with RA0
19                 TC_CMR_ACPC_CLEAR;                 // Clear TIOA on counter match with RC0
20  //Set the timing
21  REG_TC0_RA0 = 10;
22  REG_TC0_RC0 = 42;                                     
23  //And start: software trigger and enable
24  REG_TC0_CCR0 = TC_CCR_SWTRG | TC_CCR_CLKEN;        
25}
26void TC1Setup() {
27  pmc_set_writeprotect(false);
28  //Enable the clock in the PMC  
29  REG_PMC_PCER0 |= PMC_PCER0_PID28;   //Enable the clock (peripheral ID = PID28).
30  //Disable clocking while setting up
31  //REG_TC0_CCR1 = TC_CCR_CLKDIS; 
32  //Disable PIO control over the output IO Line.
33  REG_PIOA_PDR  |= PIO_PDR_P2;        //Disable PIO control over the output pin. Needed?
34  //Set the BMR register for timer chaining (datasheet p856, a TC0 common register for timing chaining 
35  REG_TC0_BMR |= TC_BMR_TC1XC1S_TIOA0;      
36  //Set the timer characteristics 
37  REG_TC0_CMR1 =                TC_CMR_TCCLKS_XC1          // capture mode, external clock XC1             
38                              | TC_CMR_ABETRG              // TIOA is used as the external trigger
39                              | TC_CMR_LDRA_RISING         // load RA on rising edge of trigger input
40                              | TC_CMR_LDRB_FALLING;       // load RB on falling edge of trigger input
41  //Set up interrupts for load RA and load RB                                    
42  REG_TC0_IER1 |= TC_IER_LDRAS | TC_IER_LDRBS;
43  // Enable TC1 interrupts            
44  NVIC_EnableIRQ(TC1_IRQn);                                 
45  //And start: software trigger and enable
46  REG_TC0_CCR1 = TC_CCR_SWTRG | TC_CCR_CLKEN;
47}
48void TC2Configure() {
49  pmc_set_writeprotect(false);
50  //Enable the clock in the PMC  
51  REG_PMC_PCER0 |= PMC_PCER0_PID29;   //Enable the clock (peripheral ID = PID29, on PMC_PCPER0). 
52  //Set the BMR register for timer chaining (datasheet p856, a TC0 common register for timing chaining
53  //Duplicates statement in TC1 config 
54  //The equivalent of the TC_BMR_TC1XC1S_TIOA0 of TC1 would be TC_BMR_TC2XC2S_TIOA0. But it does not seem to exist.
55  //According to the datasheet 36.7.14, we need to set bit 5 of TC0_BMR. And that works. 
56  REG_TC0_BMR |= 0B100000;    
57  //Set the timer characteristics                         
58  REG_TC0_CMR2 = TC_CMR_WAVE |                      // Enable wave mode
59                 TC_CMR_WAVSEL_UP_RC |              // Count up with automatic trigger on RC compare
60                 TC_CMR_TCCLKS_XC2 |                // Set the timer clock to external XC2, which is TIOA0
61             //    TC_CMR_EEVT_XC2 |                  // Set event selection to XC2 to make TIOB an output                                  
62                 TC_CMR_ACPA_CLEAR |                  // Set TIOA on counter match with RA
63                 TC_CMR_ACPC_SET ;                // Clear TIOA on counter match with RC
64             //    TC_CMR_BCPB_SET |                  // Set TIOB on counter match with RB
65             //    TC_CMR_BCPC_CLEAR;                 // Clear TIOB on counter match with RC
66  REG_TC0_IER2 = TC_IER_CPAS | TC_IER_CPCS; 
67  //  REG_TC0_IER2 = ~TC_IER_CPCS;
68  //  //Enable TC1 interrupts
69  NVIC_EnableIRQ(TC2_IRQn);
70}
71void TC2SetTiming(uint32_t ra, uint32_t rc) {
72  REG_TC0_RA2 = ra;
73  REG_TC0_RC2 = rc;   
74}
75void TC2Start() {
76  //Software trigger and enable
77  REG_TC0_CCR2 = TC_CCR_SWTRG | TC_CCR_CLKEN;  
78}
79void TC2Stop() {
80  REG_TC0_SR2;                                     //Read and clear status register (timer will hang without)  
81  REG_TC0_CCR2 = TC_CCR_CLKDIS;                    //Stop timer   
82}
83void TC6Configure() {
84  pmc_set_writeprotect(false);
85  //Enable the clock in the PMC  
86  REG_PMC_PCER1 |= PMC_PCER1_PID33;   //Enable the clock (peripheral ID = PID33, on PMC_PCPER1).   
87  //Disable PIO control over the output IO Line.
88  REG_PIOC_ABSR |= PIO_ABSR_P25 | PIO_ABSR_P26;       //These are B peripheral on PIOD (PD7 and PD8).
89  REG_PIOC_PDR  |= PIO_PDR_P25 | PIO_PDR_P26;        //Disable PIO control over the output pin.
90  //REG_PIOD_IDR  |= PIO_IDR_P7 | PIO_IDR_P8;      //Disable PIO interrupts (seems not necessary?)
91  //Set the timer characteristics                         
92  REG_TC2_CMR0 = TC_CMR_WAVE |                      // Enable wave mode
93                 TC_CMR_WAVSEL_UP_RC |              // Count up with automatic trigger on RC compare
94                 TC_CMR_TCCLKS_TIMER_CLOCK5 |       // Set the timer clock to TCLK5 (slow clock 32KHz) 
95               //TC_CMR_EEVT_XC2 |                  // Set event selection to XC2 to make TIOB an output                                  
96                 TC_CMR_ACPA_SET |                  // Set TIOA on counter match with RA
97                 TC_CMR_ACPC_CLEAR ;                // Clear TIOA on counter match with RC
98               //TC_CMR_BCPB_SET |                  // Set TIOB on counter match with RB
99               //TC_CMR_BCPC_CLEAR;                 // Clear TIOB on counter match with RC
100                 
101  //Set the timing for an approximately 2s square wave
102  REG_TC2_RA0 = tC6RA;
103  REG_TC2_RC0 = tC6RC;                                           
104}
105void TC6Start() {
106  //Software trigger and enable
107  REG_TC2_CCR0 = TC_CCR_SWTRG | TC_CCR_CLKEN;    
108}
109void TC6Stop() {
110  REG_TC2_SR0;                                     //Read and clear status register (timer will hang without)  
111  REG_TC2_CCR0 = TC_CCR_CLKDIS;                    //Stop timer  
112}
113void TC8Configure() {
114  pmc_set_writeprotect(false);
115  //Enable the clock in the PMC  
116  REG_PMC_PCER1 |= PMC_PCER1_PID35;   //Enable the clock (peripheral ID = PID35, on PMC_PCPER1).   
117  //Disable PIO control over the output IO Line. 7 and 8 outputs, 9 clock in
118  REG_PIOD_ABSR |= PIO_ABSR_P7 | PIO_ABSR_P8 | PIO_ABSR_P9;      //These are B peripheral on PIOD (PD7 and PD8).
119  REG_PIOD_PDR  |= PIO_PDR_P7 | PIO_PDR_P8 | PIO_PDR_P9;        //Disable PIO control over the output pin.
120  //REG_PIOD_IDR  |= PIO_IDR_P7 | PIO_IDR_P8;      //Disable PIO interrupts (seems not necessary?)
121
122
123  //REG_TC2_BMR |= TC_BMR_TC1XC1S_TIOA0;
124  REG_TC2_BMR |= TC_BMR_TC2XC2S_TCLK2; 
125
126  
127  //Set the timer characteristics
128  //We clear on ACPA and set on ACPC because the stepper driver acts on leading edge                         
129  REG_TC2_CMR2 = TC_CMR_WAVE |                      // Enable wave mode
130                 TC_CMR_WAVSEL_UP_RC |              // Count up with automatic trigger on RC compare 
131                 TC_CMR_TCCLKS_XC2 |                             
132                 TC_CMR_ACPA_CLEAR |              // Set TIOA on counter match with RA
133                 TC_CMR_ACPC_SET ;                // Clear TIOA on counter match with RC
134
135  //Set up interrupts for compare RA and compare RC                                    
136  REG_TC2_IER2 |= TC_IER_CPAS | TC_IER_CPCS;
137  // Enable TC8 interrupts            
138  NVIC_EnableIRQ(TC8_IRQn);                                                             
139}
140void TC8Start() {
141  //Software trigger and enable
142  REG_TC2_CCR2 = TC_CCR_SWTRG | TC_CCR_CLKEN;   
143}
144void TC8SetTiming(uint32_t ra, uint32_t rc) {
145  REG_TC2_RA2 = ra;
146  REG_TC2_RC2 = rc;      
147}
148void TC8Stop() {
149  REG_TC2_SR2;                                     //Read and clear status register (timer will hang without)  
150  REG_TC2_CCR2 = TC_CCR_CLKDIS;                    //Stop timer  
151}

1#include <arduino.h>
2
3#ifndef MOS_DEFINED 
4  #define MOS_DEFINED
5  #include "CBT_MotorSequence.h"
6#endif
7
8
9//#include "CBT_MotorSequence.h"
10// ----------------------------------------------------
11double CBT_MotorSequence::prepare(uint32_t st, uint32_t sa, double tt, bool dir, double start) {
12  //Calculates the key parameters of the sequence
13  m_st = st;
14  m_sa = sa;
15  m_tt = tt;
16  m_dir = dir;
17  m_start = start;
18
19  m_sp = m_st - 2 * m_sa;
20  if (m_sp > 0) {                     //If there is a constant speed plateau
21        m_vm = (m_st + 2 * m_sa) / m_tt;
22        m_a = m_vm * m_vm / (2 * m_sa);
23        m_ta = m_vm / m_a;
24  }
25  else {                           //If there is no plateau
26        m_vm = 2 * m_st / m_tt;
27        m_a = 2 * m_vm / m_tt;
28        m_ta = m_vm / m_a;
29  } 
30  if(st < 2 * sa) m_mseqerr = 1000; 
31  return getStartPulseInterval();
32}
33double CBT_MotorSequence::prepare(uint32_t st, uint32_t sa, double tt, bool dir) {
34  return prepare(st, sa, tt, dir, 0.0);  
35}
36double CBT_MotorSequence::prepare(uint32_t st, uint32_t sa, double tt, double start) {
37  return prepare(st, sa, tt, false, start);  
38}
39double CBT_MotorSequence::prepare(uint32_t st, uint32_t sa, double tt) {
40  return prepare(st, sa, tt, false, 0.0);  
41}
42double CBT_MotorSequence::getTimePoint(uint32_t index) {
43    if(index > m_st || index < 0) m_mseqerr = 2000;
44    //Case of constant velocity, plateau only
45    if(m_sa == 0) {
46      return index / m_vm;      
47    }
48    //Acceleration phase
49    if (index <= m_sa) return sqrt(2 * index / m_a);
50    //Plateau                       //
51    if (index >= m_sa + 1 && index <= m_st - m_sa) {
52      return m_ta + (index - m_sa) / m_vm;
53    }    
54    //Deceleration   
55    if (index >= m_st - m_sa + 1) {
56       //Acceleration is negative in deceleration phase
57       return m_tt - m_ta - (-m_vm + sqrt(m_vm * m_vm - 2 * m_a * (index - m_sp - m_sa))) / m_a;       
58    }
59    //char bstr[100];
60    //sprintf(bstr, "Debug getTimePoint, index  %u  ttt  %f", index, ttt);
61    //Serial.println(bstr);    
62    return 0; 
63}
64double CBT_MotorSequence::getPulseTime(uint32_t index) {
65  if(index > m_st - 1 || index < 0) m_mseqerr = 2100;
66  double tb, ta;
67  tb = getTimePoint(index);
68  ta = getTimePoint(index + 1); 
69  //Serial.println("**");
70  //Serial.print(index); Serial.print("    "); Serial.print(tb*1000); Serial.print("    "); Serial.println(ta*1000);  
71  return (tb + ta) / 2.0;    
72}
73double CBT_MotorSequence::getPulseInterval(uint32_t index) {
74  double tp;
75  if(index > m_st - 1 || index < 0) m_mseqerr = 2100; 
76  if(index == 0) {
77    tp = getPulseTime(0);
78  }
79  else {
80    tp = getPulseTime(index) - getPulseTime(index - 1);
81  } 
82  return tp; 
83}
84double CBT_MotorSequence::getNextPulseInterval() {
85  double previousPulseTime;
86  m_mrPulseIntervalIndex++;
87  m_mrTimePointIndexBefore++;
88  m_mrTimePointBefore = m_mrTimePointAfter;
89  m_mrTimePointIndexAfter++;
90  if(m_mrTimePointIndexAfter > m_st) return 0;  //End of sequence, signal to stop timer
91  m_mrTimePointAfter = getTimePoint(m_mrTimePointIndexAfter);
92  previousPulseTime = m_mrPulseTime;
93  m_mrPulseTimeIndex++;
94  m_mrPulseTime = (m_mrTimePointBefore + m_mrTimePointAfter) / 2;
95  return m_mrPulseTime - previousPulseTime; 
96}
97uint32_t CBT_MotorSequence::getStepCount() {
98  return m_st;
99}
100uint32_t CBT_MotorSequence::getAccelerationStepCount() {
101  return m_sa;
102}
103double CBT_MotorSequence::getTotalSequenceTime() {
104  return m_tt;
105}
106double CBT_MotorSequence::getMaximumVelocity() {
107  return m_vm;
108}
109double CBT_MotorSequence::getAcceleration() {
110  return m_a;
111}
112double CBT_MotorSequence::getStartPulseInterval() {
113  char bstr[100];
114  m_mrPulseIntervalIndex = 0;
115  m_mrTimePointIndexBefore = 0;    
116  m_mrTimePointBefore = getTimePoint(m_mrTimePointIndexBefore); 
117  m_mrTimePointIndexAfter = 1;
118  m_mrTimePointAfter = getTimePoint(m_mrTimePointIndexAfter);   
119  m_mrPulseTimeIndex = 0;
120  m_mrPulseTime = (m_mrTimePointBefore + m_mrTimePointAfter) / 2;
121  m_mrStartPulseInterval = m_mrPulseTime;     
122  return m_mrStartPulseInterval;   
123}
124double CBT_MotorSequence::getEndSequenceInterval() {
125  return m_tt - getPulseInterval(m_st);  
126}
127bool CBT_MotorSequence::getDirection() {
128  return m_dir;
129}
130double CBT_MotorSequence::getStartTime() {
131  return m_start;
132}
133double CBT_MotorSequence::getLastPulseTime() {
134  return getPulseTime(m_st - 1);
135}

1/*
22024Pendulum motor sequencer class
3For explanation, see the Matlab project of the same name
4The approach differs somewhat from that of the 2023 pendulum:
5- Input parameters are total stepcount, acceleration / deceleration stepcount, and total time: st, sa, tt
6- Maximum velocity and acceleration are calculated
7- For total stepcount st, we conceptually have st + 1 time-point from 0 to st + 1. Pulses are created in the middle
8  between these timepoints. This easily allows for motor direction setting at start of the sequence.
9- Direction 
10- Start time with respect to the start of the sequence: i.e. common delay time for entire sequence
11*/ 
12
13// --------------------------------------------------- 
14//Structure for variables related to the motor step acceleration / plateau / deceleration sequence
15class CBT_MotorSequence {
16  private:
17    //INPUTS 
18    uint32_t m_st;                          //Total stepcount
19    uint32_t m_sa;                          //Acceleration stepcount
20    double   m_tt;                          //Sequence time duration
21    bool     m_dbMode = false;              //If true, produces verbose console output
22    bool     m_dir;                          //Motor direction
23    double   m_start;                        //Time on which the sequence should start  
24    //CALCULATED
25    uint32_t m_mseqerr = 0;                 //Error numbers for this class
26    double   m_vm;                          //Maximum speed
27    double   m_a;                           //Acceleration
28    uint32_t m_sp;                          //Plateau stepcount
29    double   m_ta;                          //Acceleration time
30    //STORED VALUES TO AVOID DOUBLE CALCULATIONS
31    //In conjunction with the functions getStartPulseInterval and getNextPulseInterval, these
32    //variables support sequence generation with half the calls to getTimePoint (the 'mr' stands for most recent)
33    uint32_t m_mrPulseIntervalIndex;        //Index of most recently calculated pulse interval
34    uint32_t m_mrPulseTimeIndex;            //Index of most recently calculated pulse time
35    double   m_mrPulseTime;                 //Most recently calculated pulse time
36    uint32_t m_mrTimePointIndexBefore;      //Index of the most recently calculated time point before the pulse time
37    double   m_mrTimePointBefore;           //Most recently calculated time point before the pulse time
38    volatile uint32_t m_mrTimePointIndexAfter;       //Index of the most recently calculated time point after the pulse time
39    double   m_mrTimePointAfter;            //Most recently calculated time point after the pulse time
40    double   m_mrStartPulseInterval;        //Interval for the first pulse, calculated during prepare. Allows pre-loading the timer 
41  public:   
42    // ----------------------------------------------------
43    //Prepares the parameters of a stepper sequence
44    //Returns the interval for the first pulse
45    double prepare(uint32_t st, uint32_t sa, double tt);
46    double prepare(uint32_t st, uint32_t sa, double tt, bool dir);
47    double prepare(uint32_t st, uint32_t sa, double tt, double start);        
48    double prepare(uint32_t st, uint32_t sa, double tt, bool dir, double start);
49    // ----------------------------------------------------
50    //Returns the requested time point of the sequence curve
51    double getTimePoint(uint32_t index);
52    // ----------------------------------------------------    
53    //Returns the requested pulse time
54    double getPulseTime(uint32_t index);
55    // ----------------------------------------------------    
56    //Returns the requested inter-pulse interval. For pulse 0, from time 0; For other pulse, from the previous pulse
57    double getPulseInterval(uint32_t index);    
58    // ----------------------------------------------------    
59    //PREFER FOR EFFICIENCY, BUT NOT FULLY TESTED: returns the interval for the first pulse, from sequence start
60    double getStartPulseInterval();
61    // ----------------------------------------------------
62    //PREFER FOR EFFICIENCY, BUT NOT FULLY TESTED: in a sequence, return the interval for the next pulse. Zero for end of sequence.     
63    double getNextPulseInterval();
64    // ----------------------------------------------------
65    //Returns the interval from the last pulse to end of sequence
66    double getEndSequenceInterval();         
67    // ----------------------------------------------------
68    //Returns the total step count
69    uint32_t getStepCount();
70    // ----------------------------------------------------
71    //Returns the acceleration step count
72    uint32_t getAccelerationStepCount(); 
73    // ----------------------------------------------------
74    //Returns the total sequence time
75    double getTotalSequenceTime();
76    // ----------------------------------------------------
77    //Returns the maximum vel0city
78    double getMaximumVelocity();
79    // ----------------------------------------------------
80    //Returns the acceleration
81    double getAcceleration(); 
82    // ----------------------------------------------------
83    //Returns the direction
84    bool getDirection();     
85    // ----------------------------------------------------
86    //Returns the start (as fraction of period)
87    double getStartTime(); 
88    // ----------------------------------------------------
89    //Returns the time of the last pulse
90    double getLastPulseTime();          
91};

1//For timers IO Lines and pins, see datasheet section 36.5, and the Arduino DUE pinout.
2//For peripheral ID's, see datasheet section 9.1, table 9-1 
3//Note that timers 6, 7, 8 are controlled by power management register PMC_PCER1, as opposed to PMC_PCER0. 
4//For timer inputs / outputs, see section 36.5, table 36-4 
5
6//TC0 provides the ticks signal, constant frequency in the MHz range.
7//Output TIOA0 is on digital pin 2 (IO Line PB25).
8
9//TC1 is a capture mode timer, clocked by TC0 ticks using timer chaining . It measures the pendulum beats in ticks.
10//Input is expected to be a toggle squarewave of one period per pendulum period. 
11//Input TOIA1 is on analog pin A7 (IO Line PA2).
12//TC1 interupts are enabled, ISR is TC1_Handler. The handler discriminates between signal up (A) and down (B), i.e. between the beats.
13
14//TC2 is triggered by the TC1 A signal. Subsequently, it orchestrates the start of the motor sequences, with a preset delay after its trigger.
15//It is clocked by TC0 using timer chaining.
16//It has no output pins on the Arduino DUE.
17//TC2 interupts are enabled.
18
19//TC6 is a pendulum emulator that can be used for development.
20//It produces a square wave of about 0.5Hz.
21//Output TIOA6 is digital pin 5 (IO Line PC25), which should be connected (for testing)to A7, the TC1 input.
22//The timing parameters of TC6 (TC6RA and TC6RC) are defined in the cpp file.                                                                                                                                                                 TIOB6 is on digital pin 4 (IO Line PC26).
23
24//TC8 is the timer that runs the motor sequences. Its output connects directly to the motor power-driver optos.
25//It is clocked by TC0, by wiring from D2 (TC0 out) to D30 (IO Line PD9).
26//Output TIOA8 is on digital pin 11 (IO Line PD7), TIOB8 is on digital pin 12 (IO Line PD8).
27//It is used in RA-compare, RC-compare mode, with interrupts on these.
28
29//TC0, TC1 run permanently, are started by their TC*Setup() functions.
30//TC2, TC6 and TC8 are configured, and can then be started and stopped as required. 
31//------------------------------------------------------------------------------------------------------
32void TC0Setup();
33void TC1Setup();
34
35void TC2Configure();
36void TC2SetTiming(uint32_t ra, uint32_t rc);
37void TC2Start();
38void TC2Stop();
39
40void TC6Configure();
41void TC6Start();
42void TC6Stop();
43
44void TC8Configure();
45void TC8Start();
46void TC8SetTiming(uint32_t ra, uint32_t rc);
47void TC8Stop();

1//Code to control the parametric pendulum.
2//Features:
3//- Controls stepper motor sequence(s) to move the actuator up/down
4//- Automatic mode: a feedback system executes a series of automatic stepper sequences when pendulum amplitude is below threshold
5//- Manual mode: the stepper motor can be controlled via serial input
6//- Serial input: can only be entered when excitation is off - because it tends to interfere with the timers
7//- Serial input: enter 1 0 to go to manual mode; 1 1 to go to automatic mode; Enter 2 n to move acutaor up; 3 n to move down; 4 to set motorposition to 0. Where n is a stepcount
8//- Excitation can be forced off by grounding pin PIN_EXCPERMIT
9//- Keeps a running average of the number of ticks per period => ticks per second
10// ------------------------------------------------------------------------------------------------- 
11#include "DUETimerFunctions.h"
12#include "CBT_MotorSequence.h"
13//DECLARATIONS PINS
14  //TIOA0 output is pin 2, ticks generator out
15  //TIOA1 input is pin A7, to connect the pendulum signal (a square wave toggle PulsePerPeriod) 
16  //TIOA6 output is pin 5, pendulum emulator for testing. Then connect to A7.
17  //TIOB6 output is pin 6, same
18  //TC8 clock input is pin 30, to be connected to pin 2
19  //TIOA8 output is pin 11, to connect to STEP input of motor driver
20  //TIOB8 output is pin 12, not used  
21  //
22  const uint16_t PIN_TC1OUT = 26;       //Pin giving the output of TC1, through the TC1_Handler: the pendulum toggle
23  const uint16_t PIN_EXCITATIONON = 23; //Pin giving the excitation state, on or off
24  const uint16_t PIN_DIRECTION = 22;    //Pin for motor direction output
25  const uint16_t PIN_AMPLITUDE = 25;    //HIGH when amplitude detector rises, LOW when it lass
26  const uint16_t PIN_BOARDLED = 13;     //The board LED for testing purposes
27  //
28  const uint16_t PIN_EXCPERMIT = 24;     //Pin giving direct control of the excitationOnSignal
29  //
30  //Pins for the 7-channel detector. Each detector uses two pins, one for rising edge, one for falling edge.
31  //Note that most of these are not used, not even necessary! Only one detector (currently DET1R) is used here for amplitude control (through handlers ISRAmpDetRise, ISRAplDetFall).
32  //Note that the central detector (pendulum at bottom) is wired seperately to pin A7 (the TIOA1 pin of TC1)
33  const uint16_t PIN_DET1R = 40;
34  const uint16_t PIN_DET1F = 41;
35  const uint16_t PIN_DET2R = 42;
36  const uint16_t PIN_DET2F = 43;
37  const uint16_t PIN_DET3R = 44;
38  const uint16_t PIN_DET3F = 45;
39  const uint16_t PIN_DET4R = 46;
40  const uint16_t PIN_DET4F = 47;
41  const uint16_t PIN_DET5R = 48;
42  const uint16_t PIN_DET5F = 49;
43  const uint16_t PIN_DET6R = 50;
44  const uint16_t PIN_DET6F = 51;
45  const uint16_t PIN_DET7R = 52;
46  const uint16_t PIN_DET7F = 53;    
47//DECLARATION: general use output character buffer
48  char strBuf[200];     //String for serial output   
49//DECLARATIONS: Main features of period and beat
50  const double secondsPerPeriod = 2.0;                       //Pendulum seconds, 2.0 for a 2s pendulum
51  volatile uint32_t ticksA = 0, ticksB, ticksAPrevious;      //TC1 capture ticks at detection A, B, and previousA
52  volatile uint32_t periodCount = 0;                         //Period counter
53  const uint32_t periodCountStartOpsMode = 4;                //Period at which automatic operation will be enabled, must be > 2
54  volatile bool flagTC1A = false;                            //Flag TC1A to loop (detection of period)
55  uint32_t periodTicksA = 0;                                 //Duration of last period in ticks
56  double   averageTicksPerPeriod;                            //Average duration of period in ticks 
57  uint32_t averageTicksPerPeriodSampleCount = 0;             //Counter of samples in the averaging process
58  uint32_t averageTicksPerPeriodMaxSampleCount = 5;          //Sets the time-constant for the running average of the ticksPerPeriod
59  uint32_t ticksPerSecond;                                   //Ticks per second, derived from averageTicksPerPeriod, calculated for each period
60  uint32_t ticksBeatBA;                                      //Duration of last beat (TC1B to TC1A) in ticks
61  double beatAsymmetry;                                      //Beat asymmetry, fraction of difference between A->B and B->A beat (due to detector off-centre)
62//DECLARATIONS: Automatic motor sequences 
63  double timeTrigger;                                        //Time between TC1A trigger (pendulum bottom, start of period) and start of TC8 timer. Used by TC2
64  volatile uint32_t tC2RC;                                   //Corresponding value of TC2RC
65  const uint32_t seqCount_AutoMode = 6;                      //Number of sequences in automatic mode
66  CBT_MotorSequence seqAutoMode[seqCount_AutoMode];          //Array of motor sequence objects in automatic mode
67  CBT_MotorSequence* pSeqAutoMode[seqCount_AutoMode];        //Pointers to motor sequence objects in automatic mode
68  double timeFirstPulse[seqCount_AutoMode];                  //Helper arrays to facilitate transitions between sequences 
69  double timeLastPulse[seqCount_AutoMode];                   //Helper arrays to facilitate transitions between sequences  
70//DECLARATIONS: Manual motor sequence (may also be used for other non-automatic purposes outside of excitation)
71  CBT_MotorSequence manualSequence;                          //The manual sequence object, will be prepared ad-hoc
72  double jogSpeed = 200.0;                                   //Jog speed in manual operation, steps per second
73//DECLARATIONS: CURRENT motor sequences - either automatic or manual
74  uint32_t seqCount;                                         //Current value of sequence count
75  CBT_MotorSequence* pSeq[seqCount_AutoMode];                //Current pointer array, dimensioned for the larger size of auto mode
76  volatile bool motorDirection;                              //Current motor direction
77  volatile int32_t motorPosition = 0;                        //Current motor position
78//DECLARATIONS: Variables for management of TC8
79  const bool debugTC8Timing = false;
80  //The sequence / step to be prepared in subsequent call of TC2_Handler 
81  volatile uint32_t seqNumber;                               //Counter stepping through sequences      
82  volatile uint32_t seqStepNumber;                           //Counter stepping through steps of a sequence
83  volatile bool flagTC8A = true;                             //Flag TC8A to loop
84  volatile bool flagTC8C = false;                            //Flag TC8C to loop
85  volatile uint32_t ticksTC8Start;                           //Ticks at the start of the timer
86  volatile uint32_t ticksTC8RC;                              //Ticks at the call of TC8_Handler, both A and C
87  volatile uint32_t tC8RCFirst[seqCount_AutoMode];           //First TC8RC value to be used by the sequence, in TC8SetTiming
88  volatile uint32_t tC8RC;                                   //Current TC8RC value
89//DECLARATIONS: Amplitude feedback
90  uint32_t ticksAmpDetRise = 0, ticksAmpDetFall = 0;         //Ticks at which the amplitude detector signal rises / falls
91  uint32_t ampDetHighTicks;                                  //Number of ticks during which the amplitude detector is high
92  const uint32_t ampDetHighTicksMin = 50000;                 //Feedback criterion: minimum number of ticks for which the amplitude detector must be high. If less => excitation required, if more => no excitation
93  bool excitationRequired = false;                           //True when the amplitude detection requires excitation
94  bool excitationOn = false;                                 //True when excitation is active, i.e. motor may move on feedback. BLOCKS Serial input! 
95  bool excitationPermitPin = true;                           //Allow to block excitation by bringing the designated pin to LOW
96//DECLARATIONS: program logic
97  bool opsMode = false;                                      //True for automatic operation, false for manual -> no excitation
98// -------------------------------------------------------------------------------------------------           
99//HANDLERS
100void isrAmpDetRise() {
101  ticksAmpDetRise = getTicks();
102  digitalWrite(PIN_AMPLITUDE, HIGH);
103}
104void isrAmpDetFall() {
105  ticksAmpDetFall = getTicks();
106  digitalWrite(PIN_AMPLITUDE, LOW);
107}
108void TC1_Handler() {
109  uint32_t status = REG_TC0_SR1;
110  if((status & TC_SR_LDRAS) == TC_SR_LDRAS) {
111    ticksAPrevious = ticksA;                        
112    ticksA = REG_TC0_RA1;                        //Snapshot of the capture ticks
113    digitalWrite(PIN_TC1OUT, HIGH);              //Write to output pin
114    ampDetHighTicks = ticksAmpDetFall - ticksAmpDetRise;    //The time in ticks for which amplitude detection is high
115    //Excitation needed?
116    excitationRequired = (ampDetHighTicks < ampDetHighTicksMin)? true : false;  //Excitation if that time in ticks is below the set threshold
117    //Reset for next period
118    ticksAmpDetRise = 0;
119    ticksAmpDetFall = 0;
120    digitalWrite(PIN_AMPLITUDE, LOW);
121    //Decide on excitation
122    excitationPermitPin = digitalRead(PIN_EXCPERMIT);                    //Check the excitation control pin
123    excitationOn = false;
124    if(opsMode & excitationRequired & excitationPermitPin) {                 //Start the TC2 timer, if opsMode = true and excitation is needed
125      TC2Start(); 
126      excitationOn = true;               
127    }    
128    flagTC1A = true;
129  }
130  else if ((status & TC_SR_LDRBS) == TC_SR_LDRBS) {
131    ticksB = REG_TC0_RB1;                        //Snapshot of B capture ticks
132    digitalWrite(PIN_TC1OUT, LOW);               //Write to output pin
133  }
134}
135void TC2_Handler() {
136  uint32_t status = REG_TC0_SR2;
137  if((status & TC_SR_CPCS) == TC_SR_CPCS) {
138    TC2Stop();  //Noting TC2 runs as a one-shot per period
139    runTC8_AutoMode();
140  }
141}
142void TC8_Handler() {
143  //Somehow, TC_SRCPAS is called an extra time immediately at start timer. But this has no impact, and I am not using it.
144  ticksTC8RC = getTicks();
145  uint32_t status = REG_TC2_SR2;       // Read & Save & Clear the status register  
146  if ((status & TC_SR_CPAS) == TC_SR_CPAS) {
147    //Serial.println("  TC8_Handler A");
148    flagTC8A = true;   
149  }
150  else if ((status & TC_SR_CPCS) == TC_SR_CPCS) {
151    TC8SetTiming(tC8RC / 2, tC8RC);
152    flagTC8C = true;
153  }  
154}
155// ------------------------------------------------------------------------------------------------- 
156//SETUP and LOOP
157void setup() {
158  Serial.begin(250000);
159  Serial.println("MotorSequencerV10B");
160  //Pin modes
161  pinMode(A7, INPUT_PULLUP);
162  pinMode(PIN_DIRECTION, OUTPUT);
163  pinMode(PIN_TC1OUT, OUTPUT);
164  pinMode(PIN_EXCPERMIT, INPUT_PULLUP);
165  pinMode(PIN_EXCITATIONON, OUTPUT);
166  pinMode(PIN_AMPLITUDE, OUTPUT);
167  pinMode(PIN_BOARDLED, OUTPUT);
168  //Detector pins
169  pinMode(PIN_DET1R, INPUT_PULLUP);
170  pinMode(PIN_DET1F, INPUT_PULLUP);
171  pinMode(PIN_DET2R, INPUT_PULLUP);
172  pinMode(PIN_DET2F, INPUT_PULLUP);
173  pinMode(PIN_DET3R, INPUT_PULLUP);
174  pinMode(PIN_DET3F, INPUT_PULLUP);
175  pinMode(PIN_DET4R, INPUT_PULLUP);
176  pinMode(PIN_DET4F, INPUT_PULLUP);
177  pinMode(PIN_DET5R, INPUT_PULLUP);
178  pinMode(PIN_DET5F, INPUT_PULLUP);
179  pinMode(PIN_DET6R, INPUT_PULLUP);
180  pinMode(PIN_DET6F, INPUT_PULLUP);  
181  pinMode(PIN_DET7R, INPUT_PULLUP);
182  pinMode(PIN_DET7F, INPUT_PULLUP);
183  //Attach the necessary interrupts for the detector pins
184  attachInterrupt(digitalPinToInterrupt(PIN_DET1R), isrAmpDetRise, RISING);
185  attachInterrupt(digitalPinToInterrupt(PIN_DET1F), isrAmpDetFall, FALLING); 
186   //Prepare the sequences for the automatic operation
187  prepareSequences_AutoMode(); 
188  //Set up (i.e. configure and start) the tick generator TC0 and the pendulum detection counter TC1
189  TC0Setup();
190  TC1Setup();
191  //Configure the sequence trigger counter TC2
192  TC2Configure();
193  //USED FOR TESTING ONLY: configure and start the pendulum emulator counter: make sure TC6 output is wired to TC1 input A7
194  TC6Configure();
195  TC6Start();
196  //Configure the TC8 timer
197  TC8Configure();
198  //runTC8_AutoMode();  //Testing only
199}
200void loop() {
201  //Handle serial input. This is only handled when there is no excitation, otherwise the timer sequences are likely to be correupted
202  if(!excitationOn) ProcessSerialInput();
203  //Write the excitation state to the designated pin   
204  digitalWrite(PIN_EXCITATIONON, excitationOn? true : false);
205  //Handle flagTC1A, called by TC1_Handler on capture A: the pendulum period detection
206  if(flagTC1A) {
207    flagTC1A = false;
208    handleFlagTC1A();           
209  }
210  //Handle flagTC8A, called by TC8_Handler on compare A. Setting of direction for current pulse, and setting tC8RC for the subsequent pulse
211  if(flagTC8A) {
212    flagTC8A = false;
213    handleFlagTC8A();       
214  }
215  //Handle flagTC8C, called by TC8_Handler on compare C. Pulse output on pin D11. Sets tC8RC for the subsequent pulse and increments the pulse number
216  if(flagTC8C) {
217    flagTC8C = false;
218    handleFlagTC8C(); 
219  }  
220}
221// -------------------------------------------------------------------------------------------------
222//TICKS
223//Returns current ticks
224uint32_t getTicks() {
225  return REG_TC0_CV1;
226}
227//Calculation of the running average of ticksPerPeriod
228void updateAverageTicksPerPeriod() {
229  periodTicksA = ticksA - ticksAPrevious;
230  if(averageTicksPerPeriodSampleCount == 0) {
231    averageTicksPerPeriodSampleCount++;    
232    averageTicksPerPeriod = (double) periodTicksA;
233  }
234  else if (averageTicksPerPeriodSampleCount < averageTicksPerPeriodMaxSampleCount) {
235    averageTicksPerPeriodSampleCount++;    
236    averageTicksPerPeriod += ((double) periodTicksA - averageTicksPerPeriod) / (double) averageTicksPerPeriodSampleCount;
237  }
238  else {
239    averageTicksPerPeriod += ((double) periodTicksA - averageTicksPerPeriod) / (double) averageTicksPerPeriodMaxSampleCount;
240  }
241  ticksBeatBA = ticksA - ticksB;
242  beatAsymmetry = (double) (2 * (int32_t) ticksBeatBA - (int32_t) periodTicksA) / (double) periodTicksA;  //Note the casts!
243  // sprintf(strBuf, "    periodTicksA:  %u  avtppSampleCount:  %u  avtpp:  %f  ticksBA:  %u  Asymmetry:  %f", periodTicksA, averageTicksPerPeriodSampleCount, averageTicksPerPeriod, ticksBeatBA, beatAsymmetry);
244  // Serial.println(strBuf); 
245}
246//Sets the value for ticksPerSecond, from the averageTicksPerPeriod, including a statistical correction for truncation error
247void updateTicksPerSecond() {
248  double tsec = (averageTicksPerPeriod / secondsPerPeriod);
249  uint32_t ntsec = (uint32_t) tsec;
250  double remainder = tsec - (double) ntsec;
251  double rnd = (double) random(0, 1000000) / 1000000.0;
252  ticksPerSecond =  (rnd < remainder)? ntsec + 1 : ntsec;
253}
254// -------------------------------------------------------------------------------------------------
255//SEQUENCES: PREPARATION AND START-UP OF AUTOMATIC / MANUAL SEQUENCES
256void prepareSequences_AutoMode() {
257  //Prepare the timing for TC2
258  timeTrigger = 0.49;
259  //Prepare motor sequences
260  uint32_t sa = 100;
261  uint32_t stD = 400;
262  uint32_t stU = 800;
263  seqAutoMode[0].prepare(stD, sa, 0.23, false, 0.01);
264  seqAutoMode[1].prepare(stU, sa, 0.46, true,  0.25);
265  seqAutoMode[2].prepare(stD, sa, 0.23, false, 0.75);
266  seqAutoMode[3].prepare(stD, sa, 0.23, false, 1.01);
267  seqAutoMode[4].prepare(stU, sa, 0.46, true,  1.25);
268  seqAutoMode[5].prepare(stD, sa, 0.23, false, 1.75);
269  //Get the pointer array for the sequence objects
270  for (int i = 0; i < seqCount_AutoMode; i++) {
271    pSeqAutoMode[i] = &seqAutoMode[i];
272  }
273  //Prepare the first and sequence-bridging intervals
274 for (int i = 0; i < seqCount_AutoMode; i++)  {
275  timeFirstPulse[i] = pSeqAutoMode[i]->getStartTime() + pSeqAutoMode[i]->getPulseTime(0);
276  timeLastPulse[i] =  pSeqAutoMode[i]->getStartTime() + pSeqAutoMode[i]->getLastPulseTime();  
277 }
278  if(debugTC8Timing) {
279    sprintf(strBuf, "timeTrigger  %f", timeTrigger); Serial.println(strBuf);   
280    for (int i = 0; i < seqCount_AutoMode; i++) {
281      sprintf(strBuf, "seq:  %u time first/last pulse  %f  %f", i, timeFirstPulse[i], timeLastPulse[i]); Serial.println(strBuf);
282    }       
283  }  
284}
285void runTC8_AutoMode() {
286  //Set the seqCount and the sequence pointer array to the automatic ones
287  seqCount = seqCount_AutoMode;
288  for (int i = 0; i < seqCount_AutoMode; i++) {
289    pSeq[i] = pSeqAutoMode[i];
290  }
291  seqNumber = 0;
292  //The seqNumber and seqStepNumber are the numbers for the currently ongoing pulse, i.e. next call to TC8C coming up.
293  //Timing and direction for sequence 0, pulse 0 are set here. Subsequent values be prepared by TC8A, set by TC8C
294  seqStepNumber = 0;        
295  tC8RC = tC8RCFirst[0];                             //Timing for first step
296  TC8SetTiming(tC8RC / 2, tC8RC);
297  motorDirection = pSeq[0]->getDirection();          //Direction for the first step
298  digitalWrite(PIN_DIRECTION, motorDirection);
299  TC8Start();
300  ticksTC8Start = getTicks();
301  if(debugTC8Timing) {sprintf(strBuf, "*****  TC8 started at ticks  %u  tC8RC  %u:  Direction:  %u  position:  %d", ticksTC8Start, tC8RC, motorDirection, motorPosition); Serial.println(strBuf);}  
302}
303void jogMotor(uint32_t steps, bool dir) {
304  manualSequence.prepare(steps, 0, (double) steps / jogSpeed, dir);
305  seqCount = 1;  
306  seqNumber = 0;
307  seqStepNumber = 0;
308  pSeq[0] = &manualSequence;
309  tC8RC = (uint32_t) (pSeq[0]->getPulseTime(0) * ticksPerSecond);    //Timing for first step
310  TC8SetTiming(tC8RC / 2, tC8RC);
311  motorDirection = pSeq[0]->getDirection();
312  digitalWrite(PIN_DIRECTION, motorDirection);
313  delayMicroseconds(25);
314  TC8Start();
315  ticksTC8Start = getTicks();
316  if(debugTC8Timing) {sprintf(strBuf, "TC8 started at ticks  %u  tC8RC  %u:", ticksTC8Start, tC8RC); Serial.println(strBuf);}
317}
318// -------------------------------------------------------------------------------------------------
319//FUNCTIONS CALLED BY LOOP UPON FLAGS FROM INTERRUPT HANDLERS
320void handleFlagTC1A() {
321  //Ignore the first period, there is no value for ticksAPrevious
322  if(periodCount == 0) {
323    periodCount++;
324    return;
325  }
326  periodCount++; 
327  sprintf(strBuf, "PERIOD:  %u  ticksA:  %u  periodTicksA:  %u  ticksPerSecond:  %u  SystemSecs:  %f", periodCount, ticksA, periodTicksA, ticksPerSecond, (double) millis()/1000.0); Serial.println(strBuf);
328  sprintf(strBuf, "      ampDetHighTicks:  %u,  excitationOn:  %d", ampDetHighTicks, excitationOn); Serial.println(strBuf);
329  updateAverageTicksPerPeriod();
330  updateTicksPerSecond();
331  //Calculate timing data for the next period: it needs updating because ticksPerSecond has been updated
332  tC2RC = (uint32_t) (timeTrigger * ticksPerSecond);
333  TC2SetTiming(tC2RC / 2, tC2RC); 
334  tC8RCFirst[0] = (uint32_t)(timeFirstPulse[0] * ticksPerSecond);
335  for (int i = 1; i < seqCount_AutoMode; i++) {
336    tC8RCFirst[i] = (uint32_t)((timeFirstPulse[i] - timeLastPulse[i - 1]) * ticksPerSecond);
337  }
338  //sprintf(strBuf, "handleFlagTC1A, tC8RCFirst[1]:  %u  timeFirstPulse[1]  %f  timeLastPulse[0]  %f  ticksPerSecond:  %u", tC8RCFirst[1], timeFirstPulse[1], timeLastPulse[0], ticksPerSecond); Serial.println(strBuf);
339  if(periodCount == periodCountStartOpsMode) {
340    opsMode = true;
341    Serial.println("  Starting automatic mode");
342  }
343}
344void handleFlagTC8A() {
345    if(seqNumber >= seqCount) return;
346  //Set the direction for the currently ongoing pulse. 
347  //The time TC8A to TC8C allows for the time needed by the power driver between DIR and STEP 
348  if(seqStepNumber == 0) {    
349    motorDirection = pSeq[seqNumber]->getDirection();
350    digitalWrite(PIN_DIRECTION, motorDirection);       
351  }  
352  //Determine the seq and seqStep numbers of the pulse after the current one, and get the TC8RC value to be set 
353  //in the upcoming call of TC8C
354  if(seqNumber == seqCount - 1 & seqStepNumber == pSeq[seqNumber]->getStepCount() - 1) {
355    //If the current pulse is the last, we give a big value for TC8RC. This avoids a spurious TC8RA call, and the timer will be stopped by TC8C in any case
356    tC8RC = 4294967295;   //Max value for the uint32-t
357  }
358  else {
359    //If the current is not the last, obtain the tC8RC for the next pulse
360    uint32_t sn, ssn;
361    sn = seqNumber;
362    ssn = seqStepNumber; 
363    ssn = seqStepNumber + 1;
364    if(ssn >= pSeq[sn]->getStepCount()) {
365      ssn = 0;
366      sn++;
367    }
368    if(ssn == 0) {           //Note that seqStepNumber = 0 will not be requested for seq 0 first time, because it is already set
369      tC8RC = tC8RCFirst[sn];    
370    }
371    else {
372      tC8RC = (uint32_t) (pSeq[sn]->getPulseInterval(ssn) * ticksPerSecond);    
373    }    
374  } 
375  if(debugTC8Timing) {
376    sprintf(strBuf, "  flagTC8A, ticksTC8RC:  %u  seqNumber:  %u  seqStepNumber:  %u  tC8RC  %u:  Direction:  %d", ticksTC8RC - ticksTC8Start, seqNumber, seqStepNumber, tC8RC, motorDirection); Serial.println(strBuf);
377  } 
378}
379void handleFlagTC8C() {
380  if(motorDirection) motorPosition++;
381  else motorPosition--;
382  if(debugTC8Timing) {sprintf(strBuf, "flagTC8C, ticksTC8RC:  %u  seqNumber:  %u  seqStepNumber:  %u  position  %d", ticksTC8RC - ticksTC8Start, seqNumber, seqStepNumber, motorPosition); Serial.println(strBuf);}       
383
384  if(seqNumber == seqCount - 1 & seqStepNumber == pSeq[seqNumber]->getStepCount() - 1) {
385    TC8Stop();
386    if(debugTC8Timing) Serial.println("*****  TC8 stops");
387    return;     
388  }
389  //Increment counters
390  seqStepNumber++;
391  if(seqStepNumber >= pSeq[seqNumber]->getStepCount()) {
392    seqNumber++;
393    seqStepNumber = 0;
394  }
395}
396// -------------------------------------------------------------------------------------------------
397//SERIAL INPUT
398//Serial input can ONLY be entered when there is no excitation (excitationOn = false). This is to avoid corrupting sequences.
399//If Serial input is entered while excitation is on, only the last entry will be digested as soon as excitation goes off.
400void ProcessSerialInput()  {
401  char chSerial[64];
402  char* pIndex;
403  int i1, i2;
404  uint32_t steps;  
405  while (Serial.available() > 0) {
406    //strSerial = Serial.readString();
407    Serial.readString().toCharArray(chSerial, 64);
408    pIndex = strtok(chSerial, " ");
409    i1 = atoi(pIndex);
410    switch(i1) {
411    case 1:
412      pIndex = strtok(NULL, " ");
413      i2 = atoi(pIndex);
414      switch(i2) {
415      case 0:
416        Serial.println("opsMode = 0, manual operation");          
417        opsMode = false;
418        break;
419       case 1:
420         Serial.println("opsMode = 1, automatic operation");          
421         opsMode = true;
422         break;
423      default:
424        Serial.println("Request not recognised");    
425      }
426      break;        
427    case 2:
428      if(opsMode) Serial.println("Manual move not possible in automatic operation (when opsMode = true)");
429      else {
430        Serial.println("Motor moving up");   
431        pIndex = strtok(NULL, " ");
432        steps = (uint32_t) atoi(pIndex);
433        jogMotor(steps, (bool) true);
434      }             
435      break; 
436    case 3:
437      if(opsMode) Serial.println("Manual move not possible when opsMode = true");    
438      else {
439        Serial.println("Motor moving down");   
440        pIndex = strtok(NULL, " ");
441        steps = (uint32_t) atoi(pIndex);
442        jogMotor(steps, (bool) false);        
443      }             
444      break; 
445    case 4:
446      Serial.println("Setting current position as HOME");
447      motorPosition = 0;
448      break;           
449    default:
450      Serial.println("Request not recognised");                
451    }
452  }  
453}