Advanced Search

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

setp lcec.0.1.scale [SPINDLE_9]ENCODER_SCALE
#setp lcec.0.1.enc-pos-scale [SPINDLE_9]ENCODER_SCALE

net spindle-revs <= lcec.0.1.encpos
#net spindle-revs <= lcec.0.1.Encoder 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.1.encfrequency
#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.1.indexstatus

net  spindle-vel-fb-rps   mult2.rpm.in1  <= lcec.0.1.encfrequency
net  spindle-vel-fb-rpm   mult2.rpm.out

net  to_encoder           metalmusings-encoder.0.index-latch-enable lcec.0.1.indexlatchenable
net  from_encoder         metalmusings-encoder.0.index-status       lcec.0.1.indexstatus
net  spindle-index-enable metalmusings-encoder.0.index-c-enable

///////// Spindle Encoder
#include "MyEncoder.h"
volatile uint16_t encCnt = 0;
void indexPulseEncoderCB1(void);
MyEncoder Encoder1(TIM2, PA3, indexPulseEncoderCB1);  
void indexPulseEncoderCB1(void)
{
   encCnt++;
   Encoder1.indexPulse();
}

////// EtherCAT routines called regularly, to read data, do stuff and write data
void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation
{
   for (int i = 0; i < min(sizeof(Obj.Outputs), sizeof(OUTPUTS)); i++)
      digitalWrite(OUTPUTS[i], Obj.Outputs[i] == 1 ? HIGH : LOW);
   analogWrite(DAC1_pin, Obj.Voltage);
   Encoder1.setLatch(Obj.IndexLatchEnable);
   // Encoder1.setScale(2000);
   Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1; // Scale perhaps changed
   Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
   Step->stepgen_array[2].pos_scale = -Obj.StepsPerMM3;
   Step->stepgen_array[3].pos_scale = -Obj.StepsPerMM4;
   posCmd1 = Obj.CommandedPosition1; // The position update
   posCmd2 = Obj.CommandedPosition2;
   posCmd3 = Obj.CommandedPosition3;
   posCmd4 = Obj.CommandedPosition4;
}
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{
   Obj.Velocity = Obj.Scale * FrequencyMeasured;
   float scale = 1;
   if (Obj.Scale != 0.0)
      scale = Obj.Scale;
   Obj.Velocity = scale * sin(ESCvar.Time * 1e-8 * 6.28); // Test
   for (int i = 0; i < min(sizeof(Obj.Inputs), sizeof(INPUTS)); i++)
      Obj.Inputs[i] = digitalRead(INPUTS[i]) == HIGH ? 1 : 0;
#if 1
   Obj.IndexStatus = Encoder1.indexHappened();
   Obj.EncPos = Encoder1.currentPos();
   Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros()));
   Obj.IndexByte = Encoder1.getIndexState();
   Obj.Velocity = Obj.Scale * FrequencyMeasured;
#endif
   Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
   Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
   Obj.ActualPosition3 = Step->stepgen_array[2].pos_fb;
   Obj.ActualPosition4 = Step->stepgen_array[3].pos_fb;
}

   /* Inputs */
   float Velocity;
   uint8_t Inputs[8];
   float Frequency;
   float ActualPosition1;
   float ActualPosition2;
   float ActualPosition3;
   float ActualPosition4;
   float EncPos;
   float EncFrequency;
   uint16_t DiffT;
   uint32_t IndexByte;
   uint8_t IndexStatus;
   /* Outputs */
   float Scale;
   uint8_t Outputs[4];
   float CommandedPosition1;
   float CommandedPosition2;
   float CommandedPosition3;
   float CommandedPosition4;
   int32_t StepsPerMM1;
   int32_t StepsPerMM2;
   int32_t StepsPerMM3;
   int32_t StepsPerMM4;
   float Voltage;
   uint8_t IndexLatchEnable;