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

9
Firmware/src/StepGen3.cpp
View File

@@ -1100,7 +1100,7 @@ StepGen3::StepGen3(void)
step->pos_scale = JOINT_0_SCALE; step->pos_scale = JOINT_0_SCALE;
step->maxaccel = JOINT_0_STEPGEN_MAXACCEL; 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; step->pos_cmd = servo_thread * 1;
update_pos(step, SERVO_PERIOD); update_pos(step, SERVO_PERIOD);
@@ -1108,10 +1108,13 @@ StepGen3::StepGen3(void)
for (int base_thread = 0; base_thread < 20; base_thread++) for (int base_thread = 0; base_thread < 20; base_thread++)
{ {
make_pulses(stepgen_array, BASE_PERIOD); 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_cmd=%f\n", step->pos_cmd);
// printf("pos_fb=%f\n", step->pos_fb); // printf("pos_fb=%f\n", step->pos_fb);
printf("pickoff=%d accum=%lld addval=%d ", 1 << 28, step->accum, step->addval); // 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("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; _Objects Obj;
HardwareSerial Serial1(PA10, PA9); HardwareSerial Serial1(PA10, PA9);
// STG = StepGenTest
#define SGT 0
#if SGT
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
#define DEBUG_TIM8 1 #define DEBUG_TIM8 1
#include "MyEncoder.h" #include "MyEncoder.h"
@@ -17,8 +21,9 @@ void indexPulseEncoderCB1(void)
{ {
Encoder1.indexPulse(); Encoder1.indexPulse();
} }
#endif
#include "StepGen3.h" #include "StepGen3.h"
#if SGT
#include "extend32to64.h" #include "extend32to64.h"
CircularBuffer<uint64_t, 200> Tim; CircularBuffer<uint64_t, 200> Tim;
@@ -40,7 +45,6 @@ uint64_t reallyNowTime = 0, reallyThenTime = 0;
uint64_t timeDiff; // Timediff in nanoseconds uint64_t timeDiff; // Timediff in nanoseconds
void handleStepper(void) void handleStepper(void)
{ {
} }
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation 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; thenTime = irqTime;
Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
Obj.D1 =0; Obj.D1 = 0;
Obj.D2 = 0; Obj.D2 = 0;
Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos(); Obj.D3 = abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
Obj.D4 = 0; Obj.D4 = 0;
@@ -98,16 +102,25 @@ static esc_cfg_t config =
}; };
volatile byte serveIRQ = 0; volatile byte serveIRQ = 0;
#endif
void setup(void) void setup(void)
{ {
Serial1.begin(115200); Serial1.begin(115200);
#if SGT
rcc_config(); // probably breaks some timers. rcc_config(); // probably breaks some timers.
ecat_slv_init(&config); ecat_slv_init(&config);
#endif
pinMode(PA6, OUTPUT);
pinMode(PA7, OUTPUT);
digitalWrite(PA6, HIGH);
digitalWrite(PA7, HIGH);
} }
void loop(void) void loop(void)
{ {
StepGen3 Step;
#if SGT
uint64_t dTime; uint64_t dTime;
if (serveIRQ) if (serveIRQ)
{ {
@@ -124,16 +137,19 @@ void loop(void)
dTime = longTime.extendTime(micros()) - irqTime; dTime = longTime.extendTime(micros()) - irqTime;
if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt
ecat_slv_poll(); ecat_slv_poll();
#endif
} }
void sync0Handler(void) void sync0Handler(void)
{ {
#if SGT
ALEventIRQ = ESC_ALeventread(); ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2) // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{ {
serveIRQ = 1; serveIRQ = 1;
irqTime = longTime.extendTime(micros()); irqTime = longTime.extendTime(micros());
} }
#endif
} }
// Enable SM2 interrupts // Enable SM2 interrupts
@@ -183,6 +199,6 @@ uint16_t dc_checker(void)
{ {
// Indicate we run DC // Indicate we run DC
ESCvar.dcsync = 1; ESCvar.dcsync = 1;
//StepGen3::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs // StepGen3::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs
return 0; return 0;
} }