jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Working test-program using user LEDs and delays

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-22 21:20:31 +01:00
parent 7bff3f3789
commit 6383d6de89
3 changed files with 118 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
Firmware/include/StepGen3.h
View File

@@ -7,96 +7,51 @@
#define MAX_CYCLE 18
#define USER_STEP_TYPE 13
typedef int hal_s32_t;
typedef int hal_bit_t;
typedef unsigned int hal_u32_t;
typedef float hal_float_t;
typedef struct
{
/* stuff that is both read and written by makepulses */
unsigned int timer1; /* times out when step pulse should end */
unsigned int timer2; /* times out when safe to change dir */
unsigned int timer3; /* times out when safe to step in new dir */
int hold_dds; /* prevents accumulator from updating */
long addval; /* actual frequency generator add value */
volatile long long accum; /* frequency generator accumulator */
hal_s32_t rawcount; /* param: position feedback in counts */
int curr_dir; /* current direction */
int state; /* current position in state table */
volatile unsigned int timer1; /* times out when step pulse should end */
volatile unsigned int timer2; /* times out when safe to change dir */
volatile unsigned int timer3; /* times out when safe to step in new dir */
volatile int hold_dds; /* prevents accumulator from updating */
volatile long addval; /* actual frequency generator add value */
volatile long long accum; /* frequency generator accumulator */
volatile int rawcount; /* param: position feedback in counts */
volatile int curr_dir; /* current direction */
volatile int state; /* current position in state table */
/* stuff that is read but not written by makepulses */
hal_bit_t enable; /* pin for enable stepgen */
long target_addval; /* desired freq generator add value */
long deltalim; /* max allowed change per period */
hal_u32_t step_len; /* parameter: step pulse length */
hal_u32_t dir_hold_dly; /* param: direction hold time or delay */
hal_u32_t dir_setup; /* param: direction setup time */
int step_type; /* stepping type - see list above */
int cycle_max; /* cycle length for step types 2 and up */
int num_phases; /* number of phases for types 2 and up */
hal_bit_t phase[5]; /* pins for output signals */
const unsigned char *lut; /* pointer to state lookup table */
volatile int enable; /* pin for enable stepgen */
volatile int target_addval; /* desired freq generator add value */
volatile long deltalim; /* max allowed change per period */
volatile int step_len; /* parameter: step pulse length */
volatile unsigned int dir_hold_dly; /* param: direction hold time or delay */
volatile unsigned int dir_setup; /* param: direction setup time */
volatile int step_type; /* stepping type - see list above */
volatile int cycle_max; /* cycle length for step types 2 and up */
volatile int num_phases; /* number of phases for types 2 and up */
volatile int phase[5]; /* pins for output signals */
volatile const unsigned char *lut; /* pointer to state lookup table */
/* stuff that is not accessed by makepulses */
int pos_mode; /* 1 = position mode, 0 = velocity mode */
hal_u32_t step_space; /* parameter: min step pulse spacing */
double old_pos_cmd; /* previous position command (counts) */
hal_s32_t count; /* pin: captured feedback in counts */
hal_float_t pos_scale; /* param: steps per position unit */
double old_scale; /* stored scale value */
double scale_recip; /* reciprocal value used for scaling */
hal_float_t vel_cmd; /* pin: velocity command (pos units/sec) */
hal_float_t pos_cmd; /* pin: position command (position units) */
hal_float_t pos_fb; /* pin: position feedback (position units) */
hal_float_t freq; /* param: frequency command */
hal_float_t maxvel; /* param: max velocity, (pos units/sec) */
hal_float_t maxaccel; /* param: max accel (pos units/sec^2) */
hal_u32_t old_step_len; /* used to detect parameter changes */
hal_u32_t old_step_space;
hal_u32_t old_dir_hold_dly;
hal_u32_t old_dir_setup;
int printed_error; /* flag to avoid repeated printing */
volatile int pos_mode; /* 1 = position mode, 0 = velocity mode */
volatile unsigned int step_space; /* parameter: min step pulse spacing */
volatile double old_pos_cmd; /* previous position command (counts) */
volatile int count; /* pin: captured feedback in counts */
volatile double pos_scale; /* param: steps per position unit */
double old_scale; /* stored scale value */
double scale_recip; /* reciprocal value used for scaling */
volatile double vel_cmd; /* pin: velocity command (pos units/sec) */
volatile double pos_cmd; /* pin: position command (position units) */
volatile double pos_fb; /* pin: position feedback (position units) */
volatile double freq; /* param: frequency command */
volatile double maxvel; /* param: max velocity, (pos units/sec) */
volatile double maxaccel; /* param: max accel (pos units/sec^2) */
volatile unsigned int old_step_len; /* used to detect parameter changes */
volatile unsigned int old_step_space;
volatile unsigned int old_dir_hold_dly;
volatile unsigned int old_dir_setup;
volatile int printed_error; /* flag to avoid repeated printing */
} stepgen_t;
/* lookup tables for stepping types 2 and higher - phase A is the LSB */
static unsigned char master_lut[][MAX_CYCLE] = {
{1, 3, 2, 0, 0, 0, 0, 0, 0, 0}, /* type 2: Quadrature */
{1, 2, 4, 0, 0, 0, 0, 0, 0, 0}, /* type 3: Three Wire */
{1, 3, 2, 6, 4, 5, 0, 0, 0, 0}, /* type 4: Three Wire Half Step */
{1, 2, 4, 8, 0, 0, 0, 0, 0, 0}, /* 5: Unipolar Full Step 1 */
{3, 6, 12, 9, 0, 0, 0, 0, 0, 0}, /* 6: Unipoler Full Step 2 */
{1, 7, 14, 8, 0, 0, 0, 0, 0, 0}, /* 7: Bipolar Full Step 1 */
{5, 6, 10, 9, 0, 0, 0, 0, 0, 0}, /* 8: Bipoler Full Step 2 */
{1, 3, 2, 6, 4, 12, 8, 9, 0, 0}, /* 9: Unipolar Half Step */
{1, 5, 7, 6, 14, 10, 8, 9, 0, 0}, /* 10: Bipolar Half Step */
{1, 2, 4, 8, 16, 0, 0, 0, 0, 0}, /* 11: Five Wire Unipolar */
{3, 6, 12, 24, 17, 0, 0, 0, 0, 0}, /* 12: Five Wire Wave */
{1, 3, 2, 6, 4, 12, 8, 24, 16, 17}, /* 13: Five Wire Uni Half */
{3, 7, 6, 14, 12, 28, 24, 25, 17, 19}, /* 14: Five Wire Wave Half */
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* 15: User-defined */
};
static unsigned char cycle_len_lut[] =
{4, 3, 6, 4, 4, 4, 4, 8, 8, 5, 5, 10, 10, 0};
static unsigned char num_phases_lut[] =
{
2,
3,
3,
4,
4,
4,
4,
4,
4,
5,
5,
5,
5,
0,
};
#define MAX_STEP_TYPE 15
#define STEP_PIN 0 /* output phase used for STEP signal */
@@ -116,21 +71,21 @@ typedef enum CONTROL
class StepGen3
{
public:
int step_type[MAX_CHAN] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // stepping types for up to 16 channels
char *ctrl_type[MAX_CHAN] = {0}; // control type ("p"pos or "v"vel) for up to 16 channels
int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type
volatile int step_type[MAX_CHAN] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // stepping types for up to 16 channels
char *ctrl_type[MAX_CHAN] = {0}; // control type ("p"pos or "v"vel) for up to 16 channels
volatile int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type
stepgen_t *stepgen_array;
int num_chan = 0; // number of step generators configured */
long periodns; // makepulses function period in nanosec */
long old_periodns; // used to detect changes in periodns */
static double periodfp; // makepulses function period in seconds */
double freqscale; // conv. factor from Hz to addval counts */
double accelscale; // conv. Hz/sec to addval cnts/period */
long old_dtns; // update_freq funct period in nsec */
double dt; // update_freq period in seconds */
double recip_dt; // recprocal of period, avoids divides */
volatile int num_chan = 0; // number of step generators configured */
volatile long periodns; // makepulses function period in nanosec */
volatile long old_periodns; // used to detect changes in periodns */
volatile double periodfp; // makepulses function period in seconds */
volatile double freqscale; // conv. factor from Hz to addval counts */
volatile double accelscale; // conv. Hz/sec to addval cnts/period */
volatile long old_dtns; // update_freq funct period in nsec */
volatile double dt; // update_freq period in seconds */
volatile double recip_dt; // recprocal of period, avoids divides */
StepGen3(void);
int rtapi_app_main();
@@ -141,6 +96,47 @@ public:
int setup_user_step_type(void);
CONTROL parse_ctrl_type(const char *ctrl);
unsigned long ulceil(unsigned long value, unsigned long increment);
private:
/* lookup tables for stepping types 2 and higher - phase A is the LSB */
unsigned char master_lut[MAX_STEP_TYPE][MAX_CYCLE] = {
{1, 3, 2, 0, 0, 0, 0, 0, 0, 0}, /* type 2: Quadrature */
{1, 2, 4, 0, 0, 0, 0, 0, 0, 0}, /* type 3: Three Wire */
{1, 3, 2, 6, 4, 5, 0, 0, 0, 0}, /* type 4: Three Wire Half Step */
{1, 2, 4, 8, 0, 0, 0, 0, 0, 0}, /* 5: Unipolar Full Step 1 */
{3, 6, 12, 9, 0, 0, 0, 0, 0, 0}, /* 6: Unipoler Full Step 2 */
{1, 7, 14, 8, 0, 0, 0, 0, 0, 0}, /* 7: Bipolar Full Step 1 */
{5, 6, 10, 9, 0, 0, 0, 0, 0, 0}, /* 8: Bipoler Full Step 2 */
{1, 3, 2, 6, 4, 12, 8, 9, 0, 0}, /* 9: Unipolar Half Step */
{1, 5, 7, 6, 14, 10, 8, 9, 0, 0}, /* 10: Bipolar Half Step */
{1, 2, 4, 8, 16, 0, 0, 0, 0, 0}, /* 11: Five Wire Unipolar */
{3, 6, 12, 24, 17, 0, 0, 0, 0, 0}, /* 12: Five Wire Wave */
{1, 3, 2, 6, 4, 12, 8, 24, 16, 17}, /* 13: Five Wire Uni Half */
{3, 7, 6, 14, 12, 28, 24, 25, 17, 19}, /* 14: Five Wire Wave Half */
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* 15: User-defined */
};
unsigned char cycle_len_lut[14] =
{4, 3, 6, 4, 4, 4, 4, 8, 8, 5, 5, 10, 10, 0};
unsigned char num_phases_lut[14] =
{
2,
3,
3,
4,
4,
4,
4,
4,
4,
5,
5,
5,
5,
0,
};
};
#endif

9
Firmware/src/StepGen3.cpp
View File

@@ -1100,7 +1100,7 @@ StepGen3::StepGen3(void)
step->pos_scale = JOINT_0_SCALE;
step->maxaccel = JOINT_0_STEPGEN_MAXACCEL;
for (int servo_thread = 0; servo_thread < 00; servo_thread++)
for (int servo_thread = 0; servo_thread < 100; servo_thread++)
{
step->pos_cmd = servo_thread * 1;
update_pos(step, SERVO_PERIOD);
@@ -1108,10 +1108,13 @@ StepGen3::StepGen3(void)
for (int base_thread = 0; base_thread < 20; base_thread++)
{
make_pulses(stepgen_array, BASE_PERIOD);
digitalWrite(PA6, step->phase[DIR_PIN] ? LOW : HIGH);
digitalWrite(PA7, step->phase[STEP_PIN] ? LOW : HIGH);
delay(50);
// printf("pos_cmd=%f\n", step->pos_cmd);
// printf("pos_fb=%f\n", step->pos_fb);
printf("pickoff=%d accum=%lld addval=%d ", 1 << 28, step->accum, step->addval);
printf("dir=%d step=%d\n", step->phase[DIR_PIN], step->phase[STEP_PIN]);
// printf("pickoff=%d accum=%lld addval=%d ", 1 << 28, step->accum, step->addval);
// printf("dir=%d step=%d\n", step->phase[DIR_PIN], step->phase[STEP_PIN]);
}
}
}

26
Firmware/src/main.cpp
View File

@@ -8,6 +8,10 @@ extern "C"
_Objects Obj;
HardwareSerial Serial1(PA10, PA9);
// STG = StepGenTest
#define SGT 0
#if SGT
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
#define DEBUG_TIM8 1
#include "MyEncoder.h"
@@ -17,8 +21,9 @@ void indexPulseEncoderCB1(void)
{
Encoder1.indexPulse();
}
#endif
#include "StepGen3.h"
#if SGT
#include "extend32to64.h"
CircularBuffer<uint64_t, 200> Tim;
@@ -40,7 +45,6 @@ uint64_t reallyNowTime = 0, reallyThenTime = 0;
uint64_t timeDiff; // Timediff in nanoseconds
void handleStepper(void)
{
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -66,7 +70,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
}
thenTime = irqTime;
Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
Obj.D1 =0;
Obj.D1 = 0;
Obj.D2 = 0;
Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
Obj.D4 = 0;
@@ -98,16 +102,25 @@ static esc_cfg_t config =
};
volatile byte serveIRQ = 0;
#endif
void setup(void)
{
Serial1.begin(115200);
#if SGT
rcc_config(); // probably breaks some timers.
ecat_slv_init(&config);
#endif
pinMode(PA6, OUTPUT);
pinMode(PA7, OUTPUT);
digitalWrite(PA6, HIGH);
digitalWrite(PA7, HIGH);
}
void loop(void)
{
StepGen3 Step;
#if SGT
uint64_t dTime;
if (serveIRQ)
{
@@ -124,16 +137,19 @@ void loop(void)
dTime = longTime.extendTime(micros()) - irqTime;
if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt
ecat_slv_poll();
#endif
}
void sync0Handler(void)
{
#if SGT
ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{
serveIRQ = 1;
irqTime = longTime.extendTime(micros());
}
#endif
}
// Enable SM2 interrupts
@@ -183,6 +199,6 @@ uint16_t dc_checker(void)
{
// Indicate we run DC
ESCvar.dcsync = 1;
//StepGen3::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs
// StepGen3::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs
return 0;
}