--[[ -------------------------------------------------------------------------------------- ---- LOAD SCRIPT LOAD SCRIPT LOAD SCRIPT LOAD SCRIPT LOAD SCRIPT PLC SCRIPT ---- -------------------------------------------------------------------------------------- -- --DrivesComRS422 module package.loaded.DrivesComRS422 = nil DrivesComRS422 = require "DrivesComRS422" -------------------------------------------------------------------------------------- --- UNLOAD SCRIPT UNLOAD SCRIPT UNLOAD SCRIPT UNLOAD SCRIPT UNLOAD SCRIPT ---- -------------------------------------------------------------------------------------- --ClosePortRS422() -------------------------------------------------------------------------------------- ---- PLC SCRIPT PLC SCRIPT PLC SCRIPT PLC SCRIPT PLC SCRIPT PLC SCRIPT ---- -------------------------------------------------------------------------------------- --first run OpenPortRS422() -------------------------------------------------------------------------------------- if ServoLinkRS422 == 0 then OpenPortRS422() end if([Estop == 0]) -- Estop behavoir then [Servo_ON = 0] --Disable Servos Mach_Enable = 0 --Disable Mach mc.mcAxisDerefAll(inst) --Dereference axis, just to turn off all ref leds --GetCurrentAlarm() --Get Current Alarm if Error == lasterror --If error was reported do nothing then return else Display Error --if new error display end else if([Estop == 1])then --no Estop behavoir [Servo_ON = 1] --Enable Servos Mach_Enable = 1 --Enable Mach Sync_Start = 1 --Start Syncronization end end if(ready == 1 and homed == 0 and Sync_Start == 1)then Derefall() --Dereference all axis ABS_Sync() --Update ABS Position if Sync == 1 --Pos Sync Complete then homed = 1 --Complete Homed else Sync_Start = 0 --Syncronization failed mc.box("Servo Syncronization Failed\n") end end mc.mcAxisDerefAll(inst) --Just to turn off all ref leds mc.mcAxisHomeAll(inst) -------------------------------Syncronize ABS Position/Align Y axis/Enable Mach------------------------------------ function ABS_Sync() --Syncronize ABS Position/Align Y axis/Enable Mach ABSGetPosition() --Update ABS Position if GetPosReady == 1 --ABS pos syncronized then AxisAlign() --Align Axis if YAligned == 1 then Sync = 1 --Is really required to home the machine to work? --GoToWorkZero() --Move Machine to Zero preparing to reset Homeall() --mc.mcAxisHomeAll(inst) --Home all axis end else Sync = 0 --Pos Sync Incomplete end end ]] ------------------------------------Start of Code DrivesCom RS422----------------------------------- ---[[ local mcSerial = {} local inst = mc.mcGetInstance() package.path = package.path .. ";./Modules/?.lua;" package.cpath = package.cpath .. ";./Modules/?.dll;" --package.cpath = "C:/src/Mach4/Modules/?.dll;" rs422 = require("luars232") socket = require("socket") --------------------------------------Variable Declaration Section---------------------------------- ServoLinkRS422 = 0 port_name = "COM4" --Serial RS422 Ultracomm Sync = 0 DataRead = "" Checksum = "" ChecksumCode = "" Verification = "" Data = "" local out = io.stderr --]] -----------------------------------------------Open Com Port------------------------------------------------ function OpenPortRS422() p, Drives = rs422.open(port_name) if p ~= rs422.RS232_ERR_NOERROR then -- handle error mc.mcCntlSetLastError(inst,string.format("can't open serial port '%s', error: '%s'\n",port_name, rs422.error_tostring(p))) return end ----------------------------------------------set port settings--------------------------------------------- assert(Drives:set_baud_rate(rs422.RS232_BAUD_115200) == rs422.RS232_ERR_NOERROR) assert(Drives:set_data_bits(rs422.RS232_DATA_8) == rs422.RS232_ERR_NOERROR) assert(Drives:set_parity(rs422.RS232_PARITY_EVEN) == rs422.RS232_ERR_NOERROR) assert(Drives:set_stop_bits(rs422.RS232_STOP_1) == rs422.RS232_ERR_NOERROR) assert(Drives:set_flow_control(rs422.RS232_FLOW_OFF) == rs422.RS232_ERR_NOERROR) mc.mcCntlSetLastError(inst,string.format("OK, port open with values '%s'\n", tostring(p))) ServoLinkRS422 = 1 end --------------------------------------------------Close Com Port--------------------------------------------- function ClosePortRS422() assert(Drives:close() == rs422.RS232_ERR_NOERROR) ServoLinkRS422 = 0 --Port RS422 Closed end -------------------------------------------------Write to Serial Port---------------------------------------- function DrivesComSend(SendData) --[[local len = string.len(SendData) Send = "" for i=1,len,1 do Byte = string.format("%X", string.byte(SendData,i)) --ASCII to Hex Converter Send = Send..Byte end]] local timeout = 100 -- in miliseconds err, len_written = Drives:write(SendData, timeout) assert(p == rs422.RS232_ERR_NOERROR) mc.mcCntlSetLastError(inst,"RS422 Send: "..SendData) end ----------------------------------------------------Read Serial Port------------------------------------------ function DrivesComRead() local read_len = 22 -- read 22 bytes max timeout = 500 -- in miliseconds local err, DataRead, size = Drives:read(read_len, timeout) assert(p == rs422.RS232_ERR_NOERROR) ------------------------------------------------ASCII to Hex Converter---------------------------------------- --[[if Hex_Read ~= nil then local len = string.len(Hex_Read) DataRead = "" for i=1,len,1 do --Code = string.format("%02X", string.char(ASCII_Read,i)) --Hex to ASCII Converter Code = string.char(tonumber(Hex_Read,16),i) DataRead = DataRead..Code end end ]] mc.mcCntlSetLastError(inst,"RS422 Read: "..DataRead) end -------------------------------------------------------Flush buffer------------------------------------------- --Windows automatically fills a serial buffer with characters (up to 4096 I think) --This function reads the entire buffer to empty it out function mcSerial.FlushBuffer() local RV = 0 --RV, data_read, size = Drives:read(max_flush, timeout) RV = Drives:flush() end -------------------------------------------Decompose Frame to Extract Data--------------------------------------------- function FrameRead(Data) local Data1 = "" if string.byte(Data:sub(1)) == 1 or string.byte(Data:sub(1)) == 2 then Data1 = Data:sub(2,string.len(Data)) end StrData = "" Station = Data1:sub(1,1) ErrorCode = Data1:sub(2,2) StrData = tonumber(Data1:sub(3,(string.len(Data1))-3),16) ChecksumCode = Data1:sub(-2) --2 lsb --mc.mcCntlSetLastError(inst,"\nStation No:",Station,"\nError Code:",ErrorCode,"\nDataRead:",StrData,"\nChecksumCode:",ChecksumCode) end -------------------------------------------------Checksum calculation-------------------------------------------------- function ChecksumCalculate(Data) local DecSum = 0 local DEC = "" if string.byte(Data:sub(1)) == 1 or string.byte(Data:sub(1)) == 2 then Data = Data:sub(2,string.len(Data)) end for i=1,(string.len(Data)),1 do DEC = string.byte(Data,i) if DEC ~= nil then DecSum = DecSum + DEC end --mc.mcCntlSetLastError(inst,"DEC: "..DEC.." Sum: "..DecSum) if string.byte(Data,i+1) == 3 then DecSum = DecSum + 3 checksum = (string.format("%x", DecSum)):sub(-2) --2 lsb --mc.mcCntlSetLastError(inst,"\nCalculated Checksum:",checksum) break end end end --------------------------------------------Drives Communication function---------------------------------------------- function Comm_Drives(Msg) --Start communication with drives ---------------------------------Await for driver response-------------------------------- for retry = 1,3,1 do --3 times retransmision if no response DrivesComSend(""..Msg) --Serial transmit --mc.mcCntlSetLastError(inst,"\nPort sending Data: ",""..Msg) DrivesComRead() --Serial Read if DataRead ~= "" then --Check response have data break end --exit loop Await response wx.wxMilliSleep(300) --await 300ms DrivesComSend("") --sent End Of Transmission --mc.mcCntlSetLastError(inst,"\nPort sending Data: ","") wx.wxMilliSleep(100) --await 300ms end ---------------------------------driver no response-------------------------------- FrameRead(""..Msg) --FrameRead decompose Frame if DataRead == "" then --If response have no data mc.mcCntlSetLastError(inst,"Drive Not Responding") return end -------------------------Check Checksum Response if data ~= nil-------------------------- if DataRead ~= "" then for resend = 1,3,1 do --3 times retransmision if Error Code FrameRead(DataRead) --FrameRead decompose Frame ChecksumVerify(DataRead) if ErrorCode == "A" and Verification == 1 --Check for error codes/checksum then break else --exit loop retransmision if no Error DrivesComSend(""..Msg) --Serial transmit --mc.mcCntlSetLastError(inst,"\nPort Sending Data: ",""..Msg) DrivesComRead() --Serial Read if ErrorCode ~= "A" then ErrorProcess(Station,ErrorCode) end --Process error code if Verification ~= 1 then --Detect Checksum Error mc.mcCntlSetLastError(inst,"Drive "..Station,"Response Checksum Error") --Show error end end end end end ---------------------------------------------------Append Checksum----------------------------------------------------- function ChecksumAppend(Data) ChecksumCalculate(Data) Data = Data..checksum --mc.mcCntlSetLastError(inst,"\nSendData:",Data) end ----------------------------------------------------Verify Checksum---------------------------------------------------- function ChecksumVerify(Data) ChecksumCalculate(Data) FrameRead(Data) if(checksum == ChecksumCode)then Verification = 1 else verification = 0 --mc.mcCntlSetLastError(inst,"Calculated: "..checksum, "ChecksumCode: "..ChecksumCode, "Verification: "..Verification) end end -------------------------------------------------------Get ABS Position------------------------------------------------ function ABSGetPosition() mc.mcCntlSetLastError(inst,"ABS GET POSITION CALL") GetPosReady = 0 ServoABSPos = {[0] = Drive0POS, [1] = Drive1POS, [2] = Drive2POS, [3] = Drive3POS, [4] = Drive4POS} for Servo=0,4,1 do Data = Servo.."0291" ChecksumCalculate(Data) Data = Data..checksum Comm_Drives(Data) --Call to communition fuction FrameRead(DataRead) if ErrorCode == "A" and Verification == 1 then ServoABSPos[Servo] = StrData * 0.005 --1/0.005 = 200 pulses * mm -- Save ABS Position to Array --print("\nMotor "..Servo," ABS Counts: "..StrData, ServoABSPos[Servo].."mm") mc.mcCntlSetLastError(inst,"\nMotor "..Servo," ABS Position: "..StrData, ABSPos.."mm") --Print Data Received GetPosReady = 1 else GetPosReady = 0 ErrorProcess(Servo,"G") --Show communication error break end end end --------------------------------------------SERVO LOCK REQUEST--------------------------------------------- function Servolock() for Servo=0,4,1 do Data = Servo.."90001EA5" ChecksumCalculate(Data) Data = Data..checksum ChecksumVerify(Data) Comm_Drives(Data) --Call to communition fuction FrameRead(DataRead) if ErrorCode == "A" and Verification == 1 then --mc.mcCntlSetLastError(inst,"\nMotor "..Servo.." Locked") --Print Data Received end end end -------------------------------------------SERVO UNLOCK REQUEST-------------------------------------------- function ServoUnlock() for Servo=0,4,1 do Data = Servo.."90101EA5" ChecksumCalculate(Data) Data = Data..checksum Comm_Drives(Data) --Call to communition fuction FrameRead(DataRead) if ErrorCode == "A" and Verification == 1 then --mc.mcCntlSetLastError(inst,"\nMotor "..Servo.." Unlocked") --Print Data Received end end end ------------------------------------------GANTRY Y2 MOTOR SQUARE------------------------------------------- --Update motor positions to servo positions/square the Y2 gantry --[[function AxisAlign() YAligned = 0 local YCorrect = Drive1POS + YOffset if local YCorrect < 0.5 then Servolock()--Lock Servo Receiving of commands mc.mcCntlGcodeExecute(inst, "G53 G00 X" .. tonumber(ServoABSPos[0]))-- Update axis X dro to position of motor 0 mc.mcCntlGcodeExecute(inst, "G53 G00 Y" .. tonumber(ServoABSPos[1]))-- Update axis Y dro to position of motor 1 mc.mcCntlGcodeExecute(inst, "G53 G00 Z" .. tonumber(ServoABSPos[2]))-- Update axis Z dro to position of motor 2 mc.mcCntlGcodeExecute(inst, "G53 G00 B" .. tonumber(ServoABSPos[4]))-- Update axis B dro to position of motor 4 rc = mc.mcAxisUnmapMotor(inst, 1, 3)--Description: Unmap the motor 3(Y2) from the axis 1(Y)(inst, axisId, motorId) rc = mc.mcAxisMapMotor(inst, 5, 3)--Description: Map motor 3 to axis 6(C) mc.mcCntlGcodeExecute(inst, "G53 G00 C" .. tonumber(ServoABSPos[3]))-- Update axis C dro to position of motor 3 ServoUnlock()--Unlock Servo to Receive commands mc.mcCntlGcodeExecute(inst, "G53 G01 C" .. tonumber(YCorrect) .. "f100")-- Move motor 3 mapped to axis 5(C) to Y corrected position rc = mc.mcAxisUnmapMotor(inst, 5, 3)--Description: Unmap the motor 3 from the axis 5(C) rc = mc.mcAxisMapMotor(inst, 1, 3)--Description: Map a motor 3 to an axis 1(Y2) YAligned = 1 else mc.mcCntlSetLastError(inst,"Exessive position error Y2\n","Y ABS: "..ServoABSPos[1],"\n","Y2 ABS: "..ServoABSPos[3],) end end ]] -------------------------------------------------------Get Current Alarm-------------------------------------------- function GetCurrentAlarm() --Get Current Alarm Servo={0,1,2,3,4} for Servo=0,4,1 do Data = Servo.."0200" ChecksumCalculate(Data) Data = Data..checksum Comm_Drives(Data) FrameRead(DataRead) if string.format("%04x",StrData) == "00ff" then break else mc.mcCntlSetLastError(inst,"Motor",Servo," Alarm Code:",StrData) --Print Data Received end end end ---------------------------------------------Error Processing---------------------------------------- function ErrorProcess(Drive,ErrorCode) if ErrorCode == "a" then mc.mcCntlSetLastError(inst, "Drive ", Drive ,string.format("Alarm")) elseif ErrorCode == "B" or ErrorCode == "b" then mc.mcCntlSetLastError(inst, "Drive ", Drive, string.format("Parity error")) elseif ErrorCode == "C" or ErrorCode == "c" then mc.mcCntlSetLastError(inst, "Drive ", Drive, string.format("Checksum error")) elseif ErrorCode == "D" or ErrorCode == "d" then mc.mcCntlSetLastError(inst, "Drive ", Drive, string.format("Character error")) elseif ErrorCode == "E" or ErrorCode == "e" then mc.mcCntlSetLastError(inst, "Drive ", Drive, string.format("Command error")) elseif ErrorCode == "F" or ErrorCode == "f" then mc.mcCntlSetLastError(inst, "Drive ", Drive, string.format("Data No. error")) elseif ErrorCode == "G" then mc.mcCntlSetLastError(inst, "Fail Drive "..Drive.." Get Position Communication Fault") end end