HobbyCNC fórum - UCCNC vezérlő program

// ================================================================================================ // Multi coolant switch // ================================================================================================ // // Beállítások: // const int EnableLED = 52; M7: 52, M8: 53 // const int tmrConst = 10; Kikapcsolás késleltetése 1/20 s-ban // const int avgMax = 10; Átlagolt koordináták száma (1/20 s-onként olvas újat) // double[] coolX = {5.0, -5.0}; Fúvókák X koordinátája a főorsóhoz képest (1. fúvóka, 2. fúvóka [, ...]) // double[] coolY = {5.0, -5.0}; Fúfókák Y koordinátája a főorsóhoz képest (1. fúvóka, 2. fúvóka [, ...]) // int[] vPorts = {3, 3}; Szelep kimenetek port száma (1. fúvóka, 2. fúvóka [, ...]) // int[] vPins = {1, 2}; Szelep kimenetek láb száma (1. fúvóka, 2. fúvóka [, ...]) // bool[] vNeg = {false, false}; Kimenetek ActiveLow beállításai: ha negált, akkor true (1. fúvóka, 2. fúvóka [, ...]) // int[] coolZ = {0, 1}; Z mozgáskor melyik sorszámú szelepek menjenek, számozás 0-tól indul // // Mxxx.txt, ahol xxx egy még nem használt sorszám, MacroLoops-ban ezt a sorszámot kell beállítani, AutoRun pipával // // ================================================================================================ if (firstRun) { firstRun = false; for (int i = 0; i < avgMax; i++) { deltaXa[i] = 0.0; deltaYa[i] = 0.0; } } double machX = exec.GetXmachpos(); double machY = exec.GetYmachpos(); double machZ = exec.GetZmachpos(); double deltaX = machX - lastX; double deltaY = machY - lastY; double deltaZ = machZ - lastZ; deltaXa[avgPtr] = deltaX; deltaYa[avgPtr] = deltaY; if (++avgPtr == avgMax) avgPtr = 0; deltaX = 0.0; deltaY = 0.0; for (int i = 0; i < avgMax; i++) { deltaX += deltaXa[i]; deltaY += deltaYa[i]; } deltaX = deltaX / avgMax; deltaY = deltaY / avgMax; int valve = FindValve(deltaX, deltaY); if (exec.GetLED(EnableLED) && (exec.GetLED(M3LED) || exec.GetLED(M4LED))) // Spindle is on and coolant enabled { if (deltaZ != 0.0) { // Ha ki volt kapcsolva (lastValve), akkor valamit be kéne kapcsolni timer = tmrConst; SetValve(-2); } else if ((deltaX != 0.0) || (deltaY != 0.0)) { // Irány bekapcsolása timer = tmrConst; SetValve(valve); } } else if (timer > 0) timer = 1; // Instant off if turned on not enabled if (timer > 0) if (--timer == 0) SetValve(-1); lastX = machX; lastY = machY; lastZ = machZ; // ================================================================================================ #Events // ================================================================================================ const int M3LED = 50; const int M4LED = 51; const int EnableLED = 52; // M7: 52, M8: 53 const int tmrConst = 10; // Number of 1/20 s static double lastX = 0.0; static double lastY = 0.0; static double lastZ = 0.0; static int timer = 0; const int avgMax = 10; double[] deltaXa = new double[avgMax]; double[] deltaYa = new double[avgMax]; double[] coolX = {5.0, -5.0}; double[] coolY = {5.0, -5.0}; int[] vPorts = {3, 3}; int[] vPins = {1, 2}; bool[] vNeg = {false, false}; int[] coolZ = {0, 1}; int lastValve = -1; int avgPtr = 0; bool firstRun = true; // ================================================================================================ private void SetValve(int vset) { if (vset != lastValve) { if (vset == -2) { // Z szelepek nyitása bool inCoolZ; for (int i = 0; i < vPorts.Length; i++) { inCoolZ = false; for (int j = 0; j < coolZ.Length; j++) if (coolZ[j] == i) inCoolZ = true; if (inCoolZ) SetPin(vPorts[i], vPins[i], vNeg[i]); else ClrPin(vPorts[i], vPins[i], vNeg[i]); } } else for (int i = 0; i < vPorts.Length; i++) { if (i == vset) SetPin(vPorts[i], vPins[i], vNeg[i]); else ClrPin(vPorts[i], vPins[i], vNeg[i]); } lastValve = vset; Console.WriteLine("SetValve: " + vset.ToString()); } } // ================================================================================================ private void SetPin(int port, int pin, bool neg) { if (neg) exec.Clroutpin(port, pin); else exec.Setoutpin(port, pin); } private void ClrPin(int port, int pin, bool neg) { if (neg) exec.Setoutpin(port, pin); else exec.Clroutpin(port, pin); } // ================================================================================================ int FindValve(double deltaX, double deltaY) { double angle = 0.0; double check = 181.0; int ptr = 0; int valve = -1; if ((deltaX != 0.0) || (deltaY != 0.0)) { for (int i = 0; i < coolX.Length; i++) { angle = Math.Abs(GetAngle(coolX[i], coolY[i], deltaX, deltaY)); if ((angle == check) && (i == lastValve)) valve = i; if (angle < check) { check = angle; valve = i; } } } return valve; } // ================================================================================================ double GetAngle(double x1, double y1, double x2, double y2) { double angle = Math.Atan2(y2, x2) - Math.Atan2(y1, x1); angle = angle * 360 / (2 * Math.PI); if (angle < 0) angle += 360; if (angle > 180) angle = -(360 - angle); return angle; } // ================================================================================================