66 Commits

Author SHA1 Message Date
Hakan Bastedt
b1cc8f1717 Updated README 2024-04-11 12:19:39 +02:00
Hakan Bastedt
55f7e19e88 Updated readme 2024-04-11 11:22:21 +02:00
Hakan Bastedt
220daa4408 updated .gitignore 2024-04-10 10:36:24 +02:00
Hakan Bastedt
37c14850bf Replaced runningaverage (uses malloc) with CircularBuffer 2024-04-10 10:36:01 +02:00
Hakan Bastedt
ed9af2ec50 Reduce irq delay if it has gotten to large 2024-04-09 20:24:16 +02:00
Hakan Bastedt
939470a0c6 Mods to make encoder work again, and a possible 2nd iteration of encoder config 2024-04-09 08:25:59 +02:00
Hakan Bastedt
ec1c8fc70f Enable encoder again. Cleanup of unnecessary variables 2024-04-07 23:27:10 +02:00
Hakan Bastedt
5eda2b451e Use calibrated frequencty for the base timer. No big difference but more scientific, maybe 2024-04-07 22:55:46 +02:00
Hakan Bastedt
226cbc5035 Updated READM.md 2024-04-07 22:54:32 +02:00
Hakan Bastedt
39e2ab31f1 Adaptive (just max over time) maxIrqServeTime 2024-04-07 22:35:52 +02:00
Hakan Bastedt
b164040829 Removed debug-ish compiler directive NEEDED 2024-04-07 22:15:37 +02:00
Hakan Bastedt
084e6891b6 Updated readme 2024-04-07 21:54:21 +02:00
Hakan Bastedt
ed8e4d8d39 Working linuxcnc lathe config with 2 ms cycle time 2024-04-07 21:53:25 +02:00
Hakan Bastedt
a829b0c6e2 Standalone unix port of step generator, also stm32 arduino 2024-04-07 21:51:36 +02:00
Hakan Bastedt
ab1a306d86 Works really nice now. 20 usecs base period => 50 kHz, 25 kHz pulse freq. 2024-04-07 21:45:53 +02:00
Hakan Bastedt
544dd5ed85 I works.
Use 4.19-rt kernel
Ethernet Intel I210 controller
ethtool -C enp1s0 rx-usecs 0 tx-usecs 0
2 ms loop time in linuxcnc .ini and .xml and code
baseTimer period can be adjusted
startTime for baseTimer can also be adjusted.
2024-04-05 18:30:33 +02:00
Hakan Bastedt
511e6442e9 Removed all real work, now just echo 2024-04-05 10:53:12 +02:00
Hakan Bastedt
63fafb4936 FIx 1c13 index manually, nLoops is back 2024-04-05 09:54:56 +02:00
Hakan Bastedt
efa03e3095 Removed hex specifier 0x where it shouldn't be 2024-04-04 16:13:20 +02:00
Hakan Bastedt
dcf6c2d18a jitter flawed, removed 2024-04-04 10:38:27 +02:00
Hakan Bastedt
34e60e43d8 Adaptive max cycle time adjustment, use maxCycleTime 2024-04-03 21:21:51 +02:00
Hakan Bastedt
294831465b Added max irq time and jitter time measurement 2024-04-03 21:17:15 +02:00
Hakan Bastedt
5c128f49b5 syncTimer can be on flawed time units 2024-04-03 20:40:58 +02:00
Hakan Bastedt
9e00caf7c5 Added frequency-calibration to base-thread timer 2024-04-03 20:34:28 +02:00
Hakan Bastedt
1d14e1f295 At least it keeps nLoops at one, always 2024-03-28 10:09:12 +01:00
Hakan Bastedt
5d90765920 Didn't help using TICK_FORMAT in setOverflow() 2024-03-27 23:06:13 +01:00
Hakan Bastedt
e6ae2ad3c7 Test case for HardwareTime frequency discrepancy + doc 2024-03-27 22:03:10 +01:00
Hakan Bastedt
ea5a39d300 wip now the 50 kHz timer is only 40 kHz. How come? 2024-03-26 22:59:41 +01:00
Hakan Bastedt
88bd1ead78 pos_scale goes into make_pulses from lcnc 2024-03-23 21:49:13 +01:00
Hakan Bastedt
dccbd97a04 Tried to optimize BASE_PERIOD but 12000 ns (83 kHz) is the best it can do right now 2024-03-23 20:27:32 +01:00
Hakan Bastedt
1b8721930e Shortest BASE_PERIOD is 12000 as it is now. Plus some cleanup 2024-03-23 20:09:35 +01:00
Hakan Bastedt
8710a547fe Yesss software stepping works now. 2024-03-23 19:51:26 +01:00
Hakan Bastedt
369a795ce5 wip 2024-03-23 18:02:38 +01:00
Hakan Bastedt
2df0a0980c Stepgen3 is driven by HardwareTimer, in a way that resembles EtherCAT setup. Seems to actually work. 2024-03-22 22:38:16 +01:00
Hakan Bastedt
6383d6de89 Working test-program using user LEDs and delays 2024-03-22 21:20:31 +01:00
Hakan Bastedt
7bff3f3789 linuxcnc stepgen is now in StepGen3. Compiles 2024-03-22 17:31:21 +01:00
Hakan Bastedt
23fe81afbe Factored Stepgen2, StepGen3 is a copy of Stepgen2 2024-03-21 16:45:59 +01:00
Hakan Bastedt
fa34d81e41 Factored Stepgen2, StepGen3 is a copy of Stepgen2 2024-03-21 16:45:46 +01:00
Hakan Bastedt
30a9f10c7b Well it works, network-wise. But there are those extra peaks that have been since the start. Let's see if I can remove them. Welcome StepGen3 2024-03-21 16:28:55 +01:00
Hakan Bastedt
8f26a10224 It works. I hate RealTek. Even with the r8618_dkms driver it doesn't work. I bought a Intel network card, set irq coalesce rx-usecs and tx-usecs to 0. Now it works works WORKS. No lost packages, no delayed packages. I hate RealTek 2024-03-20 17:06:24 +01:00
Hakan Bastedt
d4fed6cfe8 Yes it works now. Made another implementation of the pulse IRQ and all that. We'll see if I keep this or go back to the older. It is IMPORTANT, REQUIRED to use a 4.9 linux kernel for it to work. There are obviously bugs in the RealTek network drivers R8168/R8169 in 5+ kernels. All this work could have been avoided with a 4.9 kernel. 2024-03-17 22:04:52 +01:00
Hakan Bastedt
27475eaecd Seems to actually work, but I have seen that before, so testing time 2024-03-14 10:15:23 +01:00
Hakan Bastedt
71ae242fc4 Fixed bug in extend32to64:extendTime() 2024-03-13 23:33:25 +01:00
Hakan Bastedt
f341eb5074 wip 2024-03-10 22:40:25 +01:00
Hakan Bastedt
fef934b103 wip 2024-03-10 01:03:50 +01:00
Hakan Bastedt
31be067dea After test in lathe. Basically ok, always use real Tstartf, not 1/n 2024-03-08 09:00:55 +01:00
Hakan Bastedt
94d68adbb1 Added Z stepgen. Both X and Z works on the scope 2024-03-03 19:01:02 +01:00
Hakan Bastedt
e8ef618fcc Test in lathe coming. 2024-03-03 17:02:04 +01:00
Hakan Bastedt
cbc45bc80b Merge branch 'Video8' into stepper2 2024-02-19 21:41:46 +01:00
Hakan Bastedt
30dc44d5e6 Direction output to dirPin. 2024-02-16 11:45:06 +01:00
Hakan Bastedt
2b2be4f63d Going for test in the lathe 2024-02-13 10:49:57 +01:00
Hakan Bastedt
cbae816bd9 More debug variables 2024-02-13 10:49:36 +01:00
Hakan Bastedt
c0d2bfcf62 doc update on stepgen problem and solution 2024-02-12 18:37:16 +01:00
Hakan Bastedt
f4a15afa8a a cycle's pwm train maight have been too long and run into the start of next cycle's pwm train. That's gone now and it seems to work.
A more brilliant solution is needed for this.
2024-02-11 19:56:16 +01:00
Hakan Bastedt
c04ac0e74b Consistently get SM2 event now. Must check directly in irq. DIG_PROCESS modified to check this ALevent copy 2024-02-09 22:58:28 +01:00
Hakan Bastedt
6d18c2cb3f Clear ALevents for DC_sync0 and SM3 might have solved the uneven pulse train. Looking better now. 2024-02-09 17:28:18 +01:00
Hakan Bastedt
d0433b29cf cleanup and removing StepGen from active code 2024-02-08 21:33:31 +01:00
Hakan Bastedt
fe3de876fa Class StepGen2 done after Stepgen.odb 2024-02-08 21:28:48 +01:00
Hakan Bastedt
2fb5252d37 CircularBuffer.h new file ending hpp due to version 1.4 od CB 2024-02-08 19:06:55 +01:00
Hakan Bastedt
43854ca4d0 wip 2024-02-05 21:22:23 +01:00
Hakan Bastedt
dbb4d0f34e Update 2024-02-05 20:32:49 +01:00
Hakan Bastedt
f9b37cfa6c Document stepgen 2024-02-05 01:04:20 +01:00
Hakan Bastedt
2b2704bb17 Added timer2 for proper start point 2024-02-02 21:43:58 +01:00
Hakan Bastedt
0384646972 wip 2024-02-02 15:26:01 +01:00
Hakan Bastedt
133df5662d wip, before timer stuff 2024-02-02 12:09:49 +01:00
Hakan Bastedt
d1eb1d9a40 logic might be there 2024-02-01 20:30:02 +01:00
99 changed files with 15033 additions and 822 deletions

View File

@@ -55,7 +55,8 @@
<option value="INTEGER64">INTEGER64</option>
<option value="UNSIGNED8">UNSIGNED8</option>
<option value="UNSIGNED16">UNSIGNED16</option>
<option value="UNSIGNED32">UNSIGNED64</option>
<option value="UNSIGNED32">UNSIGNED32</option>
<option value="UNSIGNED64">UNSIGNED64</option>
<option value="REAL32">REAL32</option>
<option value="REAL64">REAL64</option>
<option value="VISIBLE_STRING">VISIBLE STRING</option>

View File

@@ -28,11 +28,11 @@ const DTYPE = {
INTEGER8 : 'INTEGER8',
INTEGER16 : 'INTEGER16',
INTEGER32 : 'INTEGER32',
INTEGER32 : 'INTEGER64',
INTEGER64 : 'INTEGER64',
UNSIGNED8 : 'UNSIGNED8',
UNSIGNED16 : 'UNSIGNED16',
UNSIGNED32 : 'UNSIGNED32',
UNSIGNED32 : 'UNSIGNED64',
UNSIGNED64 : 'UNSIGNED64',
REAL32 : 'REAL32',
REAL64 : 'REAL64',
VISIBLE_STRING : 'VISIBLE_STRING',

View File

@@ -125,7 +125,7 @@ function objectlist_generator(form, od, indexes)
objd.items.slice(subindex).forEach(subitem => {
var subi = subindex_padded(subindex);
const value = objectlist_getItemValue(subitem, objd.dtype);
objectlist += `\n {0x${subi}, DTYPE_${objd.dtype}, ${bitsize}, ${objectlist_objdFlags(objd)}, acName${index}_${subi}, ${value}, ${subitem.data || 'NULL'}},`;
objectlist += `\n {${subi}, DTYPE_${objd.dtype}, ${bitsize}, ${objectlist_objdFlags(objd)}, acName${index}_${subi}, ${value}, ${subitem.data || 'NULL'}},`;
subindex++;
});
break;
@@ -138,7 +138,7 @@ function objectlist_generator(form, od, indexes)
const bitsize = dtype_bitsize[subitem.dtype];
const value = objectlist_getItemValue(subitem, subitem.dtype);
const atypeflag = objectlist_objdFlags(subitem);
objectlist += `\n {0x${subi}, DTYPE_${subitem.dtype}, ${bitsize}, ${atypeflag}, acName${index}_${subi}, ${value}, ${subitem.data || 'NULL'}},`;
objectlist += `\n {${subi}, DTYPE_${subitem.dtype}, ${bitsize}, ${atypeflag}, acName${index}_${subi}, ${value}, ${subitem.data || 'NULL'}},`;
subindex++;
});

1
Firmware/.gitignore vendored
View File

@@ -3,3 +3,4 @@
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch
.vscode/settings.json

View File

@@ -10,7 +10,8 @@
"vector": "cpp",
"system_error": "cpp",
"numeric": "cpp",
"ostream": "cpp"
"ostream": "cpp",
"cmath": "cpp"
},
"C_Cpp.errorSquiggles": "disabled",
"cmake.configureOnOpen": false

1
Firmware/doc/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
.~lock.*

BIN
Firmware/doc/Stepgen.odp Executable file

Binary file not shown.

View File

@@ -1,8 +1,8 @@
#ifndef MYENCODER
#define MYENCODER
#include "Stm32F4_Encoder.h"
#include <CircularBuffer.h>
#define RINGBUFFERLEN 101
#include <CircularBuffer.hpp>
#define RINGBUFFERLEN 11
class MyEncoder
{
@@ -30,6 +30,7 @@ private:
CircularBuffer<double_t, RINGBUFFERLEN> Pos;
CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
double curPos;
double oldFrequency;
TIM_TypeDef *tim_base;
};

View File

@@ -16,6 +16,7 @@ private:
volatile double_t requestedPosition;
volatile uint8_t enabled;
HardwareTimer *MyTim;
HardwareTimer *MyTim2;
uint16_t stepsPerMM;
uint8_t dirPin;
PinName stepPin;

53
Firmware/include/StepGen2.h Executable file
View File

@@ -0,0 +1,53 @@
#ifndef STEPGEN
#define STEPGEN
#include <HardwareTimer.h>
class StepGen2
{
public:
volatile double_t actualPosition;
volatile int32_t nSteps;
volatile uint32_t timerFrequency;
volatile int32_t timerPosition = 0;
volatile int32_t timerEndPosition = 0;
public:
volatile float Tstartf; // Starting delay in secs
volatile uint32_t Tstartu; // Starting delay in usecs
volatile float Tpulses; // Time it takes to do pulses. Debug
HardwareTimer *pulseTimer;
uint32_t pulseTimerChan;
HardwareTimer *startTimer; // Use timers 10,11,13,14
uint8_t dirPin;
PinName dirPinName;
PinName stepPin;
uint32_t Tjitter = 400; // Longest time from IRQ to handling in handleStepper, unit is microseconds
uint64_t dbg;
const uint16_t t2 = 5; // DIR is ahead of PUL with at least 5 usecs
const uint16_t t3 = 5; // Pulse width at least 2.5 usecs
const uint16_t t4 = 5; // Low level width not less than 2.5 usecs
const float maxAllowedFrequency = 1000000 / float(t3 + t4) * 0.9; // 150 kHz for now
public:
volatile double_t commandedPosition; // End position when this cycle is completed
volatile int32_t commandedStepPosition; // End step position when this cycle is completed
volatile double_t initialPosition; // From previous cycle
volatile int32_t initialStepPosition; // From previous cycle
int16_t stepsPerMM; // This many steps per mm
volatile uint8_t enabled; // Enabled step generator
volatile float frequency;
static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds
volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often)
StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
uint32_t handleStepper(uint64_t irqTime /* time when irq happened nanosecs */, uint16_t nLoops);
void startTimerCB();
void pulseTimerCB();
uint32_t updatePos(uint32_t i);
};
#endif

156
Firmware/include/StepGen3.h Executable file
View File

@@ -0,0 +1,156 @@
#ifndef STEPGEN3
#define STEPGEN3
#include <HardwareTimer.h>
#define MAX_CHAN 16
#define MAX_CYCLE 18
#define USER_STEP_TYPE 13
typedef struct
{
/* stuff that is both read and written by makepulses */
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 */
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 */
unsigned int step_space; /* parameter: min step pulse spacing */
double old_pos_cmd; /* previous position command (counts) */
int count; /* pin: captured feedback in counts */
#define double float
double pos_scale; /* param: steps per position unit */
double old_scale; /* stored scale value */
double scale_recip; /* reciprocal value used for scaling */
double vel_cmd; /* pin: velocity command (pos units/sec) */
double pos_cmd; /* pin: position command (position units) */
double pos_fb; /* pin: position feedback (position units) */
double freq; /* param: frequency command */
double maxvel; /* param: max velocity, (pos units/sec) */
double maxaccel; /* param: max accel (pos units/sec^2) */
unsigned int old_step_len; /* used to detect parameter changes */
unsigned int old_step_space;
unsigned int old_dir_hold_dly;
unsigned int old_dir_setup;
int printed_error; /* flag to avoid repeated printing */
} stepgen_t;
#define MAX_STEP_TYPE 15
#define STEP_PIN 0 /* output phase used for STEP signal */
#define DIR_PIN 1 /* output phase used for DIR signal */
#define UP_PIN 0 /* output phase used for UP signal */
#define DOWN_PIN 1 /* output phase used for DOWN signal */
#define PICKOFF 28 /* bit location in DDS accum */
typedef enum CONTROL
{
POSITION,
VELOCITY,
INVALID
} 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
uint32_t stepPin[MAX_CHAN] = {0};
uint32_t dirPin[MAX_CHAN] = {0};
stepgen_t *stepgen_array = 0;
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 */
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 uint64_t cnt = 0; // Debug counter
#undef double
StepGen3(void);
void updateStepGen(double pos_cmd1, double pos_cmd2);
void makeAllPulses(void);
int rtapi_app_main();
int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
void make_pulses(void *arg, long period);
void update_freq(void *arg, long period);
void update_pos(void *arg, long period);
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,
};
};
// For the example
#define BASE_PERIOD 20000
#define SERVO_PERIOD 2000000
#define JOINT_X_STEPGEN_MAXACCEL 0.0
#define JOINT_Z_STEPGEN_MAXACCEL 0.0
#define JOINT_X_SCALE -200
#define JOINT_Z_SCALE -80
#endif

View File

@@ -641,7 +641,8 @@ public:
uint16_t GetCount();
};
void rcc_config();
void encoder_config();
void encoder2_config(); // Experimental
void GpioConfigPortA(GPIO_TypeDef *GPIOx);
void GpioConfigPortC(GPIO_TypeDef *GPIOx);
void GpioConfigPortD(GPIO_TypeDef *GPIOx);

14
Firmware/include/extend32to64.h Executable file
View File

@@ -0,0 +1,14 @@
#ifndef EXTEND32TO64
#define EXTEND32TO54
#include <Arduino.h>
class extend32to64
{
public:
int64_t previousTimeValue = 0;
const uint64_t ONE_PERIOD = 4294967296; // almost UINT32_MAX;
const uint64_t HALF_PERIOD = 2147483648; // Half of that
int64_t extendTime(uint32_t in);
};
#endif

File diff suppressed because it is too large Load Diff

View File

@@ -33,8 +33,8 @@
#define SM3_smc 0x20
#define SM3_act 1
#define MAX_MAPPINGS_SM2 8
#define MAX_MAPPINGS_SM3 7
#define MAX_MAPPINGS_SM2 5
#define MAX_MAPPINGS_SM3 11
#define MAX_RXPDO_SIZE 512
#define MAX_TXPDO_SIZE 512

View File

@@ -13,25 +13,25 @@
#define IS_TXPDO(index) ((index) >= 0x1A00 && (index) < 0x1C00)
/* Global variables used by the stack */
uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE,MBXSIZEBOOT)];
uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE, MBXSIZEBOOT)];
_MBXcontrol MBXcontrol[MBXBUFFERS];
_SMmap SMmap2[MAX_MAPPINGS_SM2];
_SMmap SMmap3[MAX_MAPPINGS_SM3];
_ESCvar ESCvar;
_SMmap SMmap2[MAX_MAPPINGS_SM2];
_SMmap SMmap3[MAX_MAPPINGS_SM3];
_ESCvar ESCvar;
/* Private variables */
static volatile int watchdog;
#if MAX_MAPPINGS_SM2 > 0
static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned (8)));
static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned(8)));
#else
extern uint8_t * rxpdo;
extern uint8_t *rxpdo;
#endif
#if MAX_MAPPINGS_SM3 > 0
static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned (8)));
static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned(8)));
#else
extern uint8_t * txpdo;
extern uint8_t *txpdo;
#endif
/** Function to pre-qualify the incoming SDO download.
@@ -40,19 +40,19 @@ extern uint8_t * txpdo;
* @param[in] sub-index = sub-index of SDO download request to check
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_download_pre_objecthandler (uint16_t index,
uint8_t subindex,
void * data,
size_t size,
uint16_t flags)
uint32_t ESC_download_pre_objecthandler(uint16_t index,
uint8_t subindex,
void *data,
size_t size,
uint16_t flags)
{
if (IS_RXPDO (index) ||
IS_TXPDO (index) ||
if (IS_RXPDO(index) ||
IS_TXPDO(index) ||
index == RX_PDO_OBJIDX ||
index == TX_PDO_OBJIDX)
{
uint8_t minSub = ((flags & COMPLETE_ACCESS_FLAG) == 0) ? 0 : 1;
if (subindex > minSub && COE_maxSub (index) != 0)
if (subindex > minSub && COE_maxSub(index) != 0)
{
return ABORT_SUBINDEX0_NOT_ZERO;
}
@@ -60,11 +60,11 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
if (ESCvar.pre_object_download_hook)
{
return (ESCvar.pre_object_download_hook) (index,
subindex,
data,
size,
flags);
return (ESCvar.pre_object_download_hook)(index,
subindex,
data,
size,
flags);
}
return 0;
@@ -77,7 +77,7 @@ uint32_t ESC_download_pre_objecthandler (uint16_t index,
* @param[in] sub-index = sub-index of SDO download request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
uint32_t ESC_download_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
{
if (ESCvar.post_object_download_hook != NULL)
{
@@ -93,19 +93,19 @@ uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint
* @param[in] sub-index = sub-index of SDO upload request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_upload_pre_objecthandler (uint16_t index,
uint8_t subindex,
void * data,
size_t size,
uint16_t flags)
uint32_t ESC_upload_pre_objecthandler(uint16_t index,
uint8_t subindex,
void *data,
size_t size,
uint16_t flags)
{
if (ESCvar.pre_object_upload_hook != NULL)
{
return (ESCvar.pre_object_upload_hook) (index,
subindex,
data,
size,
flags);
return (ESCvar.pre_object_upload_hook)(index,
subindex,
data,
size,
flags);
}
return 0;
@@ -118,7 +118,7 @@ uint32_t ESC_upload_pre_objecthandler (uint16_t index,
* @param[in] sub-index = sub-index of SDO upload request to handle
* @return SDO abort code, or 0 on success
*/
uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags)
uint32_t ESC_upload_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
{
if (ESCvar.post_object_upload_hook != NULL)
{
@@ -131,11 +131,11 @@ uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16
/** Hook called from the slave stack ESC_stopoutputs to act on state changes
* forcing us to stop outputs. Here we can set them to a safe state.
*/
void APP_safeoutput (void)
void APP_safeoutput(void)
{
DPRINT ("APP_safeoutput\n");
DPRINT("APP_safeoutput\n");
if(ESCvar.safeoutput_override != NULL)
if (ESCvar.safeoutput_override != NULL)
{
(ESCvar.safeoutput_override)();
}
@@ -143,9 +143,9 @@ void APP_safeoutput (void)
/** Write local process data to Sync Manager 3, Master Inputs.
*/
void TXPDO_update (void)
void TXPDO_update(void)
{
if(ESCvar.txpdo_override != NULL)
if (ESCvar.txpdo_override != NULL)
{
(ESCvar.txpdo_override)();
}
@@ -153,26 +153,26 @@ void TXPDO_update (void)
{
if (MAX_MAPPINGS_SM3 > 0)
{
COE_pdoPack (txpdo, ESCvar.sm3mappings, SMmap3);
COE_pdoPack(txpdo, ESCvar.sm3mappings, SMmap3);
}
ESC_write (ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
ESC_write(ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
}
}
/** Read Sync Manager 2 to local process data, Master Outputs.
*/
void RXPDO_update (void)
void RXPDO_update(void)
{
if(ESCvar.rxpdo_override != NULL)
if (ESCvar.rxpdo_override != NULL)
{
(ESCvar.rxpdo_override)();
}
else
{
ESC_read (ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
ESC_read(ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
if (MAX_MAPPINGS_SM2 > 0)
{
COE_pdoUnpack (rxpdo, ESCvar.sm2mappings, SMmap2);
COE_pdoUnpack(rxpdo, ESCvar.sm2mappings, SMmap2);
}
}
}
@@ -182,7 +182,7 @@ void RXPDO_update (void)
*
* @param[in] watchdogcnt = new watchdog count value
*/
void APP_setwatchdog (int watchdogcnt)
void APP_setwatchdog(int watchdogcnt)
{
CC_ATOMIC_SET(ESCvar.watchdogcnt, watchdogcnt);
}
@@ -191,10 +191,11 @@ void APP_setwatchdog (int watchdogcnt)
* write ethercat inputs. Implement watch-dog counter to count-out if we have
* made state change affecting the App.state.
*/
void DIG_process (uint8_t flags)
void DIG_process(uint16_t ALEvent, uint8_t flags)
{
/* Handle watchdog */
if((flags & DIG_PROCESS_WD_FLAG) > 0)
if ((flags & DIG_PROCESS_WD_FLAG) > 0)
{
if (CC_ATOMIC_GET(watchdog) > 0)
{
@@ -207,7 +208,7 @@ void DIG_process (uint8_t flags)
DPRINT("DIG_process watchdog expired\n");
ESC_ALstatusgotoerror((ESCsafeop | ESCerror), ALERR_WATCHDOG);
}
else if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
else if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
{
CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
}
@@ -216,15 +217,15 @@ void DIG_process (uint8_t flags)
/* Handle Outputs */
if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
{
if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
(ESCvar.ALevent & ESCREG_ALEVENT_SM2))
if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
(ALEvent & ESCREG_ALEVENT_SM2))
{
RXPDO_update();
CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
/* Set outputs */
cb_set_outputs();
}
else if (ESCvar.ALevent & ESCREG_ALEVENT_SM2)
else if (ALEvent & ESCREG_ALEVENT_SM2)
{
RXPDO_update();
}
@@ -243,7 +244,7 @@ void DIG_process (uint8_t flags)
/* Handle Inputs */
if ((flags & DIG_PROCESS_INPUTS_FLAG) > 0)
{
if(CC_ATOMIC_GET(ESCvar.App.state) > 0)
if (CC_ATOMIC_GET(ESCvar.App.state) > 0)
{
/* Update inputs */
cb_get_inputs();
@@ -257,7 +258,7 @@ void DIG_process (uint8_t flags)
* control what interrupts that should be served and re-activated with
* event mask argument
*/
void ecat_slv_worker (uint32_t event_mask)
void ecat_slv_worker(uint32_t event_mask)
{
do
{
@@ -289,21 +290,21 @@ void ecat_slv_worker (uint32_t event_mask)
CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
}while(ESCvar.ALevent & event_mask);
} while (ESCvar.ALevent & event_mask);
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | event_mask);
}
/*
* Polling function. It should be called periodically for an application
* Polling function. It should be called periodically for an application
* when only SM2/DC interrupt is active.
* Read and handle events for the EtherCAT state, status, mailbox and eeprom.
*/
void ecat_slv_poll (void)
void ecat_slv_poll(void)
{
/* Read local time from ESC*/
ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
ESCvar.Time = etohl (ESCvar.Time);
ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
ESCvar.Time = etohl(ESCvar.Time);
/* Check the state machine */
ESC_state();
@@ -336,50 +337,50 @@ void ecat_slv_poll (void)
/*
* Poll all events in a free-run application
*/
void ecat_slv (void)
void ecat_slv(void)
{
ecat_slv_poll();
DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
DIG_process(ESC_ALeventread(), DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
}
/*
* Initialize the slave stack.
*/
void ecat_slv_init (esc_cfg_t * config)
void ecat_slv_init(esc_cfg_t *config)
{
/* Init watchdog */
watchdog = config->watchdog_cnt;
/* Call stack configuration */
ESC_config (config);
ESC_config(config);
/* Call HW init */
ESC_init (config);
ESC_init(config);
/* wait until ESC is started up */
while ((ESCvar.DLstatus & 0x0001) == 0)
{
ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
sizeof (ESCvar.DLstatus));
ESCvar.DLstatus = etohs (ESCvar.DLstatus);
ESC_read(ESCREG_DLSTATUS, (void *)&ESCvar.DLstatus,
sizeof(ESCvar.DLstatus));
ESCvar.DLstatus = etohs(ESCvar.DLstatus);
}
#if USE_FOE
/* Init FoE */
FOE_init ();
FOE_init();
#endif
#if USE_EOE
/* Init EoE */
EOE_init ();
EOE_init();
#endif
/* reset ESC to init state */
ESC_ALstatus (ESCinit);
ESC_ALerror (ALERR_NONE);
ESC_stopmbx ();
ESC_stopinput ();
ESC_stopoutput ();
ESC_ALstatus(ESCinit);
ESC_ALerror(ALERR_NONE);
ESC_stopmbx();
ESC_stopinput();
ESC_stopoutput();
/* Init Object Dictionary default values */
COE_initDefaultValues ();
COE_initDefaultValues();
}

View File

@@ -15,7 +15,7 @@
void cb_get_inputs();
/**
* This function is called when to set outputs values
* This function is called when to set outputs values
*/
void cb_set_outputs();
@@ -23,12 +23,12 @@ void cb_set_outputs();
*
* @param[in] watchdogcnt = new watchdog count value
*/
void APP_setwatchdog (int watchdogcnt);
void APP_setwatchdog(int watchdogcnt);
#define DIG_PROCESS_INPUTS_FLAG 0x01
#define DIG_PROCESS_OUTPUTS_FLAG 0x02
#define DIG_PROCESS_WD_FLAG 0x04
#define DIG_PROCESS_APP_HOOK_FLAG 0x08
#define DIG_PROCESS_INPUTS_FLAG 0x01
#define DIG_PROCESS_OUTPUTS_FLAG 0x02
#define DIG_PROCESS_WD_FLAG 0x04
#define DIG_PROCESS_APP_HOOK_FLAG 0x08
/** Implements the watch-dog counter to count if we should make a state change
* due to missing incoming SM2 events. Updates local I/O and run the application
* in the following order, call read EtherCAT outputs, execute user provided
@@ -36,7 +36,7 @@ void APP_setwatchdog (int watchdogcnt);
*
* @param[in] flags = User input what to execute
*/
void DIG_process (uint8_t flags);
void DIG_process(uint16_t ALEvent, uint8_t flags);
/**
* Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
@@ -46,24 +46,24 @@ void DIG_process (uint8_t flags);
* @param[in] event_mask = Event mask for interrupts to serve and re-activate
* after served
*/
void ecat_slv_worker (uint32_t event_mask);
void ecat_slv_worker(uint32_t event_mask);
/**
* Poll SM0/1, EEPROM and AL CONTROL events in a SM/DC synchronization
* application
*/
void ecat_slv_poll (void);
void ecat_slv_poll(void);
/**
* Poll all events in a free-run application
*/
void ecat_slv (void);
void ecat_slv(void);
/**
* Initialize the slave stack
*
* @param[in] config = User input how to configure the stack
*/
void ecat_slv_init (esc_cfg_t * config);
void ecat_slv_init(esc_cfg_t *config);
#endif /* __ECAT_SLV_H__ */

View File

@@ -57,7 +57,7 @@
"pdo_mappings": [
"txpdo"
],
"dtype": "REAL64",
"dtype": "REAL32",
"value": "0",
"data": "&Obj.EncPos"
},
@@ -68,7 +68,7 @@
"pdo_mappings": [
"txpdo"
],
"dtype": "REAL64",
"dtype": "REAL32",
"value": "0",
"data": "&Obj.EncFrequency"
},
@@ -79,7 +79,7 @@
"pdo_mappings": [
"txpdo"
],
"dtype": "UNSIGNED32",
"dtype": "UNSIGNED16",
"value": "0",
"data": "&Obj.DiffT"
},
@@ -106,59 +106,74 @@
"data": "&Obj.IndexStatus"
},
"6005": {
"otype": "RECORD",
"name": "StepGenOut1",
"otype": "VAR",
"name": "ActualPosition1",
"access": "RO",
"items": [
{
"name": "Max SubIndex"
},
{
"name": "ActualPosition",
"dtype": "REAL64",
"data": "&Obj.StepGenOut1.ActualPosition",
"value": "0",
"access": "RO"
}
],
"pdo_mappings": [
"txpdo"
]
],
"dtype": "REAL32",
"value": "0",
"data": "&Obj.ActualPosition1"
},
"6006": {
"otype": "RECORD",
"name": "StepGenOut2",
"otype": "VAR",
"name": "ActualPosition2",
"access": "RO",
"items": [
{
"name": "Max SubIndex"
},
{
"name": "ActualPosition",
"dtype": "REAL64",
"data": "&Obj.StepGenOut2.ActualPosition",
"value": "0",
"access": "RO"
}
],
"pdo_mappings": [
"txpdo"
]
],
"dtype": "REAL32",
"value": "0",
"data": "&Obj.ActualPosition2"
},
"6007": {
"otype": "VAR",
"name": "D1",
"access": "RO",
"pdo_mappings": [
"txpdo"
],
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.D1"
},
"6008": {
"otype": "VAR",
"name": "D2",
"access": "RO",
"pdo_mappings": [
"txpdo"
],
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.D2"
},
"6009": {
"otype": "VAR",
"name": "D3",
"access": "RO",
"pdo_mappings": [
"txpdo"
],
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.D3"
},
"600A": {
"otype": "VAR",
"name": "D4",
"access": "RO",
"pdo_mappings": [
"txpdo"
],
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.D4"
}
},
"rxpdo": {
"7000": {
"otype": "VAR",
"name": "EncPosScale",
"access": "RO",
"pdo_mappings": [
"rxpdo"
],
"dtype": "INTEGER32",
"value": "0",
"data": "&Obj.EncPosScale"
},
"7001": {
"otype": "VAR",
"name": "IndexLatchEnable",
"access": "RO",
@@ -169,70 +184,49 @@
"value": "0",
"data": "&Obj.IndexLatchEnable"
},
"7002": {
"otype": "RECORD",
"name": "StepGenIn1",
"7001": {
"otype": "VAR",
"name": "CommandedPosition1",
"access": "RO",
"items": [
{
"name": "Max SubIndex"
},
{
"name": "CommandedPosition",
"dtype": "REAL64",
"data": "&Obj.StepGenIn1.CommandedPosition",
"value": "0",
"access": "RO"
},
{
"name": "StepsPerMM",
"dtype": "INTEGER16",
"value": "0",
"access": "RO",
"data": "&Obj.StepGenIn1.StepsPerMM"
}
],
"pdo_mappings": [
"rxpdo"
]
],
"dtype": "REAL32",
"value": "0",
"data": "&Obj.CommandedPosition1"
},
"7002": {
"otype": "VAR",
"name": "CommandedPosition2",
"access": "RO",
"pdo_mappings": [
"rxpdo"
],
"dtype": "REAL32",
"value": "0",
"data": "&Obj.CommandedPosition2"
},
"7003": {
"otype": "RECORD",
"name": "StepGenIn2",
"otype": "VAR",
"name": "StepsPerMM1",
"access": "RO",
"items": [
{
"name": "Max SubIndex"
},
{
"name": "CommandedPosition",
"dtype": "REAL64",
"data": "&Obj.StepGenIn2.CommandedPosition",
"value": "0",
"access": "RO"
},
{
"name": "StepsPerMM",
"dtype": "INTEGER16",
"value": "0",
"access": "RO",
"data": "&Obj.StepGenIn2.StepsPerMM"
}
],
"pdo_mappings": [
"rxpdo"
]
],
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.StepsPerMM1"
},
"7004": {
"otype": "VAR",
"name": "Enable1",
"name": "StepsPerMM2",
"access": "RO",
"pdo_mappings": [
"rxpdo"
],
"dtype": "BOOLEAN",
"dtype": "INTEGER16",
"value": "0",
"data": "&Obj.Enable1"
"data": "&Obj.StepsPerMM2"
},
"60664": {
"otype": "VAR",

View File

@@ -13,24 +13,21 @@ static const char acName1018_01[] = "Vendor ID";
static const char acName1018_02[] = "Product Code";
static const char acName1018_03[] = "Revision Number";
static const char acName1018_04[] = "Serial Number";
static const char acName1600[] = "EncPosScale";
static const char acName1600[] = "IndexLatchEnable";
static const char acName1600_00[] = "Max SubIndex";
static const char acName1600_01[] = "EncPosScale";
static const char acName1601[] = "IndexLatchEnable";
static const char acName1600_01[] = "IndexLatchEnable";
static const char acName1601[] = "CommandedPosition1";
static const char acName1601_00[] = "Max SubIndex";
static const char acName1601_01[] = "IndexLatchEnable";
static const char acName1602[] = "StepGenIn1";
static const char acName1601_01[] = "CommandedPosition1";
static const char acName1602[] = "CommandedPosition2";
static const char acName1602_00[] = "Max SubIndex";
static const char acName1602_01[] = "CommandedPosition";
static const char acName1602_02[] = "StepsPerMM";
static const char acName1603[] = "StepGenIn2";
static const char acName1602_01[] = "CommandedPosition2";
static const char acName1603[] = "StepsPerMM1";
static const char acName1603_00[] = "Max SubIndex";
static const char acName1603_01[] = "CommandedPosition";
static const char acName1603_02[] = "StepsPerMM";
static const char acName1604[] = "Enable1";
static const char acName1603_01[] = "StepsPerMM1";
static const char acName1604[] = "StepsPerMM2";
static const char acName1604_00[] = "Max SubIndex";
static const char acName1604_01[] = "Enable1";
static const char acName1604_02[] = "Padding 1";
static const char acName1604_01[] = "StepsPerMM2";
static const char acName1A00[] = "EncPos";
static const char acName1A00_00[] = "Max SubIndex";
static const char acName1A00_01[] = "EncPos";
@@ -46,12 +43,24 @@ static const char acName1A03_01[] = "IndexByte";
static const char acName1A04[] = "IndexStatus";
static const char acName1A04_00[] = "Max SubIndex";
static const char acName1A04_01[] = "IndexStatus";
static const char acName1A05[] = "StepGenOut1";
static const char acName1A05[] = "ActualPosition1";
static const char acName1A05_00[] = "Max SubIndex";
static const char acName1A05_01[] = "ActualPosition";
static const char acName1A06[] = "StepGenOut2";
static const char acName1A05_01[] = "ActualPosition1";
static const char acName1A06[] = "ActualPosition2";
static const char acName1A06_00[] = "Max SubIndex";
static const char acName1A06_01[] = "ActualPosition";
static const char acName1A06_01[] = "ActualPosition2";
static const char acName1A07[] = "D1";
static const char acName1A07_00[] = "Max SubIndex";
static const char acName1A07_01[] = "D1";
static const char acName1A08[] = "D2";
static const char acName1A08_00[] = "Max SubIndex";
static const char acName1A08_01[] = "D2";
static const char acName1A09[] = "D3";
static const char acName1A09_00[] = "Max SubIndex";
static const char acName1A09_01[] = "D3";
static const char acName1A0A[] = "D4";
static const char acName1A0A_00[] = "Max SubIndex";
static const char acName1A0A_01[] = "D4";
static const char acName1C00[] = "Sync Manager Communication Type";
static const char acName1C00_00[] = "Max SubIndex";
static const char acName1C00_01[] = "Communications Type SM0";
@@ -74,32 +83,30 @@ static const char acName1C13_04[] = "PDO Mapping";
static const char acName1C13_05[] = "PDO Mapping";
static const char acName1C13_06[] = "PDO Mapping";
static const char acName1C13_07[] = "PDO Mapping";
static const char acName1C13_08[] = "PDO Mapping";
static const char acName1C13_09[] = "PDO Mapping";
static const char acName1C13_10[] = "PDO Mapping";
static const char acName1C13_11[] = "PDO Mapping";
static const char acName6000[] = "EncPos";
static const char acName6001[] = "EncFrequency";
static const char acName6002[] = "DiffT";
static const char acName6003[] = "IndexByte";
static const char acName6004[] = "IndexStatus";
static const char acName6005[] = "StepGenOut1";
static const char acName6005_00[] = "Max SubIndex";
static const char acName6005_01[] = "ActualPosition";
static const char acName6006[] = "StepGenOut2";
static const char acName6006_00[] = "Max SubIndex";
static const char acName6006_01[] = "ActualPosition";
static const char acName7000[] = "EncPosScale";
static const char acName7001[] = "IndexLatchEnable";
static const char acName7002[] = "StepGenIn1";
static const char acName7002_00[] = "Max SubIndex";
static const char acName7002_01[] = "CommandedPosition";
static const char acName7002_02[] = "StepsPerMM";
static const char acName7003[] = "StepGenIn2";
static const char acName7003_00[] = "Max SubIndex";
static const char acName7003_01[] = "CommandedPosition";
static const char acName7003_02[] = "StepsPerMM";
static const char acName7004[] = "Enable1";
static const char acName6005[] = "ActualPosition1";
static const char acName6006[] = "ActualPosition2";
static const char acName6007[] = "D1";
static const char acName6008[] = "D2";
static const char acName6009[] = "D3";
static const char acName600A[] = "D4";
static const char acName7000[] = "IndexLatchEnable";
static const char acName7001[] = "CommandedPosition1";
static const char acName7002[] = "CommandedPosition2";
static const char acName7003[] = "StepsPerMM1";
static const char acName7004[] = "StepsPerMM2";
const _objd SDO1000[] =
{
{0x0, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1000, 5001, NULL},
{0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1000, 5001, NULL},
};
const _objd SDO1008[] =
{
@@ -116,73 +123,90 @@ const _objd SDO100A[] =
const _objd SDO1018[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1018_00, 4, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1018_01, 2730, NULL},
{0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1018_02, 12303564, NULL},
{0x03, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1018_03, 2, NULL},
{0x04, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1018_04, 1, &Obj.serial},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_01, 2730, NULL},
{0x02, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_02, 12303564, NULL},
{0x03, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_03, 2, NULL},
{0x04, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_04, 1, &Obj.serial},
};
const _objd SDO1600[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1600_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1600_01, 0x70000020, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1600_01, 0x70000020, NULL},
};
const _objd SDO1601[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1601_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1601_01, 0x70010020, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1601_01, 0x70010020, NULL},
};
const _objd SDO1602[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 2, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_01, 0x70020140, NULL},
{0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_02, 0x70020210, NULL},
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1602_01, 0x70020020, NULL},
};
const _objd SDO1603[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 2, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1603_01, 0x70030140, NULL},
{0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1603_02, 0x70030210, NULL},
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1603_01, 0x70030010, NULL},
};
const _objd SDO1604[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1604_00, 2, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1604_01, 0x70040001, NULL},
{0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1604_02, 0x00000007, NULL},
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1604_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1604_01, 0x70040010, NULL},
};
const _objd SDO1A00[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A00_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A00_01, 0x60000040, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A00_01, 0x60000020, NULL},
};
const _objd SDO1A01[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A01_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A01_01, 0x60010040, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A01_01, 0x60010020, NULL},
};
const _objd SDO1A02[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A02_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A02_01, 0x60020020, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A02_01, 0x60020010, NULL},
};
const _objd SDO1A03[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A03_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A03_01, 0x60030020, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A03_01, 0x60030020, NULL},
};
const _objd SDO1A04[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A04_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A04_01, 0x60040020, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A04_01, 0x60040020, NULL},
};
const _objd SDO1A05[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A05_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A05_01, 0x60050140, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A05_01, 0x60050020, NULL},
};
const _objd SDO1A06[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A06_00, 1, NULL},
{0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A06_01, 0x60060140, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A06_01, 0x60060020, NULL},
};
const _objd SDO1A07[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A07_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A07_01, 0x60070010, NULL},
};
const _objd SDO1A08[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A08_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A08_01, 0x60080010, NULL},
};
const _objd SDO1A09[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A09_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A09_01, 0x60090010, NULL},
};
const _objd SDO1A0A[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A0A_00, 1, NULL},
{0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A0A_01, 0x600A0010, NULL},
};
const _objd SDO1C00[] =
{
@@ -203,7 +227,7 @@ const _objd SDO1C12[] =
};
const _objd SDO1C13[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 7, NULL},
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 11, NULL},
{0x01, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_01, 0x1A00, NULL},
{0x02, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_02, 0x1A01, NULL},
{0x03, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_03, 0x1A02, NULL},
@@ -211,18 +235,22 @@ const _objd SDO1C13[] =
{0x05, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_05, 0x1A04, NULL},
{0x06, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_06, 0x1A05, NULL},
{0x07, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_07, 0x1A06, NULL},
{0x08, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_08, 0x1A07, NULL},
{0x09, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_09, 0x1A08, NULL},
{0x0a, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_10, 0x1A09, NULL},
{0x0b, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_11, 0x1A0A, NULL},
};
const _objd SDO6000[] =
{
{0x0, DTYPE_REAL64, 64, ATYPE_RO | ATYPE_TXPDO, acName6000, 0, &Obj.EncPos},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6000, 0x00000000, &Obj.EncPos},
};
const _objd SDO6001[] =
{
{0x0, DTYPE_REAL64, 64, ATYPE_RO | ATYPE_TXPDO, acName6001, 0, &Obj.EncFrequency},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6001, 0x00000000, &Obj.EncFrequency},
};
const _objd SDO6002[] =
{
{0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_TXPDO, acName6002, 0, &Obj.DiffT},
{0x0, DTYPE_UNSIGNED16, 16, ATYPE_RO | ATYPE_TXPDO, acName6002, 0, &Obj.DiffT},
};
const _objd SDO6003[] =
{
@@ -234,37 +262,47 @@ const _objd SDO6004[] =
};
const _objd SDO6005[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6005_00, 1, NULL},
{0x01, DTYPE_REAL64, 64, ATYPE_RO, acName6005_01, 0, &Obj.StepGenOut1.ActualPosition},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6005, 0x00000000, &Obj.ActualPosition1},
};
const _objd SDO6006[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6006_00, 1, NULL},
{0x01, DTYPE_REAL64, 64, ATYPE_RO, acName6006_01, 0, &Obj.StepGenOut2.ActualPosition},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6006, 0x00000000, &Obj.ActualPosition2},
};
const _objd SDO6007[] =
{
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6007, 0, &Obj.D1},
};
const _objd SDO6008[] =
{
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6008, 0, &Obj.D2},
};
const _objd SDO6009[] =
{
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6009, 0, &Obj.D3},
};
const _objd SDO600A[] =
{
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName600A, 0, &Obj.D4},
};
const _objd SDO7000[] =
{
{0x0, DTYPE_INTEGER32, 32, ATYPE_RO | ATYPE_RXPDO, acName7000, 0, &Obj.EncPosScale},
{0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_RXPDO, acName7000, 0, &Obj.IndexLatchEnable},
};
const _objd SDO7001[] =
{
{0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_RXPDO, acName7001, 0, &Obj.IndexLatchEnable},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_RXPDO, acName7001, 0x00000000, &Obj.CommandedPosition1},
};
const _objd SDO7002[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 2, NULL},
{0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition},
{0x02, DTYPE_INTEGER16, 16, ATYPE_RO, acName7002_02, 0, &Obj.StepGenIn1.StepsPerMM},
{0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_RXPDO, acName7002, 0x00000000, &Obj.CommandedPosition2},
};
const _objd SDO7003[] =
{
{0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7003_00, 2, NULL},
{0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7003_01, 0, &Obj.StepGenIn2.CommandedPosition},
{0x02, DTYPE_INTEGER16, 16, ATYPE_RO, acName7003_02, 0, &Obj.StepGenIn2.StepsPerMM},
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_RXPDO, acName7003, 0, &Obj.StepsPerMM1},
};
const _objd SDO7004[] =
{
{0x0, DTYPE_BOOLEAN, 1, ATYPE_RO | ATYPE_RXPDO, acName7004, 0, &Obj.Enable1},
{0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_RXPDO, acName7004, 0, &Obj.StepsPerMM2},
};
const _objectlist SDOobjects[] =
@@ -276,9 +314,9 @@ const _objectlist SDOobjects[] =
{0x1018, OTYPE_RECORD, 4, 0, acName1018, SDO1018},
{0x1600, OTYPE_RECORD, 1, 0, acName1600, SDO1600},
{0x1601, OTYPE_RECORD, 1, 0, acName1601, SDO1601},
{0x1602, OTYPE_RECORD, 2, 0, acName1602, SDO1602},
{0x1603, OTYPE_RECORD, 2, 0, acName1603, SDO1603},
{0x1604, OTYPE_RECORD, 2, 0, acName1604, SDO1604},
{0x1602, OTYPE_RECORD, 1, 0, acName1602, SDO1602},
{0x1603, OTYPE_RECORD, 1, 0, acName1603, SDO1603},
{0x1604, OTYPE_RECORD, 1, 0, acName1604, SDO1604},
{0x1A00, OTYPE_RECORD, 1, 0, acName1A00, SDO1A00},
{0x1A01, OTYPE_RECORD, 1, 0, acName1A01, SDO1A01},
{0x1A02, OTYPE_RECORD, 1, 0, acName1A02, SDO1A02},
@@ -286,20 +324,28 @@ const _objectlist SDOobjects[] =
{0x1A04, OTYPE_RECORD, 1, 0, acName1A04, SDO1A04},
{0x1A05, OTYPE_RECORD, 1, 0, acName1A05, SDO1A05},
{0x1A06, OTYPE_RECORD, 1, 0, acName1A06, SDO1A06},
{0x1A07, OTYPE_RECORD, 1, 0, acName1A07, SDO1A07},
{0x1A08, OTYPE_RECORD, 1, 0, acName1A08, SDO1A08},
{0x1A09, OTYPE_RECORD, 1, 0, acName1A09, SDO1A09},
{0x1A0A, OTYPE_RECORD, 1, 0, acName1A0A, SDO1A0A},
{0x1C00, OTYPE_ARRAY, 4, 0, acName1C00, SDO1C00},
{0x1C12, OTYPE_ARRAY, 5, 0, acName1C12, SDO1C12},
{0x1C13, OTYPE_ARRAY, 7, 0, acName1C13, SDO1C13},
{0x1C13, OTYPE_ARRAY, 11, 0, acName1C13, SDO1C13},
{0x6000, OTYPE_VAR, 0, 0, acName6000, SDO6000},
{0x6001, OTYPE_VAR, 0, 0, acName6001, SDO6001},
{0x6002, OTYPE_VAR, 0, 0, acName6002, SDO6002},
{0x6003, OTYPE_VAR, 0, 0, acName6003, SDO6003},
{0x6004, OTYPE_VAR, 0, 0, acName6004, SDO6004},
{0x6005, OTYPE_RECORD, 1, 0, acName6005, SDO6005},
{0x6006, OTYPE_RECORD, 1, 0, acName6006, SDO6006},
{0x6005, OTYPE_VAR, 0, 0, acName6005, SDO6005},
{0x6006, OTYPE_VAR, 0, 0, acName6006, SDO6006},
{0x6007, OTYPE_VAR, 0, 0, acName6007, SDO6007},
{0x6008, OTYPE_VAR, 0, 0, acName6008, SDO6008},
{0x6009, OTYPE_VAR, 0, 0, acName6009, SDO6009},
{0x600A, OTYPE_VAR, 0, 0, acName600A, SDO600A},
{0x7000, OTYPE_VAR, 0, 0, acName7000, SDO7000},
{0x7001, OTYPE_VAR, 0, 0, acName7001, SDO7001},
{0x7002, OTYPE_RECORD, 2, 0, acName7002, SDO7002},
{0x7003, OTYPE_RECORD, 2, 0, acName7003, SDO7003},
{0x7002, OTYPE_VAR, 0, 0, acName7002, SDO7002},
{0x7003, OTYPE_VAR, 0, 0, acName7003, SDO7003},
{0x7004, OTYPE_VAR, 0, 0, acName7004, SDO7004},
{0xffff, 0xff, 0xff, 0xff, NULL, NULL}
};

View File

@@ -13,35 +13,25 @@ typedef struct
/* Inputs */
double EncPos;
double EncFrequency;
uint32_t DiffT;
float EncPos;
float EncFrequency;
uint16_t DiffT;
uint32_t IndexByte;
uint32_t IndexStatus;
struct
{
double ActualPosition;
} StepGenOut1;
struct
{
double ActualPosition;
} StepGenOut2;
float ActualPosition1;
float ActualPosition2;
int16_t D1;
int16_t D2;
int16_t D3;
int16_t D4;
/* Outputs */
int32_t EncPosScale;
uint32_t IndexLatchEnable;
struct
{
double CommandedPosition;
int16_t StepsPerMM;
} StepGenIn1;
struct
{
double CommandedPosition;
int16_t StepsPerMM;
} StepGenIn2;
uint8_t Enable1;
float CommandedPosition1;
float CommandedPosition2;
int16_t StepsPerMM1;
int16_t StepsPerMM2;
} _Objects;

View File

@@ -16,8 +16,9 @@ upload_protocol = stlink
debug_tool = stlink
debug_build_flags = -O0 -g -ggdb
monitor_port = COM7
monitor_filters = send_on_enter, time, colorize, log2file
monitor_speed = 115200
build_flags = -Wl,--no-warn-rwx-segment
lib_deps =
SPI
rlogiacco/CircularBuffer@^1.3.3
rlogiacco/CircularBuffer

View File

@@ -61,14 +61,19 @@ double MyEncoder::frequency(uint64_t time)
double diffT = 0;
double diffPos = 0;
double frequency;
TDelta.push(time); // Running average over the length of the circular buffer
Pos.push(curPos);
if (Pos.size() >= 2)
if (Pos.size() == RINGBUFFERLEN)
{
diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
diffT = 1.0e-6 * (TDelta.last() - TDelta.first()); // Time is in microseconds
diffPos = fabs(Pos.last() - Pos.first());
frequency = diffPos / diffT;
oldFrequency = frequency;
return frequency; // Revolutions per second
}
return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
else
return oldFrequency;
}
uint8_t MyEncoder::getIndexState()
{

124
Firmware/src/StepGen2.cpp Executable file
View File

@@ -0,0 +1,124 @@
#include <Arduino.h>
#include <stdio.h>
#include "StepGen2.h"
#include "extend32to64.h"
extern "C"
{
#include "esc.h"
}
extern extend32to64 longTime;
StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
{
actualPosition = 0;
commandedPosition = 0;
commandedStepPosition = 0;
initialPosition = 0;
initialStepPosition = 0;
stepsPerMM = 0;
enabled = 0;
dirPin = _dirPin;
dirPinName = digitalPinToPinName(dirPin);
stepPin = _stepPin;
pulseTimerChan = _timerChannel;
pulseTimer = new HardwareTimer(Timer);
pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
pinMode(dirPin, OUTPUT);
startTimer = new HardwareTimer(Timer2);
startTimer->attachInterrupt(irq2);
}
uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
{
frequency = 0;
nSteps = 0;
dbg = 0;
if (!enabled) // Just .... don't
return updatePos(0);
commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
nSteps = commandedStepPosition - initialStepPosition;
if (nSteps == 0) // No movement
{
return updatePos(1);
}
lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
if (abs(nSteps) < 0) // Some small number
{ //
frequency = (abs(nSteps) + 1) / lcncCycleTime; // Distribute steps inside available time
Tpulses = abs(nSteps) / frequency; //
Tstartf = (lcncCycleTime - Tpulses) / 2.0; //
} //
else // Regular step train, up or down
{ //
float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation. position = kTRAJ x time + mTRAJ
float mTRAJ = initialPosition; // Operating on incoming positions (not steps)
if (kTRAJ > 0) //
Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
else //
Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing downwards
frequency = fabs(kTRAJ * stepsPerMM); //
Tpulses = abs(nSteps) / frequency;
}
updatePos(5);
uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
dbg = timeSinceISR; //
Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging
return updatePos(1);
timerFrequency = uint32_t(ceil(frequency));
startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
startTimer->refresh();
startTimer->resume();
return 1;
}
void StepGen2::startTimerCB()
{
startTimer->pause(); // Once is enough.
digitalWriteFast(dirPinName, nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH
// There will be a short break here for t2 usecs, in the future.
timerEndPosition += nSteps;
pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
// pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
pulseTimer->refresh();
pulseTimer->resume();
}
void StepGen2::pulseTimerCB()
{
int16_t dir = digitalReadFast(dirPinName); //
if (dir == HIGH) // The step just taken
timerPosition--;
else
timerPosition++;
int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps
if (diffPosition == 0)
pulseTimer->pause();
else
{
if (diffPosition < 0 && dir == LOW) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, HIGH); // Normal is to be HIGH when decreasing
if (diffPosition > 0 && dir == HIGH) // Change direction. Should not end up here, but alas
digitalWriteFast(dirPinName, LOW); // Normal is to be LOW when increasing
// Normally nothing is needed
}
}
uint32_t StepGen2::updatePos(uint32_t i)
{ //
initialPosition = commandedPosition; // Save the numeric position for next step
initialStepPosition = commandedStepPosition; // also the step we are at}
return i;
}
uint32_t StepGen2::sync0CycleTime = 0;

1195
Firmware/src/StepGen3.cpp Executable file

File diff suppressed because it is too large Load Diff

View File

@@ -263,7 +263,7 @@ void GPIO_PinAF(GPIO_TypeDef *GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF)
GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2;
}
void rcc_config()
void encoder_config()
{
RCC->AHB1ENR |= 0x1; // GPIOA
RCC->AHB1ENR |= 0x4; // GPIOC
@@ -271,13 +271,13 @@ void rcc_config()
RCC->AHB1ENR |= 0x10; // GPIOE
RCC->APB1ENR |= 0x20000000; // ENABLE DAC
// RCC->APB2ENR |= 0x00000002; // APB2 TIM8
// RCC->APB2ENR |= 0x00000002; // APB2 TIM8
RCC->APB1ENR |= 0x00000004; // APB1 TIM4
RCC->APB1ENR |= 0x00000001; // APB1 TIM2
// RCC->APB1ENR |= 0x00000002; // APB1 TIM3
// RCC->APB1ENR |= 0x00000002; // APB1 TIM3
GpioConfigPortA(GPIOA);
// GpioConfigPortC(GPIOC);
// GpioConfigPortC(GPIOC);
GpioConfigPortD(GPIOD);
#if 0 // Skipping since TIM8 is step generator and TIM3, chan4 is smae as TIM8, chan4
GPIO_PinAF(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
@@ -326,4 +326,66 @@ void rcc_config()
TIM3->CNT = 0;
#endif
}
void encoder2_config()
{
#if 0
#include "mbed.h"
#include "stm32f4xx.h"
#include "stm32f4xx_hal_tim_ex.h"
TIM_HandleTypeDef timer;
TIM_Encoder_InitTypeDef encoder;
// direction to PA_9 -- step pulse to PA_8
int main()
{
GPIO_InitTypeDef GPIO_InitStruct;
__TIM1_CLK_ENABLE();
__GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
timer.Instance = TIM1;
timer.Init.Period = 0xffff;
timer.Init.Prescaler = 0;
timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timer.Init.CounterMode = TIM_COUNTERMODE_UP;
encoder.EncoderMode = TIM_ENCODERMODE_TI12;
encoder.IC1Filter = 0x0f;
encoder.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
encoder.IC1Prescaler = TIM_ICPSC_DIV4;
encoder.IC1Selection = TIM_ICSELECTION_DIRECTTI;
encoder.IC2Filter = 0x0f;
encoder.IC2Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
encoder.IC2Prescaler = TIM_ICPSC_DIV4;
encoder.IC2Selection = TIM_ICSELECTION_DIRECTTI;
HAL_TIM_Encoder_Init(&timer, &encoder);
HAL_TIM_Encoder_Start(&timer,TIM_CHANNEL_1);
TIM1->EGR = 1; // Generate an update event
TIM1->CR1 = 1; // Enable the counter
while (1) {
int16_t count1;
count1=TIM1->CNT;
printf("%d\r\n", count1);
wait(1.0);
};
}
#endif
}

18
Firmware/src/extend32to64.cpp Executable file
View File

@@ -0,0 +1,18 @@
#include "extend32to64.h"
// Extend from 32-bit to 64-bit precision
int64_t extend32to64::extendTime(uint32_t in)
{
int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
int64_t dif = (c64 - previousTimeValue); // 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 < int64_t(-HALF_PERIOD))
mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
int64_t unwrapped = previousTimeValue + mod_dif;
previousTimeValue = unwrapped; // load previous value
return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
}

View File

@@ -9,76 +9,137 @@ _Objects Obj;
HardwareSerial Serial1(PA10, PA9);
#define DEBUG_TIM8 1
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
HardwareTimer *baseTimer; // The base period timer
HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle"
uint16_t sync0CycleTime; // usecs
#include "MyEncoder.h"
volatile uint16_t encCnt = 0;
void indexPulseEncoderCB1(void);
MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1);
void indexPulseEncoderCB1(void)
{
encCnt++;
Encoder1.indexPulse();
}
// #include <RunningAverage.h>
// RunningAverage irqServeDelays(1000); // To get the max delay of the irq serve time over the last second
CircularBuffer<uint16_t, 1000> irqServeDelays;
#include "StepGen.h"
#include "StepGen3.h"
StepGen3 *Step = 0;
void timerCallbackStep1(void);
StepGen Step1(TIM1, 4, PA_11, PA12, timerCallbackStep1);
void timerCallbackStep1(void)
{
Step1.timerCB();
}
#include "extend32to64.h"
void timerCallbackStep2(void);
StepGen Step2(TIM3, 4, PC_9, PC10, timerCallbackStep2);
void timerCallbackStep2(void)
{
Step2.timerCB();
}
volatile uint64_t irqTime = 0, irqCnt = 0;
extend32to64 longTime;
CircularBuffer<uint32_t, 200> Tim;
volatile uint64_t nowTime = 0, thenTime = 0;
void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs);
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
{
Encoder1.setLatch(Obj.IndexLatchEnable);
Encoder1.setScale(Obj.EncPosScale);
Step1.reqPos(Obj.StepGenIn1.CommandedPosition);
Step1.setScale(Obj.StepGenIn1.StepsPerMM);
Step1.enable(Obj.Enable1);
Step2.reqPos(Obj.StepGenIn2.CommandedPosition);
Step2.setScale(Obj.StepGenIn2.StepsPerMM);
Step2.enable(Obj.Enable1);
Encoder1.setScale(2000);
}
volatile uint16_t basePeriodCnt;
volatile uint64_t makePulsesCnt = 0, prevMakePulsesCnt = 0;
volatile uint16_t deltaMakePulsesCnt;
volatile double pos_cmd1, pos_cmd2;
void syncWithLCNC()
{
syncTimer->pause();
baseTimer->pause();
deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt;
prevMakePulsesCnt = makePulsesCnt;
Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions
Step->makeAllPulses(); // Make first step right here
basePeriodCnt = SERVO_PERIOD / BASE_PERIOD; //
baseTimer->refresh(); //
baseTimer->resume(); // Make the other steps in ISR
}
void basePeriodCB(void)
{
if (--basePeriodCnt > 0) // Stop
Step->makeAllPulses();
else
baseTimer->pause();
}
int32_t delayT;
uint16_t thisCycleTime; // In usecs
int16_t maxIrqServeTime = 0;
uint64_t oldIrqTime = 0;
uint16_t nLoops;
void handleStepper(void)
{
Step1.handleStepper();
Step2.handleStepper();
if (oldIrqTime != 0) // See if there is a delay in data, normally it *should* be nLoops=1, but sometimes it is more
{
thisCycleTime = irqTime - oldIrqTime;
nLoops = round(float(thisCycleTime) / float(sync0CycleTime));
}
oldIrqTime = irqTime;
uint32_t diffT = longTime.extendTime(micros()) - irqTime; // Time from interrupt was received by isr
irqServeDelays.push(diffT);
if (irqServeDelays.isFull()) // Do max calcs, just waiting a second
{
uint16_t maxInBuffer = 0;
using index_t = decltype(irqServeDelays)::index_t;
for (index_t i = 0; i < irqServeDelays.size(); i++)
{
if (maxInBuffer < irqServeDelays[i])
maxInBuffer = irqServeDelays[i];
}
if (maxIrqServeTime > maxInBuffer) // Reduce by one, slowly eating up excess time
maxIrqServeTime--;
if (maxIrqServeTime < maxInBuffer)
maxIrqServeTime = maxInBuffer;
}
pos_cmd1 = Obj.CommandedPosition1;
pos_cmd2 = Obj.CommandedPosition2;
Obj.ActualPosition1 = Obj.CommandedPosition1;
Obj.ActualPosition2 = Obj.CommandedPosition2;
Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
// Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
// Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin
if (delayT > 0 && delayT < 900)
{
syncTimer->setOverflow(delayT, MICROSEC_FORMAT); // Work in flawed units, its ok
syncTimer->refresh();
syncTimer->resume();
}
else
{
syncWithLCNC();
}
}
float_t oldCommandedPosition = 0;
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
Obj.IndexStatus = Encoder1.indexHappened();
Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros()));
Obj.IndexByte = Encoder1.getIndexState();
Obj.StepGenOut1.ActualPosition = Step1.actPos();
Obj.StepGenOut2.ActualPosition = Step2.actPos();
uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
Tim.push(dTim);
uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++)
{
uint32_t aTim = Tim[i];
if (aTim > max_Tim)
max_Tim = aTim;
if (aTim < min_Tim)
min_Tim = aTim;
}
thenTime = nowTime;
Obj.DiffT = max_Tim - min_Tim; // Debug
Obj.DiffT = nLoops;
Obj.D1 = 1000 * Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
Obj.D2 = 1000 * Obj.ActualPosition2; // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX; // Step->stepgen_array[1].freq;
Obj.D3 = encCnt % 256; // Step->stepgen_array[1].freq;
Obj.D4 = 1000 * (Obj.CommandedPosition2 - oldCommandedPosition); // deltaMakePulsesCnt; // Step->stepgen_array[1].rawcount % UINT16_MAX;
oldCommandedPosition = Obj.CommandedPosition2;
}
void ESC_interrupt_enable(uint32_t mask);
@@ -106,35 +167,63 @@ static esc_cfg_t config =
.esc_check_dc_handler = dc_checker,
};
void measureCrystalFrequency(void);
volatile byte serveIRQ = 0;
void setup(void)
{
Serial1.begin(115200);
rcc_config(); // probably breaks some timers.
#if 0
measureCrystalFrequency(); // Calibrate crystal frequency
#endif
Step = new StepGen3;
encoder_config(); // Needed by encoder, probably breaks some timers.
ecat_slv_init(&config);
pinMode(PA11, OUTPUT); // Step X
pinMode(PA12, OUTPUT); // Dir X
pinMode(PC9, OUTPUT); // Step Z
pinMode(PC10, OUTPUT); // Dir Z
baseTimer = new HardwareTimer(TIM11); // The base period timer
setFrequencyAdjustedMicrosSeconds(baseTimer, BASE_PERIOD / 1000);
// baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); // Or the line above, This one is uncalibrated
baseTimer->attachInterrupt(basePeriodCB);
syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer
syncTimer->attachInterrupt(syncWithLCNC);
}
void loop(void)
{
uint64_t dTime;
if (serveIRQ)
{
nowTime = micros();
DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
serveIRQ = 0;
ESCvar.PrevTime = ESCvar.Time;
ecat_slv_poll();
}
uint32_t dTime = micros() - nowTime;
if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt
dTime = longTime.extendTime(micros()) - irqTime;
if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt
ecat_slv_poll();
}
void sync0Handler(void)
{
serveIRQ = 1;
ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{
irqTime = longTime.extendTime(micros());
serveIRQ = 1;
}
irqCnt++; // debug output
}
// Enable SM2 interrupts
void ESC_interrupt_enable(uint32_t mask)
{
// Enable interrupt for SYNC0 or SM2 or SM3
@@ -143,6 +232,7 @@ void ESC_interrupt_enable(uint32_t mask)
if (mask & user_int_mask)
{
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
// Set LAN9252 interrupt pin driver as push-pull active high
@@ -155,6 +245,7 @@ void ESC_interrupt_enable(uint32_t mask)
}
}
// Disable SM2 interrupts
void ESC_interrupt_disable(uint32_t mask)
{
// Enable interrupt for SYNC0 or SM2 or SM3
@@ -179,6 +270,63 @@ uint16_t dc_checker(void)
{
// Indicate we run DC
ESCvar.dcsync = 1;
StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
return 0;
}
//
// Code to calibrate the crystal.
//
#include <HardwareTimer.h>
HardwareTimer *timer;
volatile uint32_t cnt;
void CB(void)
{
if (cnt-- == 0)
{
timer->pause();
}
}
void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs)
{
const uint16_t calibrated1000 = 1042; // <- This is the factor to adjust to make 1 sec = 1 sec
uint32_t period_cyc = (usecs * (timer->getTimerClkFreq() / 1000)) / calibrated1000; // Avoid overflow during math
uint32_t Prescalerfactor = (period_cyc / 0x10000) + 1;
uint32_t PeriodTicks = period_cyc / Prescalerfactor;
timer->setPrescaleFactor(Prescalerfactor);
timer->setOverflow(PeriodTicks, TICK_FORMAT);
// Serial1.printf("Period_cyc=%u Prescalefactor =%u ticks = %u\n", period_cyc, Prescalerfactor, PeriodTicks);
}
void measureCrystalFrequency(void)
{
timer = new HardwareTimer(TIM1);
Serial1.begin(115200);
delay(3000);
Serial1.printf("Clock freq = %u\n", timer->getTimerClkFreq());
setFrequencyAdjustedMicrosSeconds(timer, 1000);
timer->refresh();
timer->attachInterrupt(CB);
cnt = 10000;
Serial1.printf("\n");
uint32_t startT = micros();
timer->resume();
while (cnt != 0)
;
uint32_t endT = micros();
uint32_t diffT = endT - startT;
Serial1.printf("\n");
Serial1.printf("diff = %u\n", diffT);
Serial1.printf("\n");
delay(10000);
Serial1.printf("\n");
exit(0);
}

124
PortStepgen/.vscode/c_cpp_properties.json vendored Executable file
View File

@@ -0,0 +1,124 @@
//
// !!! WARNING !!! AUTO-GENERATED FILE!
// PLEASE DO NOT MODIFY IT AND USE "platformio.ini":
// https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags
//
{
"configurations": [
{
"name": "PlatformIO",
"includePath": [
"Z:/src/MyOwnEtherCATDevice/PortStepgen/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/avr",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/LL",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/OpenAMP",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb/hid",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb/cdc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/STM32F4xx_HAL_Driver/Inc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/STM32F4xx_HAL_Driver/Src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/STM32F4xx",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/ST/STM32_USB_Device_Library/Core/Src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/open-amp/lib/include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/libmetal/lib/include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/virtual_driver",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/Core/Include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/DSP/Include",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/DSP/PrivateInclude",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/variants/STM32F4xx/F407V(E-G)T_F417V(E-G)T",
"C:/Users/Hakan/.platformio/lib/RunningAverage",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/CMSIS_DSP/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/EEPROM/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/IWatchdog/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Keyboard/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Mouse/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/RGB_LED_TLC59731/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SPI/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Servo/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SoftwareSerial/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SubGhz/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Wire/src",
""
],
"browse": {
"limitSymbolsToIncludedHeaders": true,
"path": [
"Z:/src/MyOwnEtherCATDevice/PortStepgen/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/avr",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/LL",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/OpenAMP",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb/hid",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino/stm32/usb/cdc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/STM32F4xx_HAL_Driver/Inc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/STM32F4xx_HAL_Driver/Src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/STM32F4xx",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/ST/STM32_USB_Device_Library/Core/Inc",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/ST/STM32_USB_Device_Library/Core/Src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/open-amp/lib/include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/libmetal/lib/include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Middlewares/OpenAMP/virtual_driver",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/Core/Include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/system/Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/DSP/Include",
"C:/Users/Hakan/.platformio/packages/framework-cmsis/CMSIS/DSP/PrivateInclude",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/cores/arduino",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/variants/STM32F4xx/F407V(E-G)T_F417V(E-G)T",
"C:/Users/Hakan/.platformio/lib/RunningAverage",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/CMSIS_DSP/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/EEPROM/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/IWatchdog/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Keyboard/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Mouse/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/RGB_LED_TLC59731/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SPI/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Servo/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SoftwareSerial/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/SubGhz/src",
"C:/Users/Hakan/.platformio/packages/framework-arduinoststm32/libraries/Wire/src",
""
]
},
"defines": [
"PLATFORMIO=60114",
"STM32F407xx",
"STM32F4",
"STM32",
"PIO_FRAMEWORK_ARDUINO_NANOLIB_FLOAT_PRINTF",
"STM32F4xx",
"ARDUINO=10808",
"ARDUINO_ARCH_STM32",
"NDEBUG",
"ARDUINO_GENERIC_F407VGTX",
"BOARD_NAME=\"GENERIC_F407VGTX\"",
"HAL_UART_MODULE_ENABLED",
"USE_FULL_LL_DRIVER",
"VARIANT_H=\"variant_generic.h\"",
"VECT_TAB_OFFSET=0x0",
""
],
"cStandard": "gnu17",
"cppStandard": "gnu++17",
"compilerPath": "C:/Users/Hakan/.platformio/packages/toolchain-gccarmnoneeabi/bin/arm-none-eabi-gcc.exe",
"compilerArgs": [
"-mcpu=cortex-m4",
"-mthumb",
"-mfpu=fpv4-sp-d16",
"-mfloat-abi=hard",
""
]
}
],
"version": 4
}

95
PortStepgen/.vscode/launch.json vendored Executable file
View File

@@ -0,0 +1,95 @@
// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY
//
// PlatformIO Debugging Solution
//
// Documentation: https://docs.platformio.org/en/latest/plus/debugging.html
// Configuration: https://docs.platformio.org/en/latest/projectconf/sections/env/options/debug/index.html
{
"version": "0.2.0",
"configurations": [
{
"name": "(gdb) Launch",
"type": "cppdbg",
"request": "launch",
"program": "${workspaceFolder}/${fileBasenameNoExtension}",
"args": [],
"stopAtEntry": false,
"cwd": "${fileDirname}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
},
{
"description": "Set Disassembly Flavor to Intel",
"text": "-gdb-set disassembly-flavor intel",
"ignoreFailures": true
}
]
},
{
"type": "platformio-debug",
"request": "launch",
"name": "PIO Debug",
"executable": "Z:/src/MyOwnEtherCATDevice/PortStepgen/.pio/build/genericSTM32F407VGT6/firmware.elf",
"projectEnvName": "genericSTM32F407VGT6",
"toolchainBinDir": "C:/Users/Hakan/.platformio/packages/toolchain-gccarmnoneeabi/bin",
"internalConsoleOptions": "openOnSessionStart",
"svdPath": "C:/Users/Hakan/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd",
"preLaunchTask": {
"type": "PlatformIO",
"task": "Pre-Debug"
}
},
{
"type": "platformio-debug",
"request": "launch",
"name": "PIO Debug (skip Pre-Debug)",
"executable": "Z:/src/MyOwnEtherCATDevice/PortStepgen/.pio/build/genericSTM32F407VGT6/firmware.elf",
"projectEnvName": "genericSTM32F407VGT6",
"toolchainBinDir": "C:/Users/Hakan/.platformio/packages/toolchain-gccarmnoneeabi/bin",
"internalConsoleOptions": "openOnSessionStart",
"svdPath": "C:/Users/Hakan/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd"
},
{
"type": "platformio-debug",
"request": "launch",
"name": "PIO Debug (without uploading)",
"executable": "Z:/src/MyOwnEtherCATDevice/PortStepgen/.pio/build/genericSTM32F407VGT6/firmware.elf",
"projectEnvName": "genericSTM32F407VGT6",
"toolchainBinDir": "C:/Users/Hakan/.platformio/packages/toolchain-gccarmnoneeabi/bin",
"internalConsoleOptions": "openOnSessionStart",
"svdPath": "C:/Users/Hakan/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd",
"loadMode": "manual"
}
],
"tasks": [
{
"type": "shell",
"label": "C/C++: g++ build active file",
"command": "/usr/bin/g++",
"args": [
"-g",
"${file}",
"-o",
"${fileDirname}/${fileBasenameNoExtension}"
],
"options": {
"cwd": "/usr/bin"
},
"problemMatcher": [
"$gcc"
],
"group": {
"kind": "build",
"isDefault": true
},
"detail": "Task generated by Debugger."
}
]
}

12
PortStepgen/platformio.ini Executable file
View File

@@ -0,0 +1,12 @@
[env:genericSTM32F407VGT6]
framework = arduino
platform = ststm32
board = genericSTM32F407VGT6
upload_protocol = stlink
debug_tool = stlink
debug_build_flags = -O0 -g -ggdb
monitor_port = COM7
;monitor_port = /dev/ttyUSB0
monitor_speed = 115200
build_flags = -Wl,--no-warn-rwx-segment -DSTM32 -DPIO_FRAMEWORK_ARDUINO_NANOLIB_FLOAT_PRINTF

1557
PortStepgen/src/main.cpp Normal file

File diff suppressed because it is too large Load Diff

View File

@@ -6,6 +6,22 @@ that I used during the creation of my own EtherCAT device.
This job is documented in a series of Youtube videos, from my first attempts to understand
how EtherCAT works, to making my own pcb, program it and testing it in LinuxCNC.
=======
## Make my own EtherCAT device 9. About time. Step generator
This was something, I almost gave up. But here it is. A working step generator.
Actually, not one but two step generators for the EaserCAT 2000 board.
For a step generator it is all about timing, timing and timing.
I'll walk you through what I had to do to get it to work.
It can give some general insight into how EtherCAT for linuxcnc works.
Biggest changes this time are in the [Firmware folder](Firmware).
The [Documentation folder](Documentation) has design details on the second step generator.
A small but fatal indexing bug was fixed in the EEPROM_generator.
For all details, read the git log. Too much to mention all.
[![Watch the video](https://img.youtube.com/vi/lBDBcseFki8/default.jpg)](https://youtu.be/lBDBcseFki8)
## Make my own EtherCAT device 8. EaserCAT 3000
@@ -18,7 +34,6 @@ The KiCAD files are in the [KiCAD folder](Kicad/Ax58100-stm32-ethercat)
[![Watch the video](https://img.youtube.com/vi/boanv6ihYtI/default.jpg)](https://youtu.be/boanv6ihYtI)
## Make my own EtherCAT device 7. Turning in the lathe
I have now put things together so the EaserCAT 2000 card controls
@@ -70,8 +85,6 @@ My first tries with the LAN9252 chip. Getting accustomed to some tools.
[![Watch the video](https://img.youtube.com/vi/IGmXsXSSA4s/default.jpg)](https://youtu.be/IGmXsXSSA4s)
# EtherCAT PCB based on LAN9252 and STM34F407
##### The firmware is in the **Firmware** folder.

View File

@@ -0,0 +1,165 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = gmoccapy_lathe
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
EMBED_TAB_NAME = Cycles
EMBED_TAB_LOCATION = ntb_preview
#EMBED_TAB_LOCATION = ntb_user_tabs
EMBED_TAB_COMMAND = halcmd loadusr -Wn gladevcp gladevcp -c gladevcp -U notouch=1 -U norun=0 -u lathehandler.py -x {XID} lathemacro.ui
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros:./
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
#POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
JOINTS = 2
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

File diff suppressed because one or more lines are too long

After

Width:  |  Height:  |  Size: 4.8 MiB

View File

@@ -0,0 +1,72 @@
This folder contains a number of simulated lathe configs.
the "lathemacro" config offers a number of simple macros to perform
the most common lathe operations.
The GUI that controls the macros can be viewed by clicking the "Cycles"
tab top left of the graphical preview window.
There are two load-time options to control the tab behaviour:
1) norun will hide the action button, use this if you want to use only
a physical button (connected to gladevcp.cycle-start) to start the macros
(Strongly recommended, especially with Touchy)
2) notouch will allow keyboard editing of the spinboxes. Otherwise the
custom numeric keyboard will be shown.
An example loadrt line, as used here in the Gmoccapy demo is:
[DISPLAY]
EMBED_TAB_COMMAND = halcmd loadusr -Wn gladevcp gladevcp -c gladevcp -U notouch=1 -U norun=0 -u lathehandler.py -x {XID} lathemacro.ui
The window will resize slowly if you grab the corner and move the mouse
inside the window. It's not as bad as it was, but still needs work.
You may need to click the unmaximise button in the toolbar to get a
window border to be able to use the resize handles.
Notes on the keyboard:
As well as the obvious functions and unit conversions, it can be used to
enter fractions. For example if you type 1.1/2 it will automatically
update to display 1.5000 and 16.17/64 will show 16.2656.
This can be used in a limited way to halve the onscreen value eg for
entering radius instead of diameter.
However it only works for whole numbers: 100/2 will become 50 but
3.14149/2 is interpreted as 3 and 14 thousand halves so won't work.
Notes on adding your own cycles:
Create a new G-code subroutine in the same format as the existing ones.
In Glade add a new tab to the 'tabs1' notebook and give it a name matching
the new cycle.
Edit the action button (inside an eventbox) to call the new G-code sub.
Rename the action button to match the tab name and append '.action' eg
MyCycle.action
Create new artwork. I used Fusion360, the models are here:
https://a360.co/3uFPZNv
and the drawings are here:
https://a360.co/3uFPZNv
Esport the drawing page as PDF and import into the lathemacro.svg file in
Inkscape. You will need to resize. Add your own arrows and annotations.
Save the new layer in a layer named "layerN" (lower case) where N is the
tab number, starting at zero. You will need to invoke the XML editor for
this (Shift-Cmd-X on Mac)
The entry boxes are positioned relative to a 1500 x 1000 image; the
original size of the SVG. So you can hover your mouse over the image in
Inkscape to determine the coordinates.
In the in the case of on-drawing controls the coordinates are entered as
an XML comment in the Tooltip for the control in x,y format (The surface speed,
tool and coolant do not need this, they are in a fixed table)
An example:
<!--300,711->Peck distance
Make sure the comment has "use markup" selected for the tooltip.
Also ensure that the control has the "show_keyb" function allocated for
the Widget->button_press event. If you copy-paste a tab and copy-paste
extra controls this should be automatic.
_All_ your new spinboxes will need their own new Adjustment, or they will
change value when you alter the original spinbox that they are a copy of.

View File

@@ -0,0 +1,159 @@
# Generated by PNCconf at Sat Sep 10 10:10:56 2016
# If you make changes to this file, they will be
# overwritten when you run PNCconf again
loadrt [KINS]KINEMATICS
#autoconverted trivkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadusr -W lcec_conf ethercat-conf.xml
loadrt lcec
loadrt metalmusings_encoder
addf lcec.read-all servo-thread
addf metalmusings-encoder.0 servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
#*******************
# AXIS X
#*******************
# --- joint signals for motion
net x-pos-cmd <= joint.0.motor-pos-cmd
net x-vel-cmd <= joint.0.vel-cmd
net x-pos-fb <= joint.0.motor-pos-fb
net x-enable <= joint.0.amp-enable-out
# --- connect stepper driver to joint
# 1/2 of stepper's p/rev 2000 => 1000
setp lcec.0.0.steps-per-mm-1 200
net x-pos-cmd => lcec.0.0.commanded-position-1
net x-pos-fb => lcec.0.0.actual-position-1
#net x-enable => lcec.0.0.enable
#*******************
# AXIS Z
#*******************
# --- joint signals for motion
net z-pos-cmd <= joint.1.motor-pos-cmd
net z-vel-cmd <= joint.1.vel-cmd
net z-pos-fb <= joint.1.motor-pos-fb
net z-enable <= joint.1.amp-enable-out
# --- connect stepper driver to the joint
setp lcec.0.0.steps-per-mm-2 80
# 1/5 of drive's step/rev
net z-pos-cmd => lcec.0.0.commanded-position-2
net z-pos-fb => lcec.0.0.actual-position-2
# net z-enable => lcec.0.0.enable
#*********************
# E-STOP
#*********************
setp iocontrol.0.emc-enable-in 1
#net estop iocontrol.0.emc-enable-in lcec.0.1.din-0
#setp lcec.0.3.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
#setp lcec.0.0.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
#net spindle-revs <= lcec.0.3.enc-pos
net spindle-revs <= lcec.0.0.enc-position
loadrt invert
loadrt mult2 names=mult2.rps,mult2.rpm
addf invert.0 servo-thread
addf mult2.rps servo-thread
addf mult2.rpm servo-thread
setp invert.0.in [SPINDLE_9]ENCODER_SCALE
setp mult2.rpm.in0 -60.0
setp mult2.rpm.in0 60.0
#net enc-invert-pos-scale mult2.rps.in0 <= invert.0.out
#net enc-get-freq-rps mult2.rps.in1 <= lcec.0.3.enc-frequency
#net spindle-vel-fb-rps mult2.rpm.in1 <= mult2.rps.out
#net spindle-vel-fb-rpm mult2.rpm.out
#net spindle-index-enable lcec.0.3.enc-index-c-enable
net spindle-vel-fb-rps mult2.rpm.in1 <= lcec.0.0.enc-frequency
net spindle-vel-fb-rpm mult2.rpm.out
net to_encoder metalmusings-encoder.0.index-latch-enable lcec.0.0.enc-index-latch-enable
net from_encoder metalmusings-encoder.0.index-status lcec.0.0.index-status
net spindle-index-enable metalmusings-encoder.0.index-c-enable
# ---setup spindle control signals---
net spindle-vel-cmd-rps <= spindle.0.speed-out-rps
net spindle-vel-cmd-rps-abs <= spindle.0.speed-out-rps-abs
net spindle-vel-cmd-rpm <= spindle.0.speed-out
net spindle-vel-cmd-rpm-abs <= spindle.0.speed-out-abs
net spindle-enable <= spindle.0.on
net spindle-cw <= spindle.0.forward
net spindle-ccw <= spindle.0.reverse
net spindle-brake <= spindle.0.brake
net spindle-revs => spindle.0.revs
net spindle-at-speed => spindle.0.at-speed
net spindle-vel-fb-rps => spindle.0.speed-in
net spindle-index-enable <=> spindle.0.index-enable
#******************************
# connect miscellaneous signals
#******************************
# ---HALUI signals---
net joint-select-a halui.axis.x.select
net x-is-homed halui.joint.0.is-homed
net jog-x-pos halui.axis.x.plus
net jog-x-neg halui.axis.x.minus
net jog-x-analog halui.axis.x.analog
net joint-select-b halui.axis.z.select
net z-is-homed halui.joint.1.is-homed
net jog-z-pos halui.axis.z.plus
net jog-z-neg halui.axis.z.minus
net jog-z-analog halui.axis.z.analog
net jog-selected-pos halui.axis.selected.plus
net jog-selected-neg halui.axis.selected.minus
net spindle-manual-cw halui.spindle.0.forward
net spindle-manual-ccw halui.spindle.0.reverse
net spindle-manual-stop halui.spindle.0.stop
net machine-is-on halui.machine.is-on
net jog-speed halui.axis.jog-speed
net MDI-mode halui.mode.is-mdi
# ---coolant signals---
net coolant-mist <= iocontrol.0.coolant-mist
net coolant-flood <= iocontrol.0.coolant-flood
# ---probe signal---
net probe-in => motion.probe-input
# ---motion control signals---
net in-position <= motion.in-position
net machine-is-enabled <= motion.motion-enabled
# ---digital in / out signals---
# ---estop signals---
net estop-out <= iocontrol.0.user-enable-out
net estop-ext => iocontrol.0.emc-enable-in
# ---manual tool change signals---
loadusr -W hal_manualtoolchange
#net tool-change-request iocontrol.0.tool-change => hal_manualtoolchange.change
#net tool-change-confirmed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
#net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
#net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared
addf lcec.write-all servo-thread

View File

@@ -0,0 +1,159 @@
# Generated by PNCconf at Sat Sep 10 10:10:56 2016
# If you make changes to this file, they will be
# overwritten when you run PNCconf again
loadrt [KINS]KINEMATICS
#autoconverted trivkins
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadusr -W lcec_conf ethercat-conf.xml
loadrt lcec
loadrt metalmusings_encoder
addf lcec.read-all servo-thread
addf metalmusings-encoder.0 servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
#*******************
# AXIS X
#*******************
# --- joint signals for motion
net x-pos-cmd <= joint.0.motor-pos-cmd
net x-vel-cmd <= joint.0.vel-cmd
net x-pos-fb <= joint.0.motor-pos-fb
net x-enable <= joint.0.amp-enable-out
# --- connect stepper driver to joint
# 1/2 of stepper's p/rev 2000 => 1000
setp lcec.0.0.steps-per-mm-1 500
net x-pos-cmd => lcec.0.0.commanded-position-1
net x-pos-fb => lcec.0.0.actual-position-1
#net x-enable => lcec.0.0.enable
#*******************
# AXIS Z
#*******************
# --- joint signals for motion
net z-pos-cmd <= joint.1.motor-pos-cmd
net z-vel-cmd <= joint.1.vel-cmd
net z-pos-fb <= joint.1.motor-pos-fb
net z-enable <= joint.1.amp-enable-out
# --- connect stepper driver to the joint
setp lcec.0.0.steps-per-mm-2 400
# 1/5 of drive's step/rev
net z-pos-cmd => lcec.0.0.commanded-position-2
net z-pos-fb => lcec.0.0.actual-position-2
# net z-enable => lcec.0.0.enable
#*********************
# E-STOP
#*********************
setp iocontrol.0.emc-enable-in 1
#net estop iocontrol.0.emc-enable-in lcec.0.1.din-0
#setp lcec.0.3.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
setp lcec.0.0.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
#net spindle-revs <= lcec.0.3.enc-pos
net spindle-revs <= lcec.0.0.enc-position
loadrt invert
loadrt mult2 names=mult2.rps,mult2.rpm
addf invert.0 servo-thread
addf mult2.rps servo-thread
addf mult2.rpm servo-thread
setp invert.0.in [SPINDLE_9]ENCODER_SCALE
setp mult2.rpm.in0 -60.0
setp mult2.rpm.in0 60.0
#net enc-invert-pos-scale mult2.rps.in0 <= invert.0.out
#net enc-get-freq-rps mult2.rps.in1 <= lcec.0.3.enc-frequency
#net spindle-vel-fb-rps mult2.rpm.in1 <= mult2.rps.out
#net spindle-vel-fb-rpm mult2.rpm.out
#net spindle-index-enable lcec.0.3.enc-index-c-enable
net spindle-vel-fb-rps mult2.rpm.in1 <= lcec.0.0.enc-frequency
net spindle-vel-fb-rpm mult2.rpm.out
net to_encoder metalmusings-encoder.0.index-latch-enable lcec.0.0.enc-index-latch-enable
net from_encoder metalmusings-encoder.0.index-status lcec.0.0.index-status
net spindle-index-enable metalmusings-encoder.0.index-c-enable
# ---setup spindle control signals---
net spindle-vel-cmd-rps <= spindle.0.speed-out-rps
net spindle-vel-cmd-rps-abs <= spindle.0.speed-out-rps-abs
net spindle-vel-cmd-rpm <= spindle.0.speed-out
net spindle-vel-cmd-rpm-abs <= spindle.0.speed-out-abs
net spindle-enable <= spindle.0.on
net spindle-cw <= spindle.0.forward
net spindle-ccw <= spindle.0.reverse
net spindle-brake <= spindle.0.brake
net spindle-revs => spindle.0.revs
net spindle-at-speed => spindle.0.at-speed
net spindle-vel-fb-rps => spindle.0.speed-in
net spindle-index-enable <=> spindle.0.index-enable
#******************************
# connect miscellaneous signals
#******************************
# ---HALUI signals---
net joint-select-a halui.axis.x.select
net x-is-homed halui.joint.0.is-homed
net jog-x-pos halui.axis.x.plus
net jog-x-neg halui.axis.x.minus
net jog-x-analog halui.axis.x.analog
net joint-select-b halui.axis.z.select
net z-is-homed halui.joint.1.is-homed
net jog-z-pos halui.axis.z.plus
net jog-z-neg halui.axis.z.minus
net jog-z-analog halui.axis.z.analog
net jog-selected-pos halui.axis.selected.plus
net jog-selected-neg halui.axis.selected.minus
net spindle-manual-cw halui.spindle.0.forward
net spindle-manual-ccw halui.spindle.0.reverse
net spindle-manual-stop halui.spindle.0.stop
net machine-is-on halui.machine.is-on
net jog-speed halui.axis.jog-speed
net MDI-mode halui.mode.is-mdi
# ---coolant signals---
net coolant-mist <= iocontrol.0.coolant-mist
net coolant-flood <= iocontrol.0.coolant-flood
# ---probe signal---
net probe-in => motion.probe-input
# ---motion control signals---
net in-position <= motion.in-position
net machine-is-enabled <= motion.motion-enabled
# ---digital in / out signals---
# ---estop signals---
net estop-out <= iocontrol.0.user-enable-out
net estop-ext => iocontrol.0.emc-enable-in
# ---manual tool change signals---
loadusr -W hal_manualtoolchange
#net tool-change-request iocontrol.0.tool-change => hal_manualtoolchange.change
#net tool-change-confirmed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
#net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
#net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared
addf lcec.write-all servo-thread

View File

@@ -0,0 +1,164 @@
# This config file was created 2016-11-26 11:13:55.093594 by the update_ini script
# The original config files may be found in the /home/debian/linuxcnc/configs/Turner/Turner.old directory
# Generated by PNCconf at Fri Sep 9 22:31:33 2016
# If you make changes to this file, they will be
# overwritten when you run PNCconf again
[EMC]
# The version string for this INI file.
VERSION = 1.1
MACHINE = Turner
DEBUG = 0
[DISPLAY]
LATHE=1
DISPLAY = gmoccapy
BACK_TOOL_LATHE = 0
CYCLE_TIME = 150
POSITION_OFFSET = RELATIVE
POSITION_FEEDBACK = ACTUAL
MAX_FEED_OVERRIDE = 2.000000
MAX_SPINDLE_OVERRIDE = 2.000000
MIN_SPINDLE_OVERRIDE = 0.500000
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 1
PROGRAM_PREFIX = /home/debian/linuxcnc/configs/Turner/nc_files
INCREMENTS = 5mm 1mm .5mm .1mm .05mm .01mm .005mm
LATHE = 1
POSITION_FEEDBACK = ACTUAL
DEFAULT_LINEAR_VELOCITY = 6.000000
MAX_LINEAR_VELOCITY = 25.000000
MIN_LINEAR_VELOCITY = 0.500000
DEFAULT_ANGULAR_VELOCITY = 12.000000
MAX_ANGULAR_VELOCITY = 1800.000000
MIN_ANGULAR_VELOCITY = 1.666667
EDITOR = gedit
#EMBED_TAB_NAME = Cycles
#EMBED_TAB_LOCATION = ntb_preview
#EMBED_TAB_LOCATION = ntb_user_tabs
#EMBED_TAB_COMMAND = halcmd loadusr -Wn gladevcp gladevcp -c gladevcp -U notouch=1 -U norun=0 -u lathehandler.py -x {XID} lathemacro.ui
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Greyscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
[RS274NGC]
PARAMETER_FILE = linuxcnc.var
SUBROUTINE_PATH = .:../../nc_files/subs:nc_files:../../../nc2
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G7 M9 M5 G64 P0.005
FEATURES=12
USER_M_PATH = nc_files:.
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
COMM_WAIT = 0.010
BASE_PERIOD = 100000
SERVO_PERIOD = 2000000
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
[HAL]
TWOPASS = true
HALUI = halui
HALFILE = Turner.hal
#HALFILE = custom_vfd.hal
HALFILE = custom.hal
#HALFILE = xhc-hb04.tcl
#HALFILE = monitor_xhc-hb04.tcl
POSTGUI_HALFILE = postgui_call_list.hal
SHUTDOWN = shutdown.hal
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
CYCLE_TIME = 0.010
DEFAULT_LINEAR_VELOCITY = 20.0
MAX_LINEAR_VELOCITY = 40.00
DEFAULT_ACCELERATION = 400.0
MAX_ACCELERATION = 400.0
NO_FORCE_HOMING = 1
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
TOOL_TABLE = tool.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
#This is a best-guess at the number of joints, it should be checked
JOINTS = 2
#********************
# Axis X
#********************
[AXIS_X]
MIN_LIMIT = -106
MAX_LIMIT = 106
MAX_VELOCITY = 15.0
MAX_ACCELERATION = 400.0
[JOINT_0]
TYPE = LINEAR
HOME = 0.0
FERROR = 0.5
MIN_FERROR = 1
MAX_VELOCITY = 20.0
MAX_ACCELERATION = 400.0
MAX_OUTPUT = 0.0
MIN_LIMIT = -106
MAX_LIMIT = 106
HOME_SEARCH_VEL = 0
HOME_LATCH_VEL = 0
HOME_SEQUENCE = 0
#********************
# Axis Z
#********************
[AXIS_Z]
MIN_LIMIT = -560.0
MAX_LIMIT = 560.0
MAX_VELOCITY = 50.0
MAX_ACCELERATION = 400.0
[JOINT_1]
TYPE = LINEAR
HOME = 0.0
FERROR = 0.5
MIN_FERROR = 1
MAX_VELOCITY = 75.0
MAX_ACCELERATION = 400.0
HOME_SEARCH_VEL = 0
HOME_LATCH_VEL = 0
HOME_SEQUENCE = 0
[SPINDLE_9]
P = 0.0
I = 0.0
D = 0.0
FF0 = 1.0
FF1 = 0.0
FF2 = 0.0
BIAS = 0.0
DEADBAND = 0.0
MAX_OUTPUT = 2400.0
OUTPUT_SCALE =5760
OUTPUT_MIN_LIMIT = 0
OUTPUT_MAX_LIMIT = 2400
#ENCODER_SCALE = -4000
ENCODER_SCALE = 2000

View File

@@ -0,0 +1,164 @@
# This config file was created 2016-11-26 11:13:55.093594 by the update_ini script
# The original config files may be found in the /home/debian/linuxcnc/configs/Turner/Turner.old directory
# Generated by PNCconf at Fri Sep 9 22:31:33 2016
# If you make changes to this file, they will be
# overwritten when you run PNCconf again
[EMC]
# The version string for this INI file.
VERSION = 1.1
MACHINE = Turner
DEBUG = 0
[DISPLAY]
LATHE=1
DISPLAY = gmoccapy
BACK_TOOL_LATHE = 0
CYCLE_TIME = 150
POSITION_OFFSET = RELATIVE
POSITION_FEEDBACK = ACTUAL
MAX_FEED_OVERRIDE = 2.000000
MAX_SPINDLE_OVERRIDE = 2.000000
MIN_SPINDLE_OVERRIDE = 0.500000
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 1
PROGRAM_PREFIX = /home/debian/linuxcnc/configs/Turner/nc_files
INCREMENTS = 5mm 1mm .5mm .1mm .05mm .01mm .005mm
LATHE = 1
POSITION_FEEDBACK = ACTUAL
DEFAULT_LINEAR_VELOCITY = 6.000000
MAX_LINEAR_VELOCITY = 25.000000
MIN_LINEAR_VELOCITY = 0.500000
DEFAULT_ANGULAR_VELOCITY = 12.000000
MAX_ANGULAR_VELOCITY = 1800.000000
MIN_ANGULAR_VELOCITY = 1.666667
EDITOR = gedit
#EMBED_TAB_NAME = Cycles
#EMBED_TAB_LOCATION = ntb_preview
#EMBED_TAB_LOCATION = ntb_user_tabs
#EMBED_TAB_COMMAND = halcmd loadusr -Wn gladevcp gladevcp -c gladevcp -U notouch=1 -U norun=0 -u lathehandler.py -x {XID} lathemacro.ui
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Greyscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
[RS274NGC]
PARAMETER_FILE = linuxcnc.var
SUBROUTINE_PATH = .:../../nc_files/subs:nc_files:../../../nc2
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G7 M9 M5 G64 P0.005
FEATURES=12
USER_M_PATH = nc_files:.
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
COMM_WAIT = 0.010
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
[HAL]
TWOPASS = true
HALUI = halui
HALFILE = Turner.hal
#HALFILE = custom_vfd.hal
HALFILE = custom.hal
#HALFILE = xhc-hb04.tcl
#HALFILE = monitor_xhc-hb04.tcl
POSTGUI_HALFILE = postgui_call_list.hal
SHUTDOWN = shutdown.hal
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
CYCLE_TIME = 0.010
DEFAULT_LINEAR_VELOCITY = 20.0
MAX_LINEAR_VELOCITY = 40.00
DEFAULT_ACCELERATION = 400.0
MAX_ACCELERATION = 400.0
NO_FORCE_HOMING = 1
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
TOOL_TABLE = tool.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
#This is a best-guess at the number of joints, it should be checked
JOINTS = 2
#********************
# Axis X
#********************
[AXIS_X]
MIN_LIMIT = -106
MAX_LIMIT = 106
MAX_VELOCITY = 15.0
MAX_ACCELERATION = 400.0
[JOINT_0]
TYPE = LINEAR
HOME = 0.0
FERROR = 0.5
MIN_FERROR = 1
MAX_VELOCITY = 20.0
MAX_ACCELERATION = 400.0
MAX_OUTPUT = 0.0
MIN_LIMIT = -106
MAX_LIMIT = 106
HOME_SEARCH_VEL = 0
HOME_LATCH_VEL = 0
HOME_SEQUENCE = 0
#********************
# Axis Z
#********************
[AXIS_Z]
MIN_LIMIT = -560.0
MAX_LIMIT = 560.0
MAX_VELOCITY = 50.0
MAX_ACCELERATION = 400.0
[JOINT_1]
TYPE = LINEAR
HOME = 0.0
FERROR = 0.5
MIN_FERROR = 1
MAX_VELOCITY = 75.0
MAX_ACCELERATION = 400.0
HOME_SEARCH_VEL = 0
HOME_LATCH_VEL = 0
HOME_SEQUENCE = 0
[SPINDLE_9]
P = 0.0
I = 0.0
D = 0.0
FF0 = 1.0
FF1 = 0.0
FF2 = 0.0
BIAS = 0.0
DEADBAND = 0.0
MAX_OUTPUT = 2400.0
OUTPUT_SCALE =5760
OUTPUT_MIN_LIMIT = 0
OUTPUT_MAX_LIMIT = 2400
#ENCODER_SCALE = -4000
ENCODER_SCALE = 2000

View File

@@ -0,0 +1,79 @@
[DEFAULT]
dro_digits = 3
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
enable_dro = False
scale_jog_vel = 24.0
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
turtle_jog_factor = 20
hide_turtle_jog_button = False
unlock_code = 123
toggle_readout = True
spindle_start_rpm = 300.0
kbd_height = 250
view = y
blockheight = 0.0
reload_tool = False
open_file =
screen1 = window
hide_titlebar = False
x_pos = 40
y_pos = 30
width = 979
height = 750
use_toolmeasurement = False
gtk_theme = Follow System Theme
icon_theme = classic
audio_enabled = True
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
mouse_btn_mode = 4
hide_cursor = False
hide_tooltips = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/debian
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = False
show_keyboard_on_mdi = False
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
blockdel = False
show_offsets = False
show_dtg = False
view_tool_path = False
view_dimension = True
run_from_line = no_run
unlock_way = no
show_preview_on_offset = False
use_keyboard_shortcuts = False
offset_axis_z = 150.0
diameter offset_axis_x = 0
offset_axis_x = 0.0

View File

@@ -0,0 +1,27 @@
THREAD servo-thread
MAXCHAN 4
HMULT 1
HZOOM 1
HPOS 4,132653e-01
CHAN 1
PIN lcec.0.0.D1
VSCALE 1
VPOS 0,781382
VOFF 0,000000e+00
CHAN 2
PIN lcec.0.0.DiffT
VSCALE 0
VPOS 0,678420
VOFF 0,000000e+00
CHAN 3
PIN lcec.0.0.D3
VSCALE 1
VPOS 0,743300
VOFF 2,000000e+02
CHAN 4
PIN lcec.0.0.D4
VSCALE 5
VPOS 0,364679
VOFF 0,000000e+00
TMODE 1
RMODE 2

View File

@@ -0,0 +1,64 @@
;boring
O<boring> sub
G8 ; Radius mode (easier maths)
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
G91.1 ; but not for arcs
M6 T#8 G43
#1 = [#1 / 2] ; because of radius mode
#14 = [#<_x>] (starting X)
#13 = #<_z> (starting Z)
#20 = [#6 * SIN[#7]]
#21 = [-#6 * COS[#7]]
#22 = [#6 / COS[#7]]
#23 = [#5 + #6 - #20]
#24 = [[#23 - #13] * TAN[#7]]
G96 D2500 S#2 ; Constant Surface Speed Mode
m3 ;Start Spindle
g95 F#4 ; Feed-Per-Rev Mode
O90 IF [#9 GT 0.5]
M8
O90 ENDIF
g4p1 ; Wait to reach speed
(debug, Turning finish dia #1 start dia #14 start length #13 finish length #5 coolant #9)
O100 WHILE [#14 LT [#1 - #3]]
g0 X #14
#14=[#14 + #3]
G1 X #14
G1 Z #23 X[#14 + #24]
O101 IF [#6 GT 0]
G3 Z#5 X[#14 + #24 + #21] I#21 K#20
G1 X[#14 + #24 + #21 - #3]
O101 ELSE
G1 X[#14 + #24 - [#3 * 1.5]]
O101 ENDIF
G0 Z[#13]
O100 ENDWHILE
G0 x#1
G1 Z #23 X[#1 + #24]
O102 IF [#6 GT 0]
G3 Z#5 X[#1 + #24 + #21] I#21 K#20
G1 X[#1 + #24 + #21 - #3]
O102 ELSE
G1 X[#1 + #24 - #3]
O102 ENDIF
G0 Z #13
G0 X #1 ; For touch-off
M5 M9
G7
O<boring> endsub
M2

View File

@@ -0,0 +1,77 @@
;chamfer
O<chamfer> sub
G8 ; Lathe radius Mode
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
M6 T#6 G43
#1 = [#1 / 2] ; because of radius mode
#14 = [#<_x>] (starting X)
#13 = [#<_z>] (starting Z)
G96 D1500 S#2 ; Constant Surface Speed Mode
M3
g95 F0.1 ; Feed-Per-Rev Mode
O90 IF [#12 GT 0.5]
M8
O90 ENDIF
#20 = 0
O101 if [#9 GT 0.5] ; front outside
o100 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 - #20] z#13
g1 z#5
g1 x#1 z[#5 - #20]
g1 x #14
g0 z#13
o100 endwhile
g0 x#14 z#13
g0 x[#1 - #8]
g1 z#5
g1 x#1 z[#5 - #8]
g1 x #14
g0 z#13
O101 elseif [#10 GT 0.5] ; front inside
o102 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 + #20] z#13
g1 z#5
g1 x#1 z[#5 - #20]
g1 x #14
g0 z#13
o102 endwhile
g0 x#14 z#13
g0 x[#1 + #8]
g1 z#5
g1 x#1 z[#5 - #8]
g1 x #14
g0 z#13
O101 elseif [#11 GT 0.5] ; back outside
o103 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 - #20] z#13
g1 z#5
g1 x#1 z[#5 + #20]
g1 x #14
g0 z#13
o103 endwhile
g0 x#14 z#13
g0 x[#1 - #8]
g1 z#5
g1 x#1 z[#5 + #8]
g1 x #14
g0 z#13
O101 endif
M5 M9
G7
O<chamfer> endsub
m2
%

View File

@@ -0,0 +1,31 @@
# core HAL config file for simulation
# first load all the RT modules that will be needed
# kinematics
loadrt [KINS]KINEMATICS
# motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
# create HAL signals for position commands from motion module
# loop position commands back to motion module feedback
net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb
net Zpos joint.1.motor-pos-cmd => joint.1.motor-pos-fb
# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
net spindle-fwd spindle.0.forward
net spindle-rev spindle.0.reverse
#net spindle-speed spindle.0.speed-out
net lube iocontrol.0.lube
net flood iocontrol.0.coolant-flood
net mist iocontrol.0.coolant-mist

View File

@@ -0,0 +1,59 @@
# core HAL config file for simulation
# first load all the RT modules that will be needed
# kinematics
loadrt [KINS]KINEMATICS
# motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
# load 6 differentiators (for velocity and accel signals
loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv
# load additional blocks
loadrt hypot names=vel_xy,vel_xyz
# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
# link the differentiator functions into the code
addf ddt_x servo-thread
addf ddt_xv servo-thread
addf ddt_y servo-thread
addf ddt_yv servo-thread
addf ddt_z servo-thread
addf ddt_zv servo-thread
addf vel_xy servo-thread
addf vel_xyz servo-thread
# create HAL signals for position commands from motion module
# loop position commands back to motion module feedback
net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb ddt_x.in
net Zpos joint.1.motor-pos-cmd => joint.1.motor-pos-fb ddt_z.in
net Cpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb
# send the position commands thru differentiators to
# generate velocity and accel signals
net Xvel ddt_x.out => ddt_xv.in vel_xy.in0
net Xacc <= ddt_xv.out
net Yvel ddt_y.out => ddt_yv.in vel_xy.in1
net Yacc <= ddt_yv.out
net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0
net Zacc <= ddt_zv.out
# Cartesian 2- and 3-axis velocities
net XYvel vel_xy.out => vel_xyz.in1
net XYZvel <= vel_xyz.out
# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
net spindle-fwd spindle.0.forward
net spindle-rev spindle.0.reverse
#net spindle-speed spindle.0.speed-out
net lube iocontrol.0.lube
net flood iocontrol.0.coolant-flood
net mist iocontrol.0.coolant-mist

View File

@@ -0,0 +1,59 @@
# core HAL config file for simulation
# first load all the RT modules that will be needed
# kinematics
loadrt [KINS]KINEMATICS
# motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
# load 6 differentiators (for velocity and accel signals
loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv
# load additional blocks
loadrt hypot names=vel_xy,vel_xyz
# add motion controller functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
# link the differentiator functions into the code
addf ddt_x servo-thread
addf ddt_xv servo-thread
addf ddt_y servo-thread
addf ddt_yv servo-thread
addf ddt_z servo-thread
addf ddt_zv servo-thread
addf vel_xy servo-thread
addf vel_xyz servo-thread
# create HAL signals for position commands from motion module
# loop position commands back to motion module feedback
net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb ddt_x.in
net Zpos joint.1.motor-pos-cmd => joint.1.motor-pos-fb ddt_z.in
net Cpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb
net Wpos joint.3.motor-pos-cmd => joint.3.motor-pos-fb
# send the position commands thru differentiators to
# generate velocity and accel signals
net Xvel ddt_x.out => ddt_xv.in vel_xy.in0
net Xacc <= ddt_xv.out
net Yvel ddt_y.out => ddt_yv.in vel_xy.in1
net Yacc <= ddt_yv.out
net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0
net Zacc <= ddt_zv.out
# Cartesian 2- and 3-axis velocities
net XYvel vel_xy.out => vel_xyz.in1
net XYZvel <= vel_xyz.out
# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# create signals for tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
net spindle-fwd spindle.0.forward
net spindle-rev spindle.0.reverse
#net spindle-speed spindle.0.speed-out
net lube iocontrol.0.lube
net flood iocontrol.0.coolant-flood
net mist iocontrol.0.coolant-mist

View File

@@ -0,0 +1,4 @@
# Gamepad
loadusr -W hal_input -KRAL F310
# Resten är i custom_postgui.hal

View File

@@ -0,0 +1,51 @@
# Include your custom HAL commands here
# Load the Huanyang VFD user component
loadusr -Wn spindle-vfd hy_vfd -n spindle-vfd -d /dev/hy_vfd -r 19200 -p none -s 1 -b 8
# motion.spindle-speed-in needs rev per second
loadrt scale names=scale.0,scale.1,scale.2
# scale.0 gearbox via M101-M106, M101 being lowest gear.
# scale.1 Inverted gear ratio
# scale.2 motion.spindle-speed.in in rev/sec
loadrt limit2 names=spindlemax
loadrt mult2 names=mult2.1
addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf mult2.1 servo-thread
addf spindlemax servo-thread
setp scale.0.gain 0.8846 # Start with fourth gear
setp scale.1.gain 1.13045
setp scale.2.gain 0.0166666667
setp mult2.1.in1 25.0
unlinkp spindle.0.forward
unlinkp spindle.0.reverse
unlinkp spindle.0.on
#unlinkp motion.spindle-at-speed
net spindle-fwd spindle.0.forward => spindle-vfd.spindle-forward
net spindle-reverse spindle.0.reverse => spindle-vfd.spindle-reverse
net spindle-on spindle.0.on => spindle-vfd.spindle-on
net spindle-at-speed <= spindle-vfd.spindle-at-speed
#net spindle-enable => spindle-vfd.enable
setp spindle-vfd.enable 1
# Prepare max limit rpm for spindle
net spindle-max-freq spindle-vfd.max-freq => mult2.1.in0
net spindle-max-rpm mult2.1.out => spindlemax.max # Max limit for motor rpm, like 67*25 = 1675
# Give speed to vfd, limited to max possible
unlinkp spindle.0.speed-out-abs
net spindle-speed-scale spindle.0.speed-out-abs => scale.1.in # Spindle rpm => Motor rpm
net spindle-speed-cmd-unlimited scale.1.out => spindlemax.in # Limit spindle rpm
net spindle-speed-cmd-limited spindlemax.out => spindle-vfd.speed-command # Feed to vfd
# Convert actual feed from rpm to rps, consider gear in use.
unlinkp spindle.0.speed-in
net spindle-speed-scale2 spindle-vfd.spindle-speed-fb => scale.2.in # Motor rpm => Motor rps
net spindle-speed-gears scale.2.out => scale.0.in # Motor rps => spindle rps
net spindle-speed-in scale.0.out => spindle.0.speed-in
# Gamepad
loadusr -W hal_input -KRAL F310
# Resten är i custom_postgui.hal

View File

@@ -0,0 +1,74 @@
# Include your custom_postgui HAL commands here
# This file will not be overwritten when you run PNCconf again
loadrt or2 count=2
loadrt mux4 count=1
loadrt and2 count=4
loadrt oneshot count=1
addf or2.0 servo-thread
addf or2.1 servo-thread
addf mux4.0 servo-thread
addf and2.0 servo-thread
addf and2.1 servo-thread
addf and2.2 servo-thread
addf and2.3 servo-thread
addf oneshot.0 servo-thread
setp mux4.0.in0 0
setp mux4.0.in1 25
setp mux4.0.in2 50
setp mux4.0.in3 1000
net remote-speed-slow or2.0.in0 <= input.0.btn-x
net remote-speed-medium or2.1.in0 <= input.0.btn-a
net remote-speed-fast or2.0.in1 <= input.0.btn-b
net remote-speed-fast or2.1.in1 <= input.0.btn-b
net joy-speed-1 mux4.0.sel0 <= or2.0.out
net joy-speed-2 mux4.0.sel1 <= or2.1.out
net jog-speed <= mux4.0.out
net jog-x-analog <= input.0.abs-y-position
net jog-z-analog <= input.0.abs-x-position
# Spindle start+manual and stop
net manual-mode-spindle-lock and2.3.in0 halui.mode.manual <= input.0.btn-mode
net spindle-safe-start and2.3.in1 <= input.0.btn-tr
net spindle-manual-cw <= and2.3.out
net spindle-manual-stop <= input.0.btn-tl
# E-stop
net estop halui.estop.activate <= input.0.btn-y
# net manual-mode halui.mode.manual <= input.0.btn-mode
# Start/pause/resume stop
setp oneshot.0.width 0.0011
setp oneshot.0.retriggerable 0
setp oneshot.0.rising 1
setp oneshot.0.falling 0
net program-is-idle halui.program.is-idle => and2.0.in0
net program-is-running halui.program.is-running => and2.1.in0
net program-is-paused halui.program.is-paused => and2.2.in0
net button-edge-trig input.0.btn-start => oneshot.0.in
net button-start oneshot.0.out => and2.0.in1 and2.1.in1 and2.2.in1
net program-start and2.0.out => halui.program.run halui.mode.auto
net program-pause and2.1.out => halui.program.pause
net program-resume and2.2.out => halui.program.resume
net program-stop input.0.btn-select => halui.program.stop
net spindle-at-speed gmoccapy.spindle_at_speed_led
net spindle-vel-fb-rpm => gmoccapy.spindle_feedback_bar
net tool-change gmoccapy.toolchange-change <= iocontrol.0.tool-change
net tool-changed gmoccapy.toolchange-changed <= iocontrol.0.tool-changed
net tool-prep-number gmoccapy.toolchange-number <= iocontrol.0.tool-prep-number
net tool-prep-loop iocontrol.0.tool-prepare <= iocontrol.0.tool-prepared
net tooloffset-x gmoccapy.tooloffset-x <= motion.tooloffset.x
net tooloffset-z gmoccapy.tooloffset-z <= motion.tooloffset.z

View File

@@ -0,0 +1,48 @@
# Include your custom HAL commands here
# Load the Huanyang VFD user component
loadusr -Wn spindle-vfd hy_vfd -n spindle-vfd -d /dev/hy_vfd -r 19200 -p none -s 1 -b 8
# motion.spindle-speed-in needs rev per second
loadrt scale names=scale.0,scale.1,scale.2
# scale.0 gearbox via M101-M106, M101 being lowest gear.
# scale.1 Inverted gear ratio
# scale.2 motion.spindle-speed.in in rev/sec
loadrt limit2 names=spindlemax
loadrt mult2 names=mult2.1
addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf mult2.1 servo-thread
addf spindlemax servo-thread
setp scale.0.gain 0.8846 # Start with fourth gear
setp scale.1.gain 1.13045
setp scale.2.gain 0.0166666667
setp mult2.1.in1 25.0
unlinkp spindle.0.forward
unlinkp spindle.0.reverse
unlinkp spindle.0.on
#unlinkp motion.spindle-at-speed
net spindle-fwd spindle.0.forward => spindle-vfd.spindle-forward
net spindle-reverse spindle.0.reverse => spindle-vfd.spindle-reverse
net spindle-on spindle.0.on => spindle-vfd.spindle-on
net spindle-at-speed <= spindle-vfd.spindle-at-speed
#net spindle-enable => spindle-vfd.enable
setp spindle-vfd.enable 1
# Prepare max limit rpm for spindle
net spindle-max-freq spindle-vfd.max-freq => mult2.1.in0
net spindle-max-rpm mult2.1.out => spindlemax.max # Max limit for motor rpm, like 67*25 = 1675
# Give speed to vfd, limited to max possible
unlinkp spindle.0.speed-out-abs
net spindle-speed-scale spindle.0.speed-out-abs => scale.1.in # Spindle rpm => Motor rpm
net spindle-speed-cmd-unlimited scale.1.out => spindlemax.in # Limit spindle rpm
net spindle-speed-cmd-limited spindlemax.out => spindle-vfd.speed-command # Feed to vfd
# Convert actual feed from rpm to rps, consider gear in use.
unlinkp spindle.0.speed-in
net spindle-speed-scale2 spindle-vfd.spindle-speed-fb => scale.2.in # Motor rpm => Motor rps
net spindle-speed-gears scale.2.out => scale.0.in # Motor rps => spindle rps
net spindle-speed-in scale.0.out => spindle.0.speed-in

View File

@@ -0,0 +1,51 @@
# Include your custom HAL commands here
# Load the Huanyang VFD user component
loadusr -Wn spindle-vfd hy_vfd -n spindle-vfd -d /dev/hy_vfd -r 19200 -p none -s 1 -b 8
# motion.spindle-speed-in needs rev per second
loadrt scale names=scale.0,scale.1,scale.2
# scale.0 gearbox via M101-M106, M101 being lowest gear.
# scale.1 Inverted gear ratio
# scale.2 motion.spindle-speed.in in rev/sec
loadrt limit2 names=spindlemax
loadrt mult2 names=mult2.1
addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf mult2.1 servo-thread
addf spindlemax servo-thread
setp scale.0.gain 0.8846 # Start with fourth gear
setp scale.1.gain 1.13045
setp scale.2.gain 0.0166666667
setp mult2.1.in1 25.0
unlinkp spindle.0.forward
unlinkp spindle.0.reverse
unlinkp spindle.0.on
#unlinkp motion.spindle-at-speed
net spindle-fwd spindle.0.forward => spindle-vfd.spindle-forward
net spindle-reverse spindle.0.reverse => spindle-vfd.spindle-reverse
net spindle-on spindle.0.on => spindle-vfd.spindle-on
net spindle-at-speed <= spindle-vfd.spindle-at-speed
#net spindle-enable => spindle-vfd.enable
setp spindle-vfd.enable 1
# Prepare max limit rpm for spindle
net spindle-max-freq spindle-vfd.max-freq => mult2.1.in0
net spindle-max-rpm mult2.1.out => spindlemax.max # Max limit for motor rpm, like 67*25 = 1675
# Give speed to vfd, limited to max possible
unlinkp spindle.0.speed-out-abs
net spindle-speed-scale spindle.0.speed-out-abs => scale.1.in # Spindle rpm => Motor rpm
net spindle-speed-cmd-unlimited scale.1.out => spindlemax.in # Limit spindle rpm
net spindle-speed-cmd-limited spindlemax.out => spindle-vfd.speed-command # Feed to vfd
# Convert actual feed from rpm to rps, consider gear in use.
unlinkp spindle.0.speed-in
net spindle-speed-scale2 spindle-vfd.spindle-speed-fb => scale.2.in # Motor rpm => Motor rps
net spindle-speed-gears scale.2.out => scale.0.in # Motor rps => spindle rps
net spindle-speed-in scale.0.out => spindle.0.speed-in
# Gamepad
loadusr -W hal_input -KRAL F310
# Resten är i custom_postgui.hal

View File

@@ -0,0 +1,49 @@
;drilling
; Use T = 0 for tailstock drilling, then this just sets speed
O<drilling> SUB
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
G91.1 ; but not for arcs
#10 = [#2 * 1000 / [ #1 * 3.141592]]
(debug, #10)
O90 IF [#7 GT 0.5]
M8
O90 ENDIF
O100 IF [#6 LT 0.5]
G97 M3 S#10
O<drilling> RETURN
O100 ENDIF
M6 T#6 G43
G0 X0
O83 IF [#5 GT 0] ; Pecking
G17
G95 F#3 ; Feed-Per-Rev Mode
G97 M3 S#10
G98
G83 R#<_z> Q#5 Z#4
M5 M9 G18
O<drilling> RETURN
O83 ENDIF
O82 IF [1]; not pecking
G17
G95 F#3 ; Feed-Per-Rev Mode
G97 M3 S#10
G98
G82 R#<_z> P0.5 Z#4
M5 M9 G18
O82 ENDIF
O<drilling> ENDSUB
O<drilling> call [10] [100] [0.03] [0] [2] [0] [1]
M2

View File

@@ -0,0 +1,59 @@
<masters>
<master idx="0" appTimePeriod="2000000" refClockSyncCycles="1">
<slave idx="0" type="generic" vid="00aaa" pid="000bbbccc" configPdos="true">
<dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="500000"/>
<syncManager idx="2" dir="out">
<pdo idx="1600">
<pdoEntry idx="7000" subIdx="00" bitLen="32" halPin="enc-index-latch-enable" halType="u32"/>
</pdo>
<pdo idx="1601">
<pdoEntry idx="7001" subIdx="00" bitLen="32" halPin="commanded-position-1" halType="float-ieee"/>
</pdo>
<pdo idx="1602">
<pdoEntry idx="7002" subIdx="00" bitLen="32" halPin="commanded-position-2" halType="float-ieee"/>
</pdo>
<pdo idx="1603">
<pdoEntry idx="7003" subIdx="00" bitLen="16" halPin="steps-per-mm-1" halType="s32"/>
</pdo>
<pdo idx="1604">
<pdoEntry idx="7004" subIdx="00" bitLen="16" halPin="steps-per-mm-2" halType="s32"/>
</pdo>
</syncManager>
<syncManager idx="3" dir="in">
<pdo idx="1a00">
<pdoEntry idx="6000" subIdx="00" bitLen="32" halPin="enc-position" halType="float-ieee"/>
</pdo>
<pdo idx="1a01">
<pdoEntry idx="6001" subIdx="00" bitLen="32" halPin="enc-frequency" halType="float-ieee"/>
</pdo>
<pdo idx="1a02">
<pdoEntry idx="6002" subIdx="00" bitLen="16" halPin="DiffT" halType="u32"/>
</pdo>
<pdo idx="1a03">
<pdoEntry idx="6003" subIdx="00" bitLen="32" halPin="index-byte" halType="u32"/>
</pdo>
<pdo idx="1a04">
<pdoEntry idx="6004" subIdx="00" bitLen="32" halPin="index-status" halType="u32"/>
</pdo>
<pdo idx="1a05">
<pdoEntry idx="6005" subIdx="00" bitLen="32" halPin="actual-position-1" halType="float-ieee"/>
</pdo>
<pdo idx="1a06">
<pdoEntry idx="6006" subIdx="00" bitLen="32" halPin="actual-position-2" halType="float-ieee"/>
</pdo>
<pdo idx="1a07">
<pdoEntry idx="6007" subIdx="00" bitLen="16" halPin="D1" halType="s32"/>
</pdo>
<pdo idx="1a08">
<pdoEntry idx="6008" subIdx="00" bitLen="16" halPin="D2" halType="s32"/>
</pdo>
<pdo idx="1a09">
<pdoEntry idx="6009" subIdx="00" bitLen="16" halPin="D3" halType="s32"/>
</pdo>
<pdo idx="1a0a">
<pdoEntry idx="600a" subIdx="00" bitLen="16" halPin="D4" halType="s32"/>
</pdo>
</syncManager>
</slave>
</master>
</masters>

View File

@@ -0,0 +1,82 @@
<masters>
<master idx="0" appTimePeriod="1000000" refClockSyncCycles="5">
<slave idx="0" type="EK1100"/>
<slave idx="1" type="EL1008"/>
<slave idx="2" type="EL2008"/>
<slave idx="3" type="EL5101"/>
<slave idx="4" type="generic" vid="00000a88" pid="0a880002" configPdos="true">
<dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="0"/>
<sdoConfig idx="2000" subIdx="0"><sdoDataRaw data ="b8 0b"/></sdoConfig> <!-- Max motor current (3.0A) -->
<sdoConfig idx="2003" subIdx="0"><sdoDataRaw data ="64 00"/></sdoConfig> <!-- Standby current percentage (100%) -->
<sdoConfig idx="2011" subIdx="0"><sdoDataRaw data ="00 00"/></sdoConfig> <!-- Open loop -->
<syncManager idx="2" dir="out">
<pdo idx="1600">
<pdoEntry idx="6040" subIdx="00" bitLen="16" halPin="cia-controlword" halType="u32"/>
<pdoEntry idx="6060" subIdx="00" bitLen="8" halPin="opmode" halType="s32"/>
<pdoEntry idx="607A" subIdx="00" bitLen="32" halPin="target-position" halType="s32"/>
<pdoEntry idx="60FF" subIdx="00" bitLen="32" halPin="target-velocity" halType="s32"/>
</pdo>
</syncManager>
<syncManager idx="3" dir="in">
<pdo idx="1a00">
<pdoEntry idx="6041" subIdx="00" bitLen="16" halPin="cia-statusword" halType="u32"/>
<pdoEntry idx="6061" subIdx="00" bitLen="8" halPin="opmode-display" halType="s32"/>
<pdoEntry idx="6064" subIdx="00" bitLen="32" halPin="actual-position" halType="s32"/>
<pdoEntry idx="606C" subIdx="00" bitLen="32" halPin="actual-velocity" halType="s32"/>
<pdoEntry idx="6077" subIdx="00" bitLen="32" halPin="actual-torque" halType="s32"/>
</pdo>
</syncManager>
</slave>
<slave idx="5" type="generic" vid="00000a88" pid="0a880002" configPdos="true">
<dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="0"/>
<sdoConfig idx="2000" subIdx="0"><sdoDataRaw data ="68 10"/></sdoConfig> <!-- Max motor current (4.2A) -->
<sdoConfig idx="2003" subIdx="0"><sdoDataRaw data ="64 00"/></sdoConfig> <!-- Standby current percentage (100%) -->
<sdoConfig idx="2011" subIdx="0"><sdoDataRaw data ="00 00"/></sdoConfig> <!-- Open loop -->
<syncManager idx="2" dir="out">
<pdo idx="1600">
<pdoEntry idx="6040" subIdx="00" bitLen="16" halPin="cia-controlword" halType="u32"/>
<pdoEntry idx="6060" subIdx="00" bitLen="8" halPin="opmode" halType="s32"/>
<pdoEntry idx="607A" subIdx="00" bitLen="32" halPin="target-position" halType="s32"/>
<pdoEntry idx="60FF" subIdx="00" bitLen="32" halPin="target-velocity" halType="s32"/>
</pdo>
</syncManager>
<syncManager idx="3" dir="in">
<pdo idx="1a00">
<pdoEntry idx="6041" subIdx="00" bitLen="16" halPin="cia-statusword" halType="u32"/>
<pdoEntry idx="6061" subIdx="00" bitLen="8" halPin="opmode-display" halType="s32"/>
<pdoEntry idx="6064" subIdx="00" bitLen="32" halPin="actual-position" halType="s32"/>
<pdoEntry idx="606C" subIdx="00" bitLen="32" halPin="actual-velocity" halType="s32"/>
<pdoEntry idx="6077" subIdx="00" bitLen="32" halPin="actual-torque" halType="s32"/>
</pdo>
</syncManager>
</slave>
<slave idx="6" type="generic" vid="00aaa" pid="000bbbccc" configPdos="true">
<dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="0"/>
<syncManager idx="2" dir="out">
<pdo idx="1600">
<pdoEntry idx="7000" subIdx="00" bitLen="32" halPin="enc-pos-scale" halType="s32"/>
</pdo>
<pdo idx="1601">
<pdoEntry idx="7001" subIdx="00" bitLen="32" halPin="enc-index-latch-enable" halType="u32"/>
</pdo>
</syncManager>
<syncManager idx="3" dir="in">
<pdo idx="1a00">
<pdoEntry idx="6000" subIdx="00" bitLen="32" halPin="enc-position" halType="float-ieee"/>
</pdo>
<pdo idx="1a01">
<pdoEntry idx="6001" subIdx="00" bitLen="32" halPin="enc-frequency" halType="float-ieee"/>
</pdo>
<pdo idx="1a02">
<pdoEntry idx="6002" subIdx="00" bitLen="32" halPin="DiffT" halType="u32"/>
</pdo>
<pdo idx="1a03">
<pdoEntry idx="6003" subIdx="00" bitLen="32" halPin="index-byte" halType="u32"/>
</pdo>
<pdo idx="1a04">
<pdoEntry idx="6004" subIdx="00" bitLen="32" halPin="index-status" halType="u32"/>
</pdo>
</syncManager>
</slave>
</master>
</masters>

View File

@@ -0,0 +1,59 @@
<masters>
<master idx="0" appTimePeriod="2000000" refClockSyncCycles="1">
<slave idx="0" type="generic" vid="00aaa" pid="000bbbccc" configPdos="true">
<dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="0"/>
<syncManager idx="2" dir="out">
<pdo idx="1600">
<pdoEntry idx="7000" subIdx="00" bitLen="32" halPin="enc-index-latch-enable" halType="u32"/>
</pdo>
<pdo idx="1601">
<pdoEntry idx="7001" subIdx="00" bitLen="32" halPin="commanded-position-1" halType="float-ieee"/>
</pdo>
<pdo idx="1602">
<pdoEntry idx="7002" subIdx="00" bitLen="32" halPin="commanded-position-2" halType="float-ieee"/>
</pdo>
<pdo idx="1603">
<pdoEntry idx="7003" subIdx="00" bitLen="16" halPin="steps-per-mm-1" halType="s32"/>
</pdo>
<pdo idx="1604">
<pdoEntry idx="7004" subIdx="00" bitLen="16" halPin="steps-per-mm-2" halType="s32"/>
</pdo>
</syncManager>
<syncManager idx="3" dir="in">
<pdo idx="1a00">
<pdoEntry idx="6000" subIdx="00" bitLen="32" halPin="enc-position" halType="float-ieee"/>
</pdo>
<pdo idx="1a01">
<pdoEntry idx="6001" subIdx="00" bitLen="32" halPin="enc-frequency" halType="float-ieee"/>
</pdo>
<pdo idx="1a02">
<pdoEntry idx="6002" subIdx="00" bitLen="16" halPin="DiffT" halType="u32"/>
</pdo>
<pdo idx="1a03">
<pdoEntry idx="6003" subIdx="00" bitLen="32" halPin="index-byte" halType="u32"/>
</pdo>
<pdo idx="1a04">
<pdoEntry idx="6004" subIdx="00" bitLen="32" halPin="index-status" halType="u32"/>
</pdo>
<pdo idx="1a05">
<pdoEntry idx="6005" subIdx="00" bitLen="32" halPin="actual-position-1" halType="float-ieee"/>
</pdo>
<pdo idx="1a06">
<pdoEntry idx="6006" subIdx="00" bitLen="32" halPin="actual-position-2" halType="float-ieee"/>
</pdo>
<pdo idx="1a07">
<pdoEntry idx="6007" subIdx="00" bitLen="16" halPin="D1" halType="s32"/>
</pdo>
<pdo idx="1a08">
<pdoEntry idx="6008" subIdx="00" bitLen="16" halPin="D2" halType="s32"/>
</pdo>
<pdo idx="1a09">
<pdoEntry idx="6009" subIdx="00" bitLen="16" halPin="D3" halType="s32"/>
</pdo>
<pdo idx="1a0a">
<pdoEntry idx="600a" subIdx="00" bitLen="16" halPin="D4" halType="s32"/>
</pdo>
</syncManager>
</slave>
</master>
</masters>

View File

@@ -0,0 +1,45 @@
;Facing
O<facing> sub
G7 ; Lathe Diameter Mode
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
M6 T#7 G43
O90 IF [#8 GT 0.5]
M8
O90 ENDIF
O10 IF [#6 NE 0]
(MSG, Angled facing isn't supported yet)
O10 ENDIF
#14 = [#<_x> * 2] (starting X)
#13 = #<_z> (starting Z)
G96 D2500 S#2 ; Constant Surface Speed Mode
M3
g95 F#4 ; Feed-Per-Rev Mode
g4p1 ; Wait to reach speed
O200 WHILE [#13 GT #5 + #3]
#13=[#13-#3]
G1 Z#13
G1 X#1
G0 Z[#13+#3]
G0 X#14
G0 Z#13
O200 ENDWHILE
G1 Z#5
G1 X#1
G0 Z[#13+#3]
G0 X[#14+#3]
G0 Z#5 ; For touch-off
M5 M9
O<facing> endsub
M2

View File

@@ -0,0 +1,83 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
view = y
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 140.4
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/emcmesa
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = False
show_keyboard_on_mdi = False
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
opstop = False
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = no
show_preview_on_offset = False
use_keyboard_shortcuts = True
dro_digits = 3
toggle_readout = True
tool_in_spindle = 3
diameter offset_axis_x = 0
offset_axis_x = 0.0
offset_axis_z = 0.0
offset_axis_r = 50.0
radius offset_axis_x = 0
use_toolmeasurement = False
kbd_height = 250
icon_theme = None
hide_tooltips = False
hide_titlebar = False
audio_enabled = True

View File

@@ -0,0 +1,72 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
view = y
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 140.4
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/emcmesa
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = True
show_keyboard_on_mdi = True
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
opstop = False
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = use
show_preview_on_offset = False
use_keyboard_shortcuts = False
dro_digits = 3
toggle_readout = True
tool_in_spindle = 0

View File

@@ -0,0 +1,23 @@
###################################################################
# moccapy_postgui.hal file from Norbert Schechner #
###################################################################
loadrt abs names=abs_spindle_feedback
addf abs_spindle_feedback servo-thread
net spindle-speed-limited => abs_spindle_feedback.in
net spindle-abs abs_spindle_feedback.out => gmoccapy.spindle_feedback_bar
net spindle-at-speed gmoccapy.spindle_at_speed_led
# the unlink pin commands are only used, because they are connected
# in core_sim.hal and we use this file to simulate
unlinkp iocontrol.0.tool-change
unlinkp iocontrol.0.tool-changed
unlinkp iocontrol.0.tool-prep-number
net tool-change gmoccapy.toolchange-change <= iocontrol.0.tool-change
net tool-changed gmoccapy.toolchange-changed <= iocontrol.0.tool-changed
net tool-prep-number gmoccapy.toolchange-number <= iocontrol.0.tool-prep-number
net tooloffset-x gmoccapy.tooloffset-x <= motion.tooloffset.x
net tooloffset-z gmoccapy.tooloffset-z <= motion.tooloffset.z

View File

@@ -0,0 +1,32 @@
;grooving
O<grooving> sub
G8 ; Radius mode (easier maths)
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
G91.1 ; but not for arcs
M6 T#4 G43
#1 = [#1 / 2] ; because of radius mode
#14 = [#<_x>] (starting X)
G96 D1500 S#2 ; Constant Surface Speed Mode
m3 ;Start Spindle
g95 F#3 ; Feed-Per-Rev Mode
O90 IF [#5 GT 0.5]
M8
O90 ENDIF
g4p1 ; Wait to reach speed
G1 F#3 X#1
G0 X#14
M5 M9
G7
O<grooving> endsub
M2

View File

@@ -0,0 +1,22 @@
# Halshow settings
# This file is generated automatically.
wm geometry . 700x475+1121+46
placeFrames 0.3
set ::ratio 0.3
set ::old_w_leftf 160
set ::watchlist {
pin+lcec.0.0.DiffT
pin+lcec.0.0.D1
pin+lcec.0.0.D2
pin+lcec.0.0.D3
pin+lcec.0.0.D4
pin+lcec.0.0.commanded-position-2
pin+lcec.0.0.actual-position-2
}
set ::workmode watchhal
set ::watchInterval 100
set ::col1_width 100
set ::ffmts
set ::ifmts
set ::alwaysOnTop 0
set ::autoSaveWatchlist 1

View File

@@ -0,0 +1,159 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = gmoccapy_lathe
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
JOINTS = 2
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 100
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = 250
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

View File

@@ -0,0 +1,74 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
gremlin_view = rbt_view_y2
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 140.4
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
use_screen2 = False
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
view = p
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/emcmesa
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = True
show_keyboard_on_mdi = True
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
opstop = False
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = use
show_preview_on_offset = False
use_keyboard_shortcuts = False
dro_digits = 3
toggle_readout = True
tool_in_spindle = 0

View File

@@ -0,0 +1,5 @@
T1 P1 X-12.0 Y0.0 Z-25.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.4 I26.0 J86.0 Q2.0 ;60 Grad vorn
T2 P2 X-10.356 Y0.0 Z12.123 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I50.0 J85.0 Q2.0 ;35 Grad vorn
T3 P3 X1.123 Y0.0 Z25.456 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.4 I-80.0 J-25.0 Q3.0 ;55 Grad vorn
T4 P4 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I94.0 J154.0 Q1.0 ;60 Grad Rück
T5 P5 X5.737 Y0.0 Z12.2399 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I0.0 J0.0 Q6.0 ;Einstichstahl

View File

@@ -0,0 +1,182 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = lathe_c
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe_C.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z C
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZC
JOINTS = 3
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
[AXIS_C]
MAX_VELOCITY = 90.0
MAX_ACCELERATION = 1200.0
[JOINT_2]
TYPE = ANGULAR
HOME = 0.0
MAX_VELOCITY = 90.0
MAX_ACCELERATION = 1200.0
BACKLASH = 0.000
INPUT_SCALE = 40
OUTPUT_SCALE = 1.000
FERROR = 5.0
MIN_FERROR = 1.0
HOME_OFFSET = 0.0
HOME_SEARCH_VEL = 0.0
HOME_LATCH_VEL = 0.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

View File

@@ -0,0 +1,210 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = lathe_cw
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe_CW.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z C W
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZCW
JOINTS = 4
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
[AXIS_C]
MAX_VELOCITY = 90.0
MAX_ACCELERATION = 1200.0
[JOINT_2]
TYPE = ANGULAR
HOME = 0.0
MAX_VELOCITY = 90.0
MAX_ACCELERATION = 1200.0
BACKLASH = 0.000
INPUT_SCALE = 40
OUTPUT_SCALE = 1.000
FERROR = 5.0
MIN_FERROR = 1.0
HOME_OFFSET = 0.0
HOME_SEARCH_VEL = 0.0
HOME_LATCH_VEL = 0.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
[AXIS_W]
MIN_LIMIT = -200.0
MAX_LIMIT = 0.0
MAX_VELOCITY = 66
MAX_ACCELERATION = 150.0
[JOINT_3]
TYPE = LINEAR
MAX_VELOCITY = 66
MAX_ACCELERATION = 150.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -200.0
MAX_LIMIT = 0.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 0.0
HOME_SEARCH_VEL = 0.0
HOME_LATCH_VEL = 0.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

View File

@@ -0,0 +1,160 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = gmoccapy_lathe
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 1
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
JOINTS = 2
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

View File

@@ -0,0 +1,73 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
reload_tool = True
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
mouse_btn_mode = 4
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = True
show_keyboard_on_mdi = False
show_keyboard_on_file_selection = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rotation of Z
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
unlock_way = no
grid_size = 1.0
view = y2
gremlin_view = rbt_view_y
tool_in_spindle = 0
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 140.4
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
hide_cursor = False
jump_to_dir = /home/emcmesa
blockdel = False
opstop = False
run_from_line = run
show_preview_on_offset = True
use_keyboard_shortcuts = True
dro_digits = 3
toggle_readout = True

View File

@@ -0,0 +1,72 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
view = y2
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 140.4
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/emcmesa
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = False
show_keyboard_on_mdi = False
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
opstop = False
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = no
show_preview_on_offset = False
use_keyboard_shortcuts = True
dro_digits = 3
toggle_readout = True
tool_in_spindle = 2

View File

@@ -0,0 +1,160 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = lathe_imperial
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 0.4 inch, 0.04 inch , 0.004 inch, 0.0004 inch
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G20 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.001
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = inch
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 1.6
MAX_LINEAR_VELOCITY = 10
POSITION_FILE = position.txt
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
JOINTS = 2
[AXIS_X]
MIN_LIMIT = -2.0
MAX_LIMIT = 40.0
MAX_VELOCITY = 6.5
MAX_ACCELERATION = 59.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 6.5
MAX_ACCELERATION = 59.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -2.0
MAX_LIMIT = 40.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 4
HOME_SEARCH_VEL = 40.0
HOME_LATCH_VEL = 2.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -3.0
MAX_LIMIT = 400.0
MAX_VELOCITY = 6.5
MAX_ACCELERATION = 59.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 6.5
MAX_ACCELERATION = 59.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -3.0
MAX_LIMIT = 400.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -4
HOME_SEARCH_VEL = 40.0
HOME_LATCH_VEL = 2.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos
# Sections for display options ------------------------------------------------

View File

@@ -0,0 +1,71 @@
[DEFAULT]
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
unlock_code = 123
blockheight = 0.0
spindle_start_rpm = 300.0
scale_jog_vel = 6.0
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
hide_turtle_jog_button = False
turtle_jog_factor = 20
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
gtk_theme = Follow System Theme
audio_alert = /usr/share/sounds/freedesktop/stereo/dialog-warning.oga
audio_error = /usr/share/sounds/freedesktop/stereo/dialog-error.oga
grid_size = 1.0
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/emcmesa
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = True
show_keyboard_on_mdi = True
show_keyboard_on_file_selection = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
opstop = False
enable_dro = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = use
show_preview_on_offset = False
use_keyboard_shortcuts = False
dro_digits = 4
toggle_readout = True
view = y

View File

@@ -0,0 +1,165 @@
# EMC controller parameters for a simulated machine.
# General note: Comments can either be preceded with a # or ; - either is
# acceptable, although # is in keeping with most linux config files.
# General section -------------------------------------------------------------
[EMC]
VERSION = 1.1
MACHINE = gmoccapy_lathe
#DEBUG = 0x7FFFFFFF
DEBUG = 0
# for details see nc_files/subroutines/maco_instructions.txt
[DISPLAY]
DISPLAY = gmoccapy
LATHE = 1
BACK_TOOL_LATHE = 0
# Cycle time, in milliseconds, that display will sleep between polls
CYCLE_TIME = 100
# Highest value that will be allowed for feed override, 1.0 = 100%
MAX_FEED_OVERRIDE = 1.5
MAX_SPINDLE_OVERRIDE = 1.2
MIN_SPINDLE_OVERRIDE = .5
# Prefix to be used
PROGRAM_PREFIX = /home/debian/linuxcnc/nc_files
# Introductory graphic
INTRO_GRAPHIC = linuxcnc.gif
INTRO_TIME = 5
# list of selectable jog increments
INCREMENTS = 1.000 mm, 0.100 mm, 0.010 mm, 0.001 mm
EMBED_TAB_NAME = Cycles
EMBED_TAB_LOCATION = ntb_preview
#EMBED_TAB_LOCATION = ntb_user_tabs
EMBED_TAB_COMMAND = halcmd loadusr -Wn gladevcp gladevcp -c gladevcp -U notouch=1 -U norun=0 -u lathehandler.py -x {XID} lathemacro.ui
[FILTER]
PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image
PROGRAM_EXTENSION = .py Python Script
png = image-to-gcode
gif = image-to-gcode
jpg = image-to-gcode
py = python
# Task controller section -----------------------------------------------------
[RS274NGC]
RS274NGC_STARTUP_CODE = G18 G21 G40 G49 G54 G80 G90 G94 G8 M9 M5 G64 P0.005
PARAMETER_FILE = sim.var
SUBROUTINE_PATH = macros:./
# Motion control section ------------------------------------------------------
[EMCMOT]
EMCMOT = motmod
COMM_TIMEOUT = 1.0
BASE_PERIOD = 100000
SERVO_PERIOD = 1000000
# Hardware Abstraction Layer section --------------------------------------------------
[TASK]
TASK = milltask
CYCLE_TIME = 0.001
# Part program interpreter section --------------------------------------------
[HAL]
HALFILE = core_sim_lathe.hal
HALFILE = spindle_sim.hal
HALFILE = simulated_home_lathe.hal
# Single file that is executed after the GUI has started.
#POSTGUI_HALFILE = gmoccapy_postgui.hal
HALUI = halui
# Trajectory planner section --------------------------------------------------
[HALUI]
#No Content
[TRAJ]
COORDINATES = X Z
LINEAR_UNITS = mm
ANGULAR_UNITS = degree
DEFAULT_LINEAR_VELOCITY = 50
MAX_LINEAR_VELOCITY = 234
POSITION_FILE = position.txt
#NO_FORCE_HOMING = 1
# First axis
[EMCIO]
EMCIO = io
CYCLE_TIME = 0.100
# tool table file
TOOL_TABLE = lathe.tbl
[KINS]
KINEMATICS = trivkins coordinates=XZ
JOINTS = 2
[AXIS_X]
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_0]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -50.0
MAX_LIMIT = 200.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 0.0
HOME = 10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 1
HOME_IS_SHARED = 1
# Third axis
[AXIS_Z]
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
[JOINT_1]
TYPE = LINEAR
MAX_VELOCITY = 166
MAX_ACCELERATION = 1500.0
BACKLASH = 0.000
INPUT_SCALE = 4000
OUTPUT_SCALE = 1.000
MIN_LIMIT = -75.0
MAX_LIMIT = 1000.0
FERROR = 0.050
MIN_FERROR = 0.010
HOME_OFFSET = 1.0
HOME = -10
HOME_SEARCH_VEL = 200.0
HOME_LATCH_VEL = 20.0
HOME_USE_INDEX = NO
HOME_IGNORE_LIMITS = NO
HOME_SEQUENCE = 0
HOME_IS_SHARED = 1
# section for main IO controller parameters -----------------------------------
[MACROS]
MACRO = i_am_lost
MACRO = halo_world
MACRO = jog_around
MACRO = increment xinc yinc
MACRO = go_to_position X-pos Y-pos Z-pos

View File

@@ -0,0 +1,217 @@
#!/usr/bin/env python3
# vim: sts=4 sw=4 et
# This is a component of EMC
# savestate.py copyright 2013 Andy Pugh
# based on code from
# probe.py Copyright 2010 Michael Haberler
#
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA''''''
import os,sys
from gladevcp.persistence import IniFile,widget_defaults,set_debug,select_widgets
import hal
import hal_glib
import glib
import linuxcnc
import cairo
import signal
import re
import gi
gi.require_version('Rsvg', '2.0')
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
from gi.repository import Gdk
from gi.repository import GObject
from gi.repository import Pango
from gi.repository import Rsvg
from gi.repository import GdkPixbuf
debug = 0
notouch = 0
norun = 0
class HandlerClass:
active = False
tab_num = 0
def on_expose(self,nb,data=None):
tab_num = nb.get_current_page()
tab = nb.get_nth_page(tab_num)
alloc = tab.get_allocation()
x, y, w, h = (alloc.x, alloc.y, alloc.width, alloc.height)
pixbuf = self.svg.get_pixbuf_sub(f'#layer{tab_num}').scale_simple(w-10, h-10, GdkPixbuf.InterpType.BILINEAR)
im = self.builder.get_object(f'Image{tab_num}')
im.set_from_pixbuf(pixbuf)
for c in im.get_parent().get_children():
if c.get_has_tooltip():
m = re.findall(r'<!--(\d+),(\d+)-->', c.get_tooltip_markup())
if len(m) > 0:
x1 = int(m[0][0]); y1 = int(m[0][1])
c.set_margin_left(max(0, w * x1/1500))
c.set_margin_top(max(0, h * y1/1000))
# decide if our window is active to mask the cycle-start hardware button
# FIXME: This is probably not as reliable as one might wish.
def event(self,w,event):
if w.is_active():
if w.has_toplevel_focus() :
self.active = True
else:
self.active = False
# Capture notify events
def on_map_event(self, widget, data=None):
top = widget.get_toplevel()
top.connect('notify', self.event)
def on_destroy(self,obj,data=None):
self.ini.save_state(self)
def on_restore_defaults(self,button,data=None):
'''
example callback for 'Reset to defaults' button
currently unused
'''
self.ini.create_default_ini()
self.ini.restore_state(self)
def __init__(self, halcomp,builder,useropts):
self.halcomp = halcomp
self.builder = builder
self.ini_filename = 'savestate.sav'
self.defaults = { IniFile.vars: dict(),
IniFile.widgets : widget_defaults(select_widgets(self.builder.get_objects(),
hal_only=False,output_only = True))
}
self.ini = IniFile(self.ini_filename,self.defaults,self.builder)
self.ini.restore_state(self)
# A pin to use a physical switch to start the cycle
self.cycle_start = hal_glib.GPin(halcomp.newpin('cycle-start', hal.HAL_BIT, hal.HAL_IN))
self.cycle_start.connect('value-changed', self.cycle_pin)
# This catches the signal from Touchy to say that the tab is exposed
t = self.builder.get_object('macrobox')
t.connect('map-event',self.on_map_event)
t.add_events(Gdk.EventMask.STRUCTURE_MASK)
self.cmd = linuxcnc.command()
# This connects the expose event to re-draw and scale the SVG frames
t = self.builder.get_object('tabs1')
t.connect_after("draw", self.on_expose)
t.connect("destroy", Gtk.main_quit)
t.add_events(Gdk.EventMask.STRUCTURE_MASK)
self.svg = Rsvg.Handle().new_from_file('LatheMacro.svg')
self.active = True
# handle Useropts
if norun:
for c in range(0,6):
print(c)
print( f'tab{c}.action')
self.builder.get_object(f'tab{c}.action').set_visible(False)
def show_keyb(self, obj, data=None):
if notouch: return False
self.active_ctrl = obj
self.keyb = self.builder.get_object('keyboard')
self.entry = self.builder.get_object('entry1')
self.entry.modify_font(Pango.FontDescription("courier 42"))
self.entry.set_text("")
resp = self.keyb.run()
return True
def keyb_prev_click(self, obj, data=None):
self.entry.set_text(self.active_ctrl.get_text())
def keyb_number_click(self, obj, data=None):
data = self.entry.get_text()
data = data + obj.get_label()
if any( x in data for x in [ '/2', '/4', '/8', '/16', '/32', '/64', '/128']):
v = [0] + [float(x) for x in data.replace('/','.').split('.')]
data = f'{v[-3] + v[-2]/v[-1]:6.7}'
self.entry.set_text(data)
def keyb_pm_click(self, obj, data=None):
data = self.entry.get_text()
if data[0] == '-':
data = data[1:]
else:
data = '-' + data
self.entry.set_text(data)
def keyb_convert_click(self, obj, data=None):
v = float(self.entry.get_text())
op = obj.get_label()
if op == 'in->mm':
self.entry.set_text(f'{v * 25.4:6.4}')
elif op == 'mm->in':
self.entry.set_text(f'{v / 25.4:6.4}')
elif op == 'tpi->pitch':
self.entry.set_text(f'{25.4 / v:6.4}')
elif op == 'pitch->tpi':
self.entry.set_text(f'{25.4 / v:6.4}')
def keyb_del_click(self, obj, data=None):
data = self.entry.get_text()
data = data[:-1]
self.entry.set_text(data)
def keyb_clear_click(self, obj, data=None):
self.entry.set_text('')
def keyb_cancel_click(self, obj, data=None):
self.keyb.hide()
def keyb_ok_click(self, obj, data=None):
if self.entry.get_text() != '':
self.active_ctrl.set_value(float(self.entry.get_text()))
self.keyb.hide()
def set_alpha(self, obj, data = None):
cr = obj.get_property('window').cairo_create()
cr.set_source_rgba(1.0, 1.0, 1.0, 0.0)
def cycle_pin(self, pin, data = None):
if pin.get() == 0:
return
if self.active:
nb = self.builder.get_object('tabs1')
print('current tab', nb.get_current_page())
c = self.builder.get_object(f"tab{nb.get_current_page()}.action")
if c is not None:
self.cmd.abort()
self.cmd.mode(linuxcnc.MODE_MDI)
self.cmd.wait_complete()
c.emit('clicked')
print(c.get_name(), "clicked")
def testing(self, obj, data = None):
print('event', data)
def get_handlers(halcomp,builder,useropts):
global debug
for cmd in useropts:
print(cmd)
exec(cmd, globals())
set_debug(debug)
return [HandlerClass(halcomp,builder,useropts)]

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,119 @@
5161 0.000000
5162 0.000000
5163 0.000000
5164 0.000000
5165 0.000000
5166 0.000000
5167 0.000000
5168 0.000000
5169 0.000000
5181 0.000000
5182 0.000000
5183 0.000000
5184 0.000000
5185 0.000000
5186 0.000000
5187 0.000000
5188 0.000000
5189 0.000000
5210 0.000000
5211 0.000000
5212 0.000000
5213 0.000000
5214 0.000000
5215 0.000000
5216 0.000000
5217 0.000000
5218 0.000000
5219 0.000000
5220 1.000000
5221 0.000000
5222 0.000000
5223 -138.733334
5224 0.000000
5225 0.000000
5226 0.000000
5227 0.000000
5228 0.000000
5229 0.000000
5230 0.000000
5241 0.000000
5242 0.000000
5243 0.000000
5244 0.000000
5245 0.000000
5246 0.000000
5247 0.000000
5248 0.000000
5249 0.000000
5250 0.000000
5261 0.000000
5262 0.000000
5263 0.000000
5264 0.000000
5265 0.000000
5266 0.000000
5267 0.000000
5268 0.000000
5269 0.000000
5270 0.000000
5281 0.000000
5282 0.000000
5283 0.000000
5284 0.000000
5285 0.000000
5286 0.000000
5287 0.000000
5288 0.000000
5289 0.000000
5290 0.000000
5301 0.000000
5302 0.000000
5303 0.000000
5304 0.000000
5305 0.000000
5306 0.000000
5307 0.000000
5308 0.000000
5309 0.000000
5310 0.000000
5321 0.000000
5322 0.000000
5323 0.000000
5324 0.000000
5325 0.000000
5326 0.000000
5327 0.000000
5328 0.000000
5329 0.000000
5330 0.000000
5341 0.000000
5342 0.000000
5343 0.000000
5344 0.000000
5345 0.000000
5346 0.000000
5347 0.000000
5348 0.000000
5349 0.000000
5350 0.000000
5361 0.000000
5362 0.000000
5363 0.000000
5364 0.000000
5365 0.000000
5366 0.000000
5367 0.000000
5368 0.000000
5369 0.000000
5370 0.000000
5381 0.000000
5382 0.000000
5383 0.000000
5384 0.000000
5385 0.000000
5386 0.000000
5387 0.000000
5388 0.000000
5389 0.000000
5390 0.000000

View File

@@ -0,0 +1,119 @@
5161 0.000000
5162 0.000000
5163 0.000000
5164 0.000000
5165 0.000000
5166 0.000000
5167 0.000000
5168 0.000000
5169 0.000000
5181 0.000000
5182 0.000000
5183 0.000000
5184 0.000000
5185 0.000000
5186 0.000000
5187 0.000000
5188 0.000000
5189 0.000000
5210 0.000000
5211 0.000000
5212 0.000000
5213 0.000000
5214 0.000000
5215 0.000000
5216 0.000000
5217 0.000000
5218 0.000000
5219 0.000000
5220 1.000000
5221 0.000000
5222 0.000000
5223 -138.733334
5224 0.000000
5225 0.000000
5226 0.000000
5227 0.000000
5228 0.000000
5229 0.000000
5230 0.000000
5241 0.000000
5242 0.000000
5243 0.000000
5244 0.000000
5245 0.000000
5246 0.000000
5247 0.000000
5248 0.000000
5249 0.000000
5250 0.000000
5261 0.000000
5262 0.000000
5263 0.000000
5264 0.000000
5265 0.000000
5266 0.000000
5267 0.000000
5268 0.000000
5269 0.000000
5270 0.000000
5281 0.000000
5282 0.000000
5283 0.000000
5284 0.000000
5285 0.000000
5286 0.000000
5287 0.000000
5288 0.000000
5289 0.000000
5290 0.000000
5301 0.000000
5302 0.000000
5303 0.000000
5304 0.000000
5305 0.000000
5306 0.000000
5307 0.000000
5308 0.000000
5309 0.000000
5310 0.000000
5321 0.000000
5322 0.000000
5323 0.000000
5324 0.000000
5325 0.000000
5326 0.000000
5327 0.000000
5328 0.000000
5329 0.000000
5330 0.000000
5341 0.000000
5342 0.000000
5343 0.000000
5344 0.000000
5345 0.000000
5346 0.000000
5347 0.000000
5348 0.000000
5349 0.000000
5350 0.000000
5361 0.000000
5362 0.000000
5363 0.000000
5364 0.000000
5365 0.000000
5366 0.000000
5367 0.000000
5368 0.000000
5369 0.000000
5370 0.000000
5381 0.000000
5382 0.000000
5383 0.000000
5384 0.000000
5385 0.000000
5386 0.000000
5387 0.000000
5388 0.000000
5389 0.000000
5390 0.000000

View File

@@ -0,0 +1,14 @@
component metalmusings_encoder;
pin io bit index-c-enable;
pin in u32 index-status;
pin out u32 index-latch-enable;
function _;
license "GPL";
;;
//main function
FUNCTION(_) {
index_latch_enable = index_c_enable;
index_c_enable = index_status;
}

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=0.1923
MAXRPM=380
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=0.3189
MAXRPM=630
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=0.4638
MAXRPM=920
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=0.8846
MAXRPM=1770
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=1.4672
MAXRPM=2930
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,6 @@
#!/bin/bash
R=2.1334
MAXRPM=4260
halcmd setp scale.0.gain $R
halcmd setp scale.1.gain $(echo "scale=5;1.0 / $R"| bc -q)
exit 0

View File

@@ -0,0 +1,10 @@
%
(1)
#1 = 0
o100 while [#1 LT 10]
G0 X-200 Z-200
G0 X200 Z200
#1 = [#1+1]
o100 endwhile
M2
%

View File

@@ -0,0 +1,16 @@
0.55999995357589616
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000
0.00000000000000000

View File

@@ -0,0 +1,3 @@
# These files are loaded post GUI, in the order they appear
source custom_postgui.hal

View File

@@ -0,0 +1,76 @@
;radius
O<radius> sub
G8 ; Lathe radius Mode
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
M6 T#6 G43
#1 = [#1 / 2] ; because of radius mode
#14 = [#<_x>] (starting X)
#13 = [#<_z>] (starting Z)
G96 D2400 S#2 ; Constant Surface Speed Mode
M3
g95 F0.1 ; Feed-Per-Rev Mode
O90 IF [#12 GT 0.5]
M8
O90 ENDIF
#20 = 0
O101 if [#9 GT 0.5] ; Front outside
o100 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 - #20] z#13
g1 z#5
g3 x#1 z[#5 - #20] K[-#20]
g1 x #14
g0 z#13
o100 endwhile
g0 x#14 z#13
g0 x[#1 - #8]
g1 z#5
g3 x#1 z[#5 - #8] K[-#8]
g1 x #14
g0 z#13
O101 elseif [#10 GT 0.5] ; front inside
o102 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 + #20] z#13
g1 z#5
g2 x#1 z[#5 - #20] K[-#20]
g1 x #14
g0 z#13
o102 endwhile
g0 x#14 z#13
g0 x[#1 + #8]
g1 z#5
g2 x#1 z[#5 - #8] K[-#8]
g1 x #14
g0 z#13
O101 elseif [#11 GT 0.5] ; back outside
o103 while [[#20 + #3] lt #8]
#20 = [#20 + #3]
g0 x[#1 - #20] z#13
g1 z#5
g2 x#1 z[#5 + #20] K#20
g1 x #14
g0 z#13
o103 endwhile
g0 x#14 z#13
g0 x[#1 - #8]
g1 z#5
g2 x#1 z[#5 + #8] K#8
g1 x #14
g0 z#13
O101 endif
M5 M9
G7
O<radius> endsub
m2
%

View File

@@ -0,0 +1,71 @@
# generated by gladevcp.persistence.create_default_ini() on Tue Jul 12 01:49:47 2022
[ini]
signature = a9fe768b61bb9201e86d89ad226616650f20e7ac
version = 1
[vars]
[widgets]
chamfer.fi = False
chamfer.fo = False
turn.rad = 3.0
groove.coolant = True
turn.z = 120.0
turn.x = 15.0
drill.dia = 10.0
radius.x = 0.0
radius.z = 0.0
thread.pitch = 1.0
turn.feed = 0.15
thread.x = 6.0
bore.rad = 0.0
radius.coolant = False
radius.bo = False
thread.z = 0.0
face.coolant = False
drill.peck = 2.0
face.cut = 2.8999999999999977
radius.rad = 1.0
thread.internal = False
face.sf = 0.0
bore.sf = 100.0
turn.tool = 1.0
face.feed = 0.15
chamfer.tool = 1.0
chamfer.bo = True
groove.sf = 100.0
thread.tool = 5.0
groove.feed = 0.03
bore.feed = 0.15
radius.sf = 100.0
chamfer.x = 0.0
chamfer.z = 0.0
thread.external = True
turn.coolant = True
face.z = 0.0
bore.x = 0.0
bore.z = 0.0
groove.x = 0.0
face.tool = 1.0
face.x = 0.0
chamfer.size = 1.0
turn.cut = 1.0
drill.coolant = True
chamfer.sf = 100.0
bore.tool = 1.0
groove.tool = 4.0
face.angle = 0.0
bore.coolant = True
drill.feed = 0.020000000000000004
drill.z = 0.0
turn.sf = 60.0
thread.sf = 60.0
radius.fo = False
turn.angle = 0.0
thread.coolant = False
bore.cut = 1.0
radius.fi = True
drill.tool = 3.0
drill.sf = 100.0
radius.tool = 1.0
bore.angle = 0.0
chamfer.coolant = False
# last update by gladevcp.persistence.save_state() on Wed Mar 20 16:28:06 2024

View File

@@ -0,0 +1,2 @@
# Include your shutdown HAL commands here
# This file will not be overwritten when you run PNCconf again

119
linuxcnc/Turner.2ms/sim.var Normal file
View File

@@ -0,0 +1,119 @@
5161 0.000000
5162 0.000000
5163 0.000000
5164 0.000000
5165 0.000000
5166 0.000000
5167 0.000000
5168 0.000000
5169 0.000000
5181 0.000000
5182 0.000000
5183 0.000000
5184 0.000000
5185 0.000000
5186 0.000000
5187 0.000000
5188 0.000000
5189 0.000000
5210 0.000000
5211 0.000000
5212 0.000000
5213 0.000000
5214 0.000000
5215 0.000000
5216 0.000000
5217 0.000000
5218 0.000000
5219 0.000000
5220 1.000000
5221 0.000000
5222 0.000000
5223 0.000000
5224 0.000000
5225 0.000000
5226 0.000000
5227 0.000000
5228 0.000000
5229 0.000000
5230 0.000000
5241 0.000000
5242 0.000000
5243 0.000000
5244 0.000000
5245 0.000000
5246 0.000000
5247 0.000000
5248 0.000000
5249 0.000000
5250 0.000000
5261 0.000000
5262 0.000000
5263 0.000000
5264 0.000000
5265 0.000000
5266 0.000000
5267 0.000000
5268 0.000000
5269 0.000000
5270 0.000000
5281 0.000000
5282 0.000000
5283 0.000000
5284 0.000000
5285 0.000000
5286 0.000000
5287 0.000000
5288 0.000000
5289 0.000000
5290 0.000000
5301 0.000000
5302 0.000000
5303 0.000000
5304 0.000000
5305 0.000000
5306 0.000000
5307 0.000000
5308 0.000000
5309 0.000000
5310 0.000000
5321 0.000000
5322 0.000000
5323 0.000000
5324 0.000000
5325 0.000000
5326 0.000000
5327 0.000000
5328 0.000000
5329 0.000000
5330 0.000000
5341 0.000000
5342 0.000000
5343 0.000000
5344 0.000000
5345 0.000000
5346 0.000000
5347 0.000000
5348 0.000000
5349 0.000000
5350 0.000000
5361 0.000000
5362 0.000000
5363 0.000000
5364 0.000000
5365 0.000000
5366 0.000000
5367 0.000000
5368 0.000000
5369 0.000000
5370 0.000000
5381 0.000000
5382 0.000000
5383 0.000000
5384 0.000000
5385 0.000000
5386 0.000000
5387 0.000000
5388 0.000000
5389 0.000000
5390 0.000000

View File

@@ -0,0 +1,119 @@
5161 0.000000
5162 0.000000
5163 0.000000
5164 0.000000
5165 0.000000
5166 0.000000
5167 0.000000
5168 0.000000
5169 0.000000
5181 0.000000
5182 0.000000
5183 0.000000
5184 0.000000
5185 0.000000
5186 0.000000
5187 0.000000
5188 0.000000
5189 0.000000
5210 0.000000
5211 0.000000
5212 0.000000
5213 0.000000
5214 0.000000
5215 0.000000
5216 0.000000
5217 0.000000
5218 0.000000
5219 0.000000
5220 1.000000
5221 0.000000
5222 0.000000
5223 0.000000
5224 0.000000
5225 0.000000
5226 0.000000
5227 0.000000
5228 0.000000
5229 0.000000
5230 0.000000
5241 0.000000
5242 0.000000
5243 0.000000
5244 0.000000
5245 0.000000
5246 0.000000
5247 0.000000
5248 0.000000
5249 0.000000
5250 0.000000
5261 0.000000
5262 0.000000
5263 0.000000
5264 0.000000
5265 0.000000
5266 0.000000
5267 0.000000
5268 0.000000
5269 0.000000
5270 0.000000
5281 0.000000
5282 0.000000
5283 0.000000
5284 0.000000
5285 0.000000
5286 0.000000
5287 0.000000
5288 0.000000
5289 0.000000
5290 0.000000
5301 0.000000
5302 0.000000
5303 0.000000
5304 0.000000
5305 0.000000
5306 0.000000
5307 0.000000
5308 0.000000
5309 0.000000
5310 0.000000
5321 0.000000
5322 0.000000
5323 0.000000
5324 0.000000
5325 0.000000
5326 0.000000
5327 0.000000
5328 0.000000
5329 0.000000
5330 0.000000
5341 0.000000
5342 0.000000
5343 0.000000
5344 0.000000
5345 0.000000
5346 0.000000
5347 0.000000
5348 0.000000
5349 0.000000
5350 0.000000
5361 0.000000
5362 0.000000
5363 0.000000
5364 0.000000
5365 0.000000
5366 0.000000
5367 0.000000
5368 0.000000
5369 0.000000
5370 0.000000
5381 0.000000
5382 0.000000
5383 0.000000
5384 0.000000
5385 0.000000
5386 0.000000
5387 0.000000
5388 0.000000
5389 0.000000
5390 0.000000

View File

@@ -0,0 +1,21 @@
loadrt comp names=comp_x,comp_z
addf comp_x servo-thread
addf comp_z servo-thread
net Xhomeswpos => comp_x.in0
net Zhomeswpos => comp_z.in0
sets Xhomeswpos 1
sets Zhomeswpos 2
net Xpos => comp_x.in1
net Zpos => comp_z.in1
setp comp_x.hyst .02
setp comp_z.hyst .02
net Xhomesw <= comp_x.out => joint.0.home-sw-in
net Zhomesw <= comp_z.out => joint.1.home-sw-in

View File

@@ -0,0 +1,21 @@
loadrt comp names=comp_x,comp_z
addf comp_x servo-thread
addf comp_z servo-thread
net Xhomeswpos => comp_x.in0
net Zhomeswpos => comp_z.in0
sets Xhomeswpos 1
sets Zhomeswpos 2
net Xpos => comp_x.in1
net Zpos => comp_z.in1
setp comp_x.hyst .02
setp comp_z.hyst .02
net Xhomesw <= comp_x.out => joint.0.home-sw-in
net Zhomesw <= comp_z.out => joint.1.home-sw-in

View File

@@ -0,0 +1,69 @@
# counting the spindle encoder in software
loadrt encoder names=encoder_0
# simulate the encoder
loadrt sim_encoder names=sim_encoder_0
loadrt limit2 names=limit_speed
addf limit_speed servo-thread
#######################################################
# Beginning of threading related stuff
#######################################################
# spindle speed control
net spindle-speed-cmd spindle.0.speed-out => limit_speed.in
net spindle-speed-limited limit_speed.out => sim_encoder_0.speed
# simulate spindle mass
setp limit_speed.maxv 3000.0 # rpm/second
# spindle encoder
# connect encoder signals to encoder counter
net spindle-phase-A sim_encoder_0.phase-A => encoder_0.phase-A
net spindle-phase-B sim_encoder_0.phase-B => encoder_0.phase-B
net spindle-phase-Z sim_encoder_0.phase-Z => encoder_0.phase-Z
# assume 120 ppr = 480 counts/rev for the spindle
setp sim_encoder_0.ppr 12
# iocontrol output is in rpm, but sim-encoder speed is rps
setp sim_encoder_0.scale 60
# scale encoder output to read in revolutions
# (that way thread pitches can be straightforward,
# a 20 tpi thread would multiply the encoder output
# by 1/20, etc)
setp encoder_0.position-scale 48
# encoder reset control
# hook up motion controller's sync output
net spindle-index-enable spindle.0.index-enable <=> encoder_0.index-enable
# report our revolution count to the motion controller
net spindle-pos encoder_0.position => spindle.0.revs
# for spindle velocity estimate
loadrt lowpass names=lowpass_velocity
loadrt scale names=scale_to_rpm
net spindle-rps-raw encoder_0.velocity lowpass_velocity.in
net spindle-rps-filtered lowpass_velocity.out scale_to_rpm.in spindle.0.speed-in
net spindle-rpm-filtered scale_to_rpm.out
setp scale_to_rpm.gain 60
setp lowpass_velocity.gain .07
addf lowpass_velocity servo-thread
addf scale_to_rpm servo-thread
# for at-speed detection
loadrt near names=near_speed
addf near_speed servo-thread
setp near_speed.scale 1.1
setp near_speed.difference 10
net spindle-speed-cmd => near_speed.in1
net spindle-rpm-filtered => near_speed.in2
net spindle-at-speed near_speed.out spindle.0.at-speed
net spindle-fwd <= spindle.0.forward
addf encoder.capture-position servo-thread
addf sim-encoder.update-speed servo-thread
addf sim-encoder.make-pulses base-thread
addf encoder.update-counters base-thread

View File

@@ -0,0 +1,59 @@
;threading
O<threading> sub
G7 ; Lathe Diameter Mode
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
M6 T#3 G43
#14 = [#<_x> * 2] (starting X)
#13 = #<_z> (starting Z)
G96 D200 S#2 ; Constant Surface Speed Mode
M3
g95 F0.25 ; Feed-Per-Rev Mode
O90 IF [#8 GT 0.5]
M8
O90 ENDIF
g4p1 ; Wait to reach speed
;Threading
O51 IF [#6 GT 0.5]
#<OD> = [#1]
#<ID> = [#1 - 1.3 * #4]
;g1X [#<ID> - 1] ;thread truncation
;g0 Z #13
;g1 X #<ID>
;g1 Z #5
G0 X[#<ID> - 1]
g0 Z #13
#3 = [#4 * 1.3]
(debug, INTERNAL Threading thread dia-#1 start-#13 finish-#5 Pitch-#4 Depth-#3)
g1X [#<ID> - 1]
g76 p#4 z#5 i1 j1 k#3 h3 r1.5 q29.5 e0 l0
O51 ELSE
#<OD> = [#1 - 0.108 * #4]
#<ID> = [#1 - 1.0825 * #4]
(debug, EXTERNAL Threading OD = #<OD> ID = #<ID>)
#3 = [#4 * 1.0825]
g1X [#<OD> + 1] ;final thread truncation
g0 z#13
g1 X #<OD>
g1 Z #5
G0 X[#<OD> +1]
G0 Z #13
g76 p#4 z#5 i-1 j1 k#3 h3 r1.5 q29.5 e0 l0
O51 ENDIF
G0 Z #13
m5 M9
O<threading> endsub
M2

View File

@@ -0,0 +1,19 @@
T1 P1 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;# rh turning
T2 P2 X-25.3242 Y0.0 Z15.924 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I80.0 J20.0 Q3.0 ;
T3 P3 X-15.4825 Y0.0 Z258.6854 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D3.0 I90.0 J90.0 Q6.0 ;
T4 P4 X-0.5336 Y0.0 Z54.9583 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I87.0 J40.0 Q2.0 ;
T5 P5 X-15.0582 Y0.0 Z117.9963 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I60.0 J200.0 Q5.0 ;
T6 P6 X2.6969 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.5 I120.0 J70.0 Q6.0 ;# threading
T7 P7 X-8.408 Y0.0 Z13.7459 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.8 I107.5 J77.5 Q6.0 ;# vcmt110304
T8 P8 X-11.16 Y0.0 Z-31.05 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I200.0 J300.0 Q8.0 ;
T9 P9 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.1 I0.0 J0.0 Q9.0 ;
T10 P10 X-0.07 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;
T11 P11 X-23.0718 Y0.0 Z-3.7292 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;
T12 P12 X-1.8392 Y0.0 Z-3.8279 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;
T13 P13 X-10.8533 Y0.0 Z-6.2652 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;
T14 P14 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I300.0 J200.0 Q4.0 ;
T15 P15 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I200.0 J200.0 Q5.0 ;
T16 P16 X-15.13 Y0.0 Z14.91 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I100.0 J60.0 Q6.0 ;
T17 P17 X-15.14 Y0.0 Z5.992 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I-30.0 J30.0 Q7.0 ;
T18 P18 X0.0 Y0.0 Z0.0 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I200.0 J300.0 Q8.0 ;
T19 P19 X29.7817 Y0.0 Z-21.8231 A0.0 B0.0 C0.0 U0.0 V0.0 W0.0 D0.0 I0.0 J0.0 Q9.0 ;

View File

@@ -0,0 +1,66 @@
;Turning
O<turning> sub
G8 ; Radius mode (easier maths)
G18 ; XZ Plane
G21 ; Metric Units
G90 ; Absolute Distance
G91.1 ; but not for arcs
M6 T#8 G43
#1 = [#1 / 2] ; because of radius mode
#14 = [#<_x>] (starting X)
#13 = #<_z> (starting Z)
#20 = [#6 * SIN[#7]]
#21 = [#6 * COS[#7]]
#22 = [#6 / COS[#7]]
#23 = [#5 + #6 - #20]
#24 = [[#13 - #23] * TAN[#7]]
G96 D2500 S#2 ; Constant Surface Speed Mode
m3 ;Start Spindle
g95 F#4 ; Feed-Per-Rev Mode
O90 IF [#9 GT 0.5]
M8
O90 ENDIF
g4p1 ; Wait to reach speed
O100 WHILE [#14 GT [#1 + #3 / 2]]
g0 X #14
#14=[#14-#3 / 2]
G1 X #14
G1 Z #23 X[#14 + #24]
O101 IF [#6 GT 0]
G2 Z#5 X[#14 + #24 + #21] I#21 K#20
G1 X[#14 + #24 + #21 + #3/2]
O101 ELSE
G1 X[#14 + #24 + [#3 * .6]]
O101 ENDIF
O104 IF [#7 LT 0]
G0 X#14
O104 ENDIF
G0 Z[#13]
O100 ENDWHILE
G0 x#1
G1 Z #23 X[#1 + #24]
O102 IF [#6 GT 0]
G2 Z#5 X[#1 + #24 + #21] I#21 K#20
G1 X[#1 + #24 + #21 + #3]
O102 ELSE
G1 X[#1 + #24 + #3]
O102 ENDIF
O106 IF [#7 LT 0]
G0 X#14
O106 ENDIF
M9
G0 Z #13
G0 X #1 ; For touch-off
M5
G7
O<turning> endsub
M2

View File

@@ -10,5 +10,12 @@ license "GPL";
//main function
FUNCTION(_) {
index_latch_enable = index_c_enable;
index_c_enable = index_status;
if (index_latch_enable) {
if (index_status) {
index_c_enable = 0;
}
// else wait for index-status
} else {
index_c_enable = index_status;
}
}