322 lines
9.8 KiB
C++
Executable File
322 lines
9.8 KiB
C++
Executable File
#include <Arduino.h>
|
|
#include <HardwareTimer.h>
|
|
|
|
#include <stdio.h>
|
|
extern "C"
|
|
{
|
|
#include "ecat_slv.h"
|
|
#include "utypes.h"
|
|
};
|
|
#include <CircularBuffer.h>
|
|
#define RINGBUFFERLEN 101
|
|
CircularBuffer<double_t, RINGBUFFERLEN> Pos;
|
|
CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
|
|
|
|
#include <Stm32F4_Encoder.h>
|
|
int64_t PreviousEncoderCounterValue = 0;
|
|
int64_t unwrap_encoder(uint16_t in, int64_t *prev);
|
|
Encoder EncoderInit;
|
|
Encoder *encP = &EncoderInit;
|
|
|
|
#define INDEX_PIN PA2
|
|
HardwareSerial Serial1(PA10, PA9);
|
|
_Objects Obj;
|
|
|
|
void indexPulse(void);
|
|
double PosScaleRes = 1.0;
|
|
uint32_t CurPosScale = 1;
|
|
uint8_t OldLatchCEnable = 0;
|
|
volatile uint8_t indexPulseFired = 0;
|
|
volatile uint8_t pleaseZeroTheCounter = 0;
|
|
|
|
#define STEPPER_DIR_PIN PA12
|
|
#define STEPPER_STEP_PIN PA11
|
|
HardwareTimer *MyTim;
|
|
volatile uint32_t stepCount = 0, stepPulses = 0;
|
|
volatile double_t actualPosition = 0;
|
|
volatile double_t requestedPosition, requestedVelocity;
|
|
|
|
uint32_t sync0CycleTime = 0; // nanoseconds
|
|
|
|
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
|
|
{
|
|
if (Obj.IndexLatchEnable && !OldLatchCEnable) // Should only happen first time IndexCEnable is set
|
|
{
|
|
pleaseZeroTheCounter = 1;
|
|
}
|
|
OldLatchCEnable = Obj.IndexLatchEnable;
|
|
|
|
if (CurPosScale != Obj.EncPosScale && Obj.EncPosScale != 0)
|
|
{
|
|
CurPosScale = Obj.EncPosScale;
|
|
PosScaleRes = 1.0 / double(CurPosScale);
|
|
}
|
|
requestedPosition = Obj.StepGenIn1.CommandedPosition;
|
|
requestedVelocity = Obj.StepGenIn1.CommandedVelocity;
|
|
}
|
|
|
|
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
|
|
{
|
|
Obj.IndexStatus = 0;
|
|
if (indexPulseFired)
|
|
{
|
|
Obj.IndexStatus = 1;
|
|
indexPulseFired = 0;
|
|
PreviousEncoderCounterValue = 0;
|
|
}
|
|
// Obj.DiffT = sync0CycleTime;
|
|
|
|
int64_t pos = unwrap_encoder(TIM2->CNT, &PreviousEncoderCounterValue);
|
|
double CurPos = pos * PosScaleRes;
|
|
Obj.EncPos = CurPos;
|
|
|
|
double diffT = 0;
|
|
double diffPos = 0;
|
|
TDelta.push(ESCvar.Time); // Running average over the length of the circular buffer
|
|
Pos.push(CurPos);
|
|
if (Pos.size() >= 2)
|
|
{
|
|
diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
|
|
diffPos = fabs(Pos.last() - Pos.first());
|
|
}
|
|
Obj.EncFrequency = diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
|
|
|
|
Obj.IndexByte = digitalRead(INDEX_PIN);
|
|
if (Obj.IndexByte)
|
|
Serial1.printf("IS 1\n");
|
|
|
|
Obj.StepGenOut1.ActualPosition = actualPosition;
|
|
Obj.DiffT = 10000 * requestedPosition; // deltaT;
|
|
}
|
|
|
|
void ESC_interrupt_enable(uint32_t mask);
|
|
void ESC_interrupt_disable(uint32_t mask);
|
|
uint16_t dc_checker(void);
|
|
void TimerStep_CB(void);
|
|
void sync0Handler(void);
|
|
void handleStepper(void);
|
|
void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* nr of pulses to do*/);
|
|
|
|
static esc_cfg_t config =
|
|
{
|
|
.user_arg = NULL,
|
|
.use_interrupt = 1,
|
|
.watchdog_cnt = 150,
|
|
.set_defaults_hook = NULL,
|
|
.pre_state_change_hook = NULL,
|
|
.post_state_change_hook = NULL,
|
|
.application_hook = handleStepper,
|
|
.safeoutput_override = NULL,
|
|
.pre_object_download_hook = NULL,
|
|
.post_object_download_hook = NULL,
|
|
.rxpdo_override = NULL,
|
|
.txpdo_override = NULL,
|
|
.esc_hw_interrupt_enable = ESC_interrupt_enable,
|
|
.esc_hw_interrupt_disable = ESC_interrupt_disable,
|
|
.esc_hw_eep_handler = NULL,
|
|
.esc_check_dc_handler = dc_checker,
|
|
};
|
|
|
|
void sync0Handler(void);
|
|
volatile byte serveIRQ = 0;
|
|
|
|
void setup(void)
|
|
{
|
|
Serial1.begin(115200);
|
|
rcc_config();
|
|
|
|
TIM_TypeDef *Instance = TIM1;
|
|
MyTim = new HardwareTimer(Instance);
|
|
MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
|
|
MyTim->attachInterrupt(TimerStep_CB);
|
|
pinMode(STEPPER_DIR_PIN, OUTPUT);
|
|
|
|
// Set starting count value
|
|
EncoderInit.SetCount(Tim2, 0);
|
|
attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // When Index triggered
|
|
// EncoderInit.SetCount(Tim3, 0);
|
|
// EncoderInit.SetCount(Tim4, 0);
|
|
// EncoderInit.SetCount(Tim8, 0);
|
|
|
|
ecat_slv_init(&config);
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
ESCvar.PrevTime = ESCvar.Time;
|
|
if (serveIRQ)
|
|
{
|
|
DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
|
|
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
|
|
serveIRQ = 0;
|
|
}
|
|
ecat_slv_poll();
|
|
}
|
|
|
|
void indexPulse(void)
|
|
{
|
|
if (pleaseZeroTheCounter)
|
|
{
|
|
TIM2->CNT = 0;
|
|
indexPulseFired = 1;
|
|
Pos.clear();
|
|
TDelta.clear();
|
|
pleaseZeroTheCounter = 0;
|
|
}
|
|
}
|
|
|
|
void sync0Handler(void)
|
|
{
|
|
serveIRQ = 1;
|
|
}
|
|
|
|
volatile uint8_t timerIsRunning = 0;
|
|
volatile uint8_t reloadStepperTimer = 0;
|
|
volatile int32_t currentPosition = 0;
|
|
volatile int32_t direction = 1;
|
|
volatile int32_t timerStepPosition = 0;
|
|
volatile int32_t timerStepDirection = 0;
|
|
volatile int32_t timerStepPositionAtEnd = 0;
|
|
|
|
void handleStepper(void)
|
|
{
|
|
int32_t pulsesAtEndOfCycle = 100 * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm
|
|
makePulses(/*sync0CycleTime / 1000*/ 1200, pulsesAtEndOfCycle); // Make the pulses using hardware timer
|
|
actualPosition = requestedPosition;
|
|
}
|
|
|
|
volatile int32_t timerNewEndStepPosition = 0;
|
|
volatile uint64_t timerNewEndTime = 0;
|
|
|
|
void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/)
|
|
{
|
|
if (1 /*!timerIsRunning*/)
|
|
{
|
|
// Start the timer
|
|
int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or -
|
|
if (steps != 0)
|
|
{
|
|
if (abs(steps) * 1000000 / cycleTime > 100000) // 100 kHz is too much for driver, reduce
|
|
{
|
|
int32_t stepsMax = 100000 * cycleTime / 1000000;
|
|
steps = stepsMax * (steps > 0 ? 1 : -1);
|
|
pulsesAtEnd = timerStepPositionAtEnd + steps;
|
|
}
|
|
byte sgn = steps > 0 ? HIGH : LOW;
|
|
digitalWrite(STEPPER_DIR_PIN, sgn);
|
|
uint32_t freq = 1.4 * abs(steps) * 1000000 / cycleTime;
|
|
MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
|
|
MyTim->setOverflow(freq, HERTZ_FORMAT);
|
|
MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 %
|
|
timerStepDirection = steps > 0 ? 1 : -1;
|
|
timerStepPositionAtEnd = pulsesAtEnd; // Current Position
|
|
timerIsRunning = 1;
|
|
MyTim->resume();
|
|
}
|
|
}
|
|
else // Timer is running, reload
|
|
{
|
|
// Set variables, they will be picked up by the timer_CB and the timer is reloaded.
|
|
|
|
timerNewEndStepPosition = pulsesAtEnd;
|
|
timerNewEndTime = micros() + cycleTime;
|
|
}
|
|
}
|
|
|
|
void TimerStep_CB(void)
|
|
{
|
|
timerStepPosition += timerStepDirection; // The step that was just completed
|
|
if (timerNewEndTime != 0) // Are we going to reload?
|
|
{
|
|
// Input for reload is timerNewEndStepPosition and timerNewEndTime
|
|
// The timer has current position and current time and from this
|
|
// can set new frequency and new endtarget for steps
|
|
MyTim->pause();
|
|
int32_t steps = timerNewEndStepPosition - timerStepPosition;
|
|
uint64_t cycleTime = timerNewEndTime - micros();
|
|
byte sgn = steps > 0 ? HIGH : LOW;
|
|
digitalWrite(STEPPER_DIR_PIN, sgn);
|
|
uint32_t freq = abs(steps) * 1000000 / cycleTime;
|
|
MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
|
|
MyTim->setOverflow(freq, HERTZ_FORMAT);
|
|
MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 %
|
|
timerStepDirection = steps > 0 ? 1 : -1;
|
|
timerStepPositionAtEnd = timerNewEndStepPosition;
|
|
timerNewEndStepPosition = 0; // Set to zero to not reload next time
|
|
timerNewEndTime = 0;
|
|
timerIsRunning = 1;
|
|
MyTim->resume();
|
|
}
|
|
if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
|
|
{
|
|
timerIsRunning = 0;
|
|
MyTim->pause();
|
|
}
|
|
}
|
|
|
|
void ESC_interrupt_enable(uint32_t mask)
|
|
{
|
|
// Enable interrupt for SYNC0 or SM2 or SM3
|
|
uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 |
|
|
ESCREG_ALEVENT_SM2 |
|
|
ESCREG_ALEVENT_SM3;
|
|
if (mask & user_int_mask)
|
|
{
|
|
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
|
|
attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
|
|
|
|
// Set LAN9252 interrupt pin driver as push-pull active high
|
|
uint32_t bits = 0x00000111;
|
|
ESC_write(0x54, &bits, 4);
|
|
|
|
// Enable LAN9252 interrupt
|
|
bits = 0x00000001;
|
|
ESC_write(0x5c, &bits, 4);
|
|
}
|
|
}
|
|
|
|
void ESC_interrupt_disable(uint32_t mask)
|
|
{
|
|
// Enable interrupt for SYNC0 or SM2 or SM3
|
|
uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 |
|
|
ESCREG_ALEVENT_SM2 |
|
|
ESCREG_ALEVENT_SM3;
|
|
|
|
if (mask & user_int_mask)
|
|
{
|
|
// Disable interrupt from SYNC0
|
|
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask));
|
|
detachInterrupt(digitalPinToInterrupt(PC3));
|
|
// Disable LAN9252 interrupt
|
|
uint32_t bits = 0x00000000;
|
|
ESC_write(0x5c, &bits, 4);
|
|
}
|
|
}
|
|
|
|
extern "C" uint32_t ESC_SYNC0cycletime(void);
|
|
// Setup of DC
|
|
uint16_t dc_checker(void)
|
|
{
|
|
// Indicate we run DC
|
|
ESCvar.dcsync = 0;
|
|
sync0CycleTime = ESC_SYNC0cycletime();
|
|
return 0;
|
|
}
|
|
#define ONE_PERIOD 65536
|
|
#define HALF_PERIOD 32768
|
|
|
|
int64_t unwrap_encoder(uint16_t in, int64_t *prev)
|
|
{
|
|
int64_t c64 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
|
|
int64_t dif = (c64 - *prev); // core concept: prev + (current - prev) = current
|
|
|
|
// wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
|
|
int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
|
|
if (dif < -HALF_PERIOD)
|
|
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
|
|
|
|
int64_t unwrapped = *prev + mod_dif;
|
|
*prev = unwrapped; // load previous value
|
|
|
|
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
|
|
} |