I'm currently working on a closed loop stepper, based on a AS5600.
I need to read external input signals at high frequency and have no clue how to get it running.
if I use interrupts the MCU does not count accuratly if the speed of the external board increases.
I tried an example with a modified PWM counter but if i add the Code for the encoder the counter reads are not fast enough.
Code: Select all
#include <Arduino.h>
#include <TMCStepper.h>
#include <Wire.h>
#include <AS5600.h>
#include <STM32FreeRTOS.h>
#if !defined(STM32_CORE_VERSION) || (STM32_CORE_VERSION < 0x01090000)
#error "Due to API change, this sketch is compatible with STM32_CORE_VERSION >= 0x01090000"
#endif
#define EX_STEP_PIN PA8
#define EN_PIN PB9
#define DIR_PIN PA12
#define STEP_PIN PA11
#define SW_TX PB4
#define SW_RX PB4
#define DRIVER_ADDRESS 0b00
#define EX_EN_PIN PB13
#define R_SENSE 0.11f
#define SERIAL Serial
#define EX_DIR_PIN PB3
#define REGISTER_MEM_LOCATION 0x20000200
uint32_t channel;
volatile int Counter;
uint32_t input_freq = 0;
int32_t absolutAngle = 0;
int magnetStatus = 0;
int lowbyte;
word highbyte;
int rawAngle;
float degAngle;
int quadrantNumber, previousquadrantNumber;
float numberofTurns = 0;
float correctedAngle = 0;
float startAngle = 0;
float startPoint = 0;
bool started = false;
float totalAngle = 0;
float FullStepsperRev = 200;
int startStepsC = 0;
uint16_t RMS_Current = 600;
uint8_t MStepping = 16;
float RawToStep = 4096/(FullStepsperRev*MStepping);
int totalAngleInt = 0;
HardwareTimer *MyTim;
TMC2209Stepper driver(SW_RX, SW_TX, R_SENSE, DRIVER_ADDRESS);
AS5600 as5600;
//SemaphoreHandle_t mutexHandle;
SemaphoreHandle_t xSemaphore;
void ReadRawAngle( void *pvParameters );
void correctAngle( void *pvParameters );
void checkQuadrant( void *pvParameters );
void PrintOutput( void *pvParameters );
void InputCapture_IT_callback(void)
{
/*
// Increment the counter using direct register access
__asm__("ldr r0, =Counter\n\t" // Load the address of "counter" into R0
"ldr r1, [r0]\n\t" // Load the value of "counter" into R1
"add r1, r1, #1\n\t" // Increment the value of R1 by 1
"str r1, [r0]\n\t"); // Store the value of R1 back into "counter"
}
// Enter critical section
/*///taskENTER_CRITICAL();
if (xSemaphoreTakeFromISR(xSemaphore, NULL) == pdTRUE){
// Mutex acquired, we can access the "Counter" variable safely
if (digitalReadFast(digitalPinToPinName(EX_DIR_PIN)) == HIGH){(*((volatile uint32_t *)REGISTER_MEM_LOCATION)) ++;}
if (digitalReadFast(digitalPinToPinName(EX_DIR_PIN)) == LOW){(*((volatile uint32_t *)REGISTER_MEM_LOCATION)) --;}
xSemaphoreGiveFromISR(xSemaphore, NULL);
//taskEXIT_CRITICAL();
}
}
void setup()
{
Serial.begin(115200);
SERIAL.begin(115200);
SERIAL.println("Serial Started ");
Wire.begin();
pinMode(EN_PIN, OUTPUT);
pinMode(STEP_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);
pinMode(EX_DIR_PIN, INPUT);
xSemaphore = xSemaphoreCreateMutex();
*((volatile uint32_t *)REGISTER_MEM_LOCATION) = 0;
TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(digitalPinToPinName(EX_STEP_PIN), PinMap_PWM);
channel = STM_PIN_CHANNEL(pinmap_function(digitalPinToPinName(EX_STEP_PIN), PinMap_PWM));
MyTim = new HardwareTimer(Instance);
MyTim->setMode(channel, TIMER_INPUT_CAPTURE_RISING, EX_STEP_PIN);
uint32_t PrescalerFactor = 1;
MyTim->setPrescaleFactor(PrescalerFactor);
MyTim->setOverflow(0x100000);
MyTim->attachInterrupt(channel, InputCapture_IT_callback);
MyTim->resume();
//input_freq = MyTim->getTimerClkFreq() / MyTim->getPrescaleFactor();
xTaskCreate(
EnableDriver
, (const portCHAR *)"EnableDriver"
, 128
, NULL
, -1
, NULL );
/*xTaskCreate(
ReadRawAngle
, (const portCHAR *)"ReadRawAngle"
, 56
, NULL
, 2
, NULL );
*/
xTaskCreate(
correctAngle
, (const portCHAR *) "correctAngle"
, 128
, NULL
, 2
, NULL );
xTaskCreate(
checkQuadrant
, (const portCHAR *) "checkQuadrant"
, 72
, NULL
, 1
, NULL );
xTaskCreate(
CorrectStartPos
, (const portCHAR *) "CorrectStartPos"
, 128
, NULL
, 0
, NULL );
xTaskCreate(
PrintOutput
, (const portCHAR *) "PrintOutput"
, 72
, NULL
, 0
, NULL );
vTaskStartScheduler();
}
void EnableDriver(void *pvParameters __attribute__((unused))){
driver.beginSerial(11520);
driver.pdn_disable(false);
driver.I_scale_analog(false);
driver.rms_current(600, 0.5);
driver.mstep_reg_select(true);
driver.mres(8);
driver.microsteps(MStepping);
driver.toff(2);
digitalWrite(EN_PIN, LOW);
driver.en_spreadCycle(true);
driver.pwm_autoscale(true);
SERIAL.println("begin driver conf");
SERIAL.println("driver Status:");
SERIAL.println(driver.DRV_STATUS());
SERIAL.println("GCONF:");
SERIAL.println(driver.GCONF());
SERIAL.println("GSTAT:");
SERIAL.println(driver.GSTAT());
SERIAL.println("drv_err:");
SERIAL.println(driver.drv_err());
SERIAL.println("microsteps:");
SERIAL.println(driver.microsteps());
SERIAL.println("rms_current:");
SERIAL.println(driver.rms_current());
Wire.begin();
if (as5600.detectMagnet() == 0){
Serial.println(" NO Magnet ");
}
if (as5600.detectMagnet() == 1){
Serial.println("Magnet detected ");
}
vTaskSuspend(NULL);
}
void ReadRawAngle(void *pvParameters __attribute__((unused))){
for (;;)
{
/*Wire.beginTransmission(0x36); //connect to the sensor
Wire.write(0x0D); //figure 21 - register map: Raw angle (7:0)
Wire.endTransmission(); //end transmission
Wire.requestFrom(0x36, 1); //request from the sensor
while (Wire.available() == 0); //wait until it becomes available
lowbyte = Wire.read(); //Reading the data after the request
Wire.beginTransmission(0x36);
Wire.write(0x0C); //figure 21 - register map: Raw angle (11:8)
Wire.endTransmission();
Wire.requestFrom(0x36, 1);
while (Wire.available() == 0);
highbyte = Wire.read();
highbyte = highbyte << 8; //shifting to left
rawAngle = highbyte | lowbyte; //int is 16 bits (as well as the word)
degAngle = rawAngle;
*/
//Serial.println("ReadRawAngle");
//delay(1);
vTaskDelay(10);
}
}
void correctAngle(void *pvParameters __attribute__((unused))){
for (;;) // A Task shall never return or exit.
{
degAngle = as5600.rawAngle();
correctedAngle = degAngle - startAngle;
if (correctedAngle < 0){
correctedAngle = correctedAngle + 4096;
}
else {}
/*TaskHandle_t taskHandle = xTaskGetCurrentTaskHandle();
UBaseType_t stackHighWaterMark = uxTaskGetStackHighWaterMark(taskHandle);
Serial.println(String("stackHighWaterMark of : correctAngle ")+stackHighWaterMark);*/
vTaskDelay(12);
}
}
void checkQuadrant(void *pvParameters __attribute__((unused))){
for (;;) // A Task shall never return or exit.
{
if (correctedAngle >= 0 && correctedAngle <= 4096/3){quadrantNumber = 1; }
if (correctedAngle > 4096/3 && correctedAngle <= 4096/3*2){quadrantNumber = 2;}
if (correctedAngle > 4096/3*2 && correctedAngle <= 4096){quadrantNumber = 3;}
if (quadrantNumber != previousquadrantNumber) //if we changed quadrant
{
if (quadrantNumber == 1 && previousquadrantNumber == 3){numberofTurns++;} // 4 --> 1 transition: CW rotation
if (quadrantNumber == 3 && previousquadrantNumber == 1){ numberofTurns--;} // 1 --> 4 transition: CCW rotation
//this could be done between every quadrants so one can count every 1/4th of transition
previousquadrantNumber = quadrantNumber; //update to the current quadrant
}
totalAngle = (numberofTurns * 4096/RawToStep) + (correctedAngle/RawToStep);
totalAngle = totalAngle + startPoint;
totalAngleInt = (int)totalAngle;
while (started == false){
startPoint = startPoint - totalAngle;
started = true;
}
/*TaskHandle_t taskHandle = xTaskGetCurrentTaskHandle();
UBaseType_t stackHighWaterMark = uxTaskGetStackHighWaterMark(taskHandle);
Serial.println(String("stackHighWaterMark of : checkQuadrant ")+stackHighWaterMark);*/
vTaskDelay(10);
}
}
void CorrectStartPos(void *pvParameters __attribute__((unused))){
vTaskDelay(1000);
startStepsC = totalAngleInt;
vTaskSuspend(NULL);
}
void PrintOutput(void *pvParameters __attribute__((unused))){
for (;;) // A Task shall never return or exit.
{
Counter = *((volatile uint32_t *)REGISTER_MEM_LOCATION);
Serial.println(String("Total Steps Done: ") + (totalAngleInt-startStepsC) +String(" startStepsC Counter: ") + startStepsC +String(" Step Counter: ") + Counter );
//Serial.println(String(" Step Counter: ") + Counter );
/*TaskHandle_t taskHandle = xTaskGetCurrentTaskHandle();
UBaseType_t stackHighWaterMark = uxTaskGetStackHighWaterMark(taskHandle);
Serial.println(String("stackHighWaterMark of : PrintOutput ")+stackHighWaterMark);*/
vTaskDelay(1000);
}
}
void loop() {}