Source file: /~heha/basteln/NC/Mach3Usb/Mach3Usb.zip/MachDevImplementation.cpp

#include "MachDevice.h"
#include "MachDevImplementation.h"
// >>>>> See this file for the Mach functions that can be
//		called and the pointers to the variable blocks.

#include <mmsystem.h>
#include <math.h>
 
//extern CMach4View *MachView;


static void SendWaveform(char*data, int len, int nAxes) {
 if (!len) return;
 TCHAR s[256];
 wsprintf(s,TEXT("SendWaveform: %d bytes\n"),len);
 OutputDebugString(s);
}

void myUpdate(void) {
// 40Hz update loop
 if (Engine->TrajIndex == Engine->TrajHead) return;
 int nAxes=0;
 for (int i=0; i<6; i++) if (Engine->Axis[i].Enable) nAxes++;
 static int lastpos[6];
 char *buf = new char[4096*6];
 char *p = buf;
 TCHAR s[100];
 HFILE hLog=_lopen("wave.log",OF_WRITE|OF_SHARE_DENY_NONE);
 if (hLog==HFILE_ERROR) hLog=_lcreat("wave.log",0);
 _llseek(hLog,0,SEEK_END);
 _lwrite(hLog,s,wsprintf(s,"---\n"));
 static DWORD myTrajIndex;
 if (Engine->TrajIndex!=myTrajIndex) {
  _lwrite(hLog,s,wsprintf(s,"** TrayIndex moved! %u!=%u\n",Engine->TrajIndex,myTrajIndex));
 }
 for (;Engine->TrajIndex != Engine->TrajHead;
   Engine->TrajIndex = (Engine->TrajIndex+1)&0xFFF) {	// 4K Ringpuffer
  static DWORD nSamp;
  char *sp=s+wsprintf(s,"%u\t",nSamp++);
  char v_or=0;
  for (int i=0; i<6; i++) if (Engine->Axis[i].Enable) {
   int pos = Engine->Trajectories[Engine->TrajIndex].Points[i];
   char v = (char)(pos - lastpos[i]);	// calculate velocity, should not exceed ± 64
   v_or |= v;
   sp+=wsprintf(sp,"%d\t",v);
   *p++=v;
   lastpos[i] = pos;
  }
  sp[-1]='\n';
  if (v_or) _lwrite(hLog,s,sp-s);
 }
 _lclose(hLog);
 myTrajIndex=Engine->TrajIndex;
 SendWaveform(buf,p-buf,nAxes);
 delete buf;
// Check here for disconnection for G100.
// If more than 1/2 second has passed and no ethernet message, trigger an EStop
// Inputs to InActive..
}