servo_tach.ino

#include<Servo.h>
#include <wiring_pulse.h>




static uint32_t PWM_in = PA0;
int servoPin = PA1;
int TACH_PIN = PA9;



int last_move = 0;


static uint32_t min_durationLong_low = (uint32_t)(-1);
static uint32_t max_durationLong_low = 0;
static uint32_t min_durationLong_high = (uint32_t)(-1);
static uint32_t max_durationLong_high = 0;


uint32_t last_pulse = 0;

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
//#include <Adafruit_SSD1306_STM32.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#include <Fonts/FreeMono12pt7b.h>

Servo servo;                   
int pos = 0;  


#include <neotimer.h>

Neotimer moving_timer = Neotimer(1000);
Neotimer display_timer = Neotimer(200);
Neotimer pwm_read_timer = Neotimer(100);
boolean moving = false;





uint32_t channelRising, channelFalling;
volatile uint32_t PFrequencyMeasured, DutycycleMeasured, LastPeriodCapture = 0, PCurrentCapture, HighStateMeasured;
uint32_t Pinput_freq = 0;
volatile uint32_t ProlloverCompareCount = 0;
HardwareTimer *PTim;
uint32_t P_last_roll = 0;

/**
    @brief  Input capture interrupt callback : Compute frequency and dutycycle of input signal
*/
void TIMINPUT_Capture_Rising_IT_callback(HardwareTimer*)
{
  PCurrentCapture = PTim->getCaptureCompare(channelRising);
  /* frequency computation */
  if (PCurrentCapture > LastPeriodCapture)
  {
    //PFrequencyMeasured = Pinput_freq / (PCurrentCapture - LastPeriodCapture);
    DutycycleMeasured = (HighStateMeasured * 100) / (PCurrentCapture - LastPeriodCapture);
  }
  else if (PCurrentCapture <= LastPeriodCapture)
  {
    /* 0x1000 is max overflow value */
    //PFrequencyMeasured = Pinput_freq / (0x10000 + PCurrentCapture - LastPeriodCapture);
    DutycycleMeasured = (HighStateMeasured * 100) / (0x10000 + PCurrentCapture - LastPeriodCapture);
  }

  LastPeriodCapture = PCurrentCapture;
  ProlloverCompareCount = 0;
}

/* In case of timer rollover, frequency is to low to be measured set values to 0
   To reduce minimum frequency, it is possible to increase prescaler. But this is at a cost of precision. */
void PRollover_IT_callback(HardwareTimer*)
{
  ProlloverCompareCount++;

  if (ProlloverCompareCount > 1)
  {
    PFrequencyMeasured = 0;
    DutycycleMeasured = 0;
  }
}

/**
    @brief  Input capture interrupt callback : Compute frequency and dutycycle of input signal
*/
void TIMINPUT_Capture_Falling_IT_callback(HardwareTimer*)
{
  /* prepare DutyCycle computation */
  PCurrentCapture = PTim->getCaptureCompare(channelFalling);

  if (PCurrentCapture > LastPeriodCapture)
  {
    HighStateMeasured = PCurrentCapture - LastPeriodCapture;
  }
  else if (PCurrentCapture <= LastPeriodCapture)
  {
    /* 0x1000 is max overflow value */
    HighStateMeasured = 0x10000 + PCurrentCapture - LastPeriodCapture;
  }
}



void setup_pwm_timer(){

  TIM_TypeDef *Instance2 = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(PWM_in), PinMap_PWM);
  channelRising = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(PWM_in), PinMap_PWM));
  switch (channelRising) {
    case 1:
      channelFalling = 2;
      break;
    case 2:
      channelFalling = 1;
      break;
    case 3:
      channelFalling = 4;
      break;
    case 4:
      channelFalling = 3;
      break;
  }

  // Instantiate HardwareTimer object. Thanks to 'new' instantiation, HardwareTimer is not destructed when setup() function is finished.
  PTim = new HardwareTimer(Instance2);

  // Configure rising edge detection to measure frequency
  PTim->setMode(channelRising, TIMER_INPUT_FREQ_DUTY_MEASUREMENT, PWM_in);

  // With a PrescalerFactor = 1, the minimum frequency value to measure is : TIM counter clock / CCR MAX
  //  = (SystemCoreClock) / 65535
  // Example on Nucleo_L476RG with systemClock at 80MHz, the minimum frequency is around 1,2 khz
  // To reduce minimum frequency, it is possible to increase prescaler. But this is at a cost of precision.
  // The maximum frequency depends on processing of both interruptions and thus depend on board used
  // Example on Nucleo_L476RG with systemClock at 80MHz the interruptions processing is around 10 microseconds and thus Max frequency is around 100kHz
  uint32_t PrescalerFactor = 1;
  PTim->setPrescaleFactor(PrescalerFactor);
  PTim->setOverflow(0x100000); // Max Period value to have the largest possible time to detect rising edge and avoid timer rollover
  //PTim->setOverflow(50000, MICROSEC_FORMAT); // Max Period value to have the largest possible time to detect rising edge and avoid timer rollover
  PTim->attachInterrupt(channelRising, TIMINPUT_Capture_Rising_IT_callback);
  PTim->attachInterrupt(channelFalling, TIMINPUT_Capture_Falling_IT_callback);
  PTim->attachInterrupt(PRollover_IT_callback);

  PTim->resume();

  // Compute this scale factor only once
  Pinput_freq = PTim->getTimerClkFreq() / PTim->getPrescaleFactor();
}


// Tachometer

volatile uint32_t spindle_pulses = 0;
void sig_detect(){
  spindle_pulses += 1;
}
uint32_t get_rpm(){
  return spindle_pulses * 60;
}


void setup() {
  // put your setup code here, to run once:
  //servo.attach(servoPin,0,360); 
  attachInterrupt(TACH_PIN, sig_detect, FALLING);
  servo.attach(servoPin);
  servo.write(0);
  //pinMode(in, INPUT);
  Serial.begin(115200);
  delay(1000);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  display.clearDisplay();

  // draw a single pixel
  display.drawPixel(10, 10, WHITE);
  // Show the display buffer on the hardware.
  // NOTE: You _must_ call display after making any drawing commands
  // to make them visible on the display hardware!
  //display.cp437(true);
  //display.setFont(&FreeMono12pt7b);
  display.setCursor(15,15);
  display.println("Startup");
  display.display();
  
  
  //setup_tach_timer();
  setup_pwm_timer();
}

void startMoving(int val){ 
  int delta = abs(val - last_move);
  if(delta > 20 ){
    if (val == 0){
      if (ProlloverCompareCount == P_last_roll){
        Serial.print(" perhaps a real zero");
      }
      else{
        Serial.print("maybe rollover");
        Serial.println(ProlloverCompareCount);
        P_last_roll = ProlloverCompareCount;
      }
    }
    if (val < 20) val = 0;
    Serial.print("starting move pos: ");
    Serial.print(val);
    Serial.print(" d:");
    Serial.println(delta);
    
    servo.write(val);
    moving_timer.start();
    moving = true;
    last_move = val;
  }
  
}

void finishMoving(){
  if(moving){
    if (moving_timer.done()){
      Serial.println("stopping move");
      moving = false;
    }
  }
}

int cnt = 0;
int sval = 0;
uint32_t durationLong = 0;
void loop(){
  

  if(pwm_read_timer.repeat()){
    
    if((last_pulse != DutycycleMeasured) && !moving ){
      //Serial.println(DutycycleMeasured);
      sval = map(DutycycleMeasured,0,100,0,300);
      //Serial.println(sval);
      //servo.attach(servoPin);
      startMoving(sval);
        
      //servo.detach();
      last_pulse = DutycycleMeasured;
      
    }
  }
  
   //Serial.println((String)"Frequency = " + FrequencyMeasured);
   if(display_timer.repeat()){
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0,0);
    display.print("d: ");
    display.print(DutycycleMeasured);
    display.print(" p: ");
    display.print(sval);
    display.print(" s: ");
    display.println(last_pulse);
    display.setCursor(0,16);
    display.print(" M: ");
    display.print(moving);
    display.print(" DC:");
    display.print(DutycycleMeasured);
    display.print(" RPM: ");
    display.println(get_rpm());
    display.display();
    
   }
   finishMoving();
}