#include "rs485header.as"	;RS485 header and module
#include "exobject.as"									;extension object macro
#include "rs485comm.as"									;RS485 communication module
#include "colorfulobject.as"							;Colorful object Module
#include "ExeWrightMod.hsp"								;Execution file Wright Module
#include "hgimg3.as"


RS485ComNum=3
RS485BaudRate=7


;/ʂ̏
;--------+---------+---------+---------+---------+---------+---------+---------+
; ʃTCYA^CgAtHgAwiACAbZ[W
;--------+---------+---------+---------+---------+---------+---------+---------+
;/XN[̏
	title2 "RS485_Motion"				;^Cg\
	font msgothic,14 : objmode 2		;tHg"lr SVbN"
;--------+---------+---------+---------+---------+---------+---------+---------+
;/ʐMݒ
; ʐM|[gX^eBbNRg[
	pos 20,20
	mes "ʐM|[g"
; ʐM|[gR{{bNX
	cmbobj 90,15,60,20,RS485ComNum,80,"COM 1\nCOM 2\nCOM 3\nCOM 4\nCOM 5\nCOM 6\nCOM 7\nCOM 8\nCOM 9\nCOM10",ComNumcmbID
; ʐMxX^eBbNRg[
	pos 170,20
	mes "ʐM[g"
; ʐMxR{{bNX
	cmbobj 240,15,90,15,RS485BaudRate,80,"    9,600\n   14,400\n   19,200\n   28,800\n   38,400\n   57,600\n   76,800\n  115,200\n  153,600\n  230,400\n  460,800\n  691,200\n1,382,400",BaudRatecmbID
;--------+---------+---------+---------+---------+---------+---------+---------+
;/VA|[g̐ݒ
;--------+---------+---------+---------+---------+---------+---------+---------+
;/RS485<->USBRo[^(BTE061),PCI<->RS485 CONTEC COM-2PD(PCI)H̃hCo[`FbN
; hCo[COM3-10COM1ɖ߂
	Directory=dir_cur								;ÑJgfBNg
	do
		if RS485ComNum<=1 : _break
		chdir dir_sys+"\\drivers"			;\system32\driversɈړ
		dirlist reg,"ftser2k.sys"			;ftser2k.sys̃`FbN
		if reg="ftser2k.sys\n" : _break
		dirlist reg,"ComDrv2k.sys"			;ComDrv2k.sys̃`FbN
		if reg="ComDrv2k.sys\n" : _break
		p(pRS485ComNum)=0 : RS485ComNum=p(pRS485ComNum) : objprm ComNumcmbID,RS485ComNum 
	until
	chdir Directory									;ÑJgfBNgɖ߂

;/VA|[g̃I[v
	rs485open RS485ComNum, RS485BaudRate	;Timeout 100ms
;--------+---------+---------+---------+---------+---------+---------+---------+



;--------+---------+---------+---------+---------+---------+---------+---------+
;/C
;--------+---------+---------+---------+---------+---------+---------+---------+
		dim MasterServoID,10
		dim SlaveServoID,10

		MasterServoID=31,32,33,34		;}X^[T[{ID
		SlaveServoID =41,42,43,44		;X[uT[{ID
		
		;X[üړ̖ڕWԁB0ƓMNVNB傫ƒǂɂB
		TargetTime=5
				
		;C[`̑ҋ@ԁBƉǓMNVNB傫Ɗ炩ɓǔxB
		SamplingRate=5

		;}X^[ɗ͂Ԃꍇ臒l
		SlaveLoadBoarder = 800
	
		;}X^[̃gNOFF
		repeat 4
			SetTorqueOFF MasterServoID(cnt)
		loop

		;X[ũgNON
		repeat 4
			SetTorqueON SlaveServoID(cnt)
		loop

		;}X^[̃RvCAXX[vAgNݒ
		repeat 4
			SetCWComplianceSlope MasterServoID(cnt),20
			SetCCWComplianceSlope MasterServoID(cnt),20
			SetMaxTorque MasterServoID(cnt),100
		loop
		
		;}X^[̃RvCAX}[Wݒ
		repeat 4
			SetCWComplianceMargin MasterServoID(cnt),50
			SetCCWComplianceMargin MasterServoID(cnt),50
		loop

		;炪C[`B
		repeat -1

			;}X^[˃X[u
			repeat 4
	
				;}X^[݈̌ʒu擾
				PresentPosition(MasterServoID(cnt))=GetPresentPosion(MasterServoID(cnt))

				;X[u̖ڕWʒuݒ
				TargetPosition(SlaveServoID(cnt))=PresentPosition(MasterServoID(cnt))
;				TargetPosition(SlaveServoID(cnt))=-1*PresentPosition(MasterServoID(cnt))	;]]ꍇ
	
				;ڕWʒȗM
				SetPositionTime  SlaveServoID(cnt),TargetPosition(SlaveServoID(cnt)),TargetTime,0
				await 1
			loop

			;X[u˃}X^[
			repeat 4

				;X[u̕׎擾
				PresentLoad(SlaveServoID(cnt))=GetPresentLoad(SlaveServoID(cnt))

				;X[uׂ̕傫gNON
				if PresentLoad(SlaveServoID(cnt)) > SlaveLoadBoarder {

					SetTorqueON MasterServoID(cnt)

					;X[ǔ݈ʒu擾
					PresentPosition(SlaveServoID(cnt))=GetPresentPosion(SlaveServoID(cnt))

					;}X^݈̌ʒuƃX[ǔ݈ʒur
					if PresentPosition(MasterServoID(cnt)) > PresentPosition(SlaveServoID(cnt))	{
						SetCWComplianceSlope MasterServoID(cnt),255		;Փˉ̃RvCAXX[vE}[W
						SetCWComplianceMargin MasterServoID(cnt),255
						SetCCWComplianceSlope MasterServoID(cnt),150-150*PresentLoad(SlaveServoID(cnt))/10000	;Փ˂̃RvCAX
						}
					else {		;Α

;						;Փˉ̃RvCAXX[vE}[W
						SetCCWComplianceSlope MasterServoID(cnt),255
						SetCCWComplianceMargin MasterServoID(cnt),255
;
;						;Փ˂̃RvCAX
						SetCWComplianceSlope MasterServoID(cnt),150-150*PresentLoad(SlaveServoID(cnt))/10000
					}
				}
				else {
					SetTorqueOFF MasterServoID(cnt)
 					SetCWComplianceSlope MasterServoID(cnt),2
 					SetCCWComplianceSlope MasterServoID(cnt),2
					setCWComplianceMargin MasterServoID(cnt),1
					setCCWComplianceMargin MasterServoID(cnt),1
					}

				await 1
			loop
			

			;̑܂łƋxeB
			await SamplingRate
	
		loop
		;C[`IB





