
EnableDisableButtons
DecelerateAndStopTest
Return

Label TestCommunication
	
Return

Label WaitGoodCommunication
	
Return

Label EnableDisableButtons
	
Return

' ===============================================
'  INITIALIZATIONS
' ===============================================

Label InitSlots
	Variable Numeric IS1
	IS1 = IS1 * 100
	Variable Numeric S_ActualPos     = IS1 + 10
	Variable Numeric S_ActualSpeed   = IS1 + 18
	'Variable Numeric S_ActualCurrent = IS1 + 20
	Variable Numeric S_ActualVoltage = IS1 + 22
	Variable Numeric S_ActualTemp    = IS1 + 24
	Variable Numeric s_WriteProt     = IS1 + 50
	Variable Numeric s_WorkingMode   = IS1 + 51
	Variable Numeric s_GoalPos       = IS1 + 58
	Variable Numeric s_GoalTorque    = IS1 + 62
	Variable Numeric s_GoalAcc       = IS1 + 64
	Variable Numeric s_GoalSpeed     = IS1 + 66
	Variable Numeric s_PidP          = IS1 + 68
	Variable Numeric s_PidI          = IS1 + 69
	Variable Numeric s_PidD          = IS1 + 70
	Variable Numeric s_DeadBand      = IS1 + 71
	Variable Numeric s_ZeroCurrent   = IS1 + 72
	
	Variable Numeric s_CurrentADC  = 2
	Variable Numeric s_TorqueADC   = 13
	
	Variable Numeric s_Current     = 20
	Variable Numeric s_Newton      = 21
	Variable Numeric s_ElecW       = 22
	Variable Numeric s_MecW        = 23
	Variable Numeric s_Efficiency  = 24
	Variable Numeric s_FactorNmW   = 25
Return

Label InitAllParams
	Slot s_GoalTorque = 0
	Slot s_GoalAcc = 0
	Slot s_GoalSpeed = 0
	Slot s_PidP = 0
	Slot s_PidI = 0
	Slot s_PidD = 0
	Slot s_DeadBand = 0
	Slot s_ZeroCurrent = 0
	Position = Slot(S_ActualPos)
	Destination = Position
	Slot(s_GoalPos) = Destination
	Slot s_GoalTorque = MaxTORQUE
	Slot s_GoalAcc = MaxACC
	Slot s_GoalSpeed = MaxRPM
	Slot s_PidP = PidKP
	Slot s_PidI = PidKI
	Slot s_PidD = PidKD
	Slot s_DeadBand = DeadBand
	Slot s_ZeroCurrent = ZeroSpeedCurrent
	Wait Seconds 0.1
	SetWorkingMode(1) ' 1 = Position
Return

' ------------- SET WORKING MODE 1=Position 2=Speed
Label SetWorkingMode
	Variable Numeric SWM1
	Slot(s_WriteProt) = 1   ' Write-Protection ON
	Wait Seconds 0.05
	Slot(s_WriteProt) = 0   ' Write-Protection OFF
	Wait Seconds 0.05
	Slot(s_WorkingMode) = 0 ' Working mode = 0
	Wait Seconds 0.05
	Slot(s_WorkingMode) = SWM1 ' Working mode
	Wait Seconds 0.05
	Slot(s_WriteProt) = 1   ' Write-Protection ON
	Wait Seconds 0.05
Return

' ------------------------------------- TORQUE
Label B_Torque
	Select Slot(s_GoalTorque)
		Case 10
			Slot(s_GoalTorque) = 20
		Case 20
			Slot(s_GoalTorque) = 50
		Case 50
			Slot(s_GoalTorque) = 100
		Case 100
			Slot(s_GoalTorque) = 200
		Case 200
			Slot(s_GoalTorque) = 500
		Case 500
			Slot(s_GoalTorque) = 1000
		Case 1000
			Slot(s_GoalTorque) = 2000
		CaseElse
			Slot(s_GoalTorque) = 20
	EndSelect
	ShowTorque
Return

Label ShowTorque
	
Return

' ------------------------------------- ACCELERATION
Label B_Acceleration
	Select Slot(s_GoalAcc)
		Case 20
			Slot(s_GoalAcc) = 100
		Case 100
			Slot(s_GoalAcc) = 1000
		Case 1000
			Slot(s_GoalAcc) = 10000
		CaseElse
			Slot(s_GoalAcc) = 20
	EndSelect
	ShowAcceleration
Return

Label ShowAcceleration
	
Return

' ------------------------------------- SPEED
Label B_Speed
	Select Slot(s_GoalSpeed)
		Case 500
			Slot(s_GoalSpeed) = 1000
		Case 1000
			Slot(s_GoalSpeed) = 1500
		Case 1500
			Slot(s_GoalSpeed) = 2000
		CaseElse
			Slot(s_GoalSpeed) = 500
	EndSelect
	ShowSpeed
Return

Label ShowSpeed
	
Return

' ------------------------------------- Newton Meter
Label B_NewtonMeter
	Select TestNm
		Case 2
			TestNm = 4
		Case 4
			TestNm = 6
		Case 6
			TestNm = 8
		CaseElse
			TestNm = 2
	EndSelect
	ShowNm
Return

Label ShowNm
	
Return


' ------------------------------------- MOTOR CONTROL
Variable Numeric MaxTORQUE
Variable Numeric MaxACC
Variable Numeric MaxRPM
Variable Numeric Position
Variable Numeric Destination
Variable Numeric PidKP
Variable Numeric PidKI
Variable Numeric PidKD
Variable Numeric DeadBand
Variable Numeric ZeroSpeedCurrent



' ------------------------------------- MOVE BUTTONS
Label +10_turns
	Destination = Destination + 3600
Return
Label -10_turns
	Destination = Destination - 3600
Return
Label +100_turns
	Destination = Destination + 36000
Return
Label -100_turns
	Destination = Destination - 36000
Return

Label Goto-Zero
	Destination = 0
Return


' ------------------------------------- KEYS
Label ReadKeys
	Variable Numeric JogSpeed
	JogSpeed = 1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 10
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.1
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.01
	EndSelect
	If Key("UP")
		Destination = Destination + JogSpeed
	EndIf
	If Key("DOWN")
		Destination = Destination - JogSpeed
	EndIf
Return





' ===============================================
'  MEASURE MOTOR PARAMETERS
' ===============================================

' -------------------------------------------
'  TARATURA CELLA DI CARICO IN NEWTON/METRO
' -------------------------------------------
'  - Fare prima la tara
'  - Aggiungere 1020 grammi a 20 centimetri
'  - Regolare il K_Newton per ottenere 2
' -------------------------------------------

Variable Numeric K_Current = 0.0045 ' 0.00375 teoric with 0.04 ohm
Variable Numeric K_Newton = 1.01

Variable Numeric TorqueCellZero
Variable Numeric NewtonMeters
Variable Numeric Velocity
Variable Numeric Voltage
Variable Numeric Current
Variable Numeric Temperature
Variable Numeric ElectricalWatt
Variable Numeric MechanicalWatt
Variable Numeric FactorNmW
Variable Numeric Efficiency

Label ResetNewton
	TorqueCellZero = Slot(s_TorqueADC)
Return

Label MeasureCurrentAndTorque
	' -------------------------------------------------------- TORQUE
	NewtonMeters = K_Newton * (Slot(s_TorqueADC) - TorqueCellZero)
	Slot(s_Newton) = NewtonMeters
	'Button B_TorqueNm Text "Torque " + Format(NewtonMeters, "0.0") + " N/m"
	' -------------------------------------------------------- SPEED
	Velocity = Slot(S_ActualSpeed )
	'Button B_Velocity Text "Speed " + Velocity + " RPM"
	' -------------------------------------------------------- VOLTAGE
	Voltage = Slot(S_ActualVoltage)
	'Button B_Voltage Text "Voltage " + Format(Voltage, "0.0") + " V"
	' -------------------------------------------------------- CURRENT
	Current = K_Current * Slot(s_CurrentADC)
	Slot(s_Current) = Current
	'Button B_Current Text "Current " + Format(Current, "0.00") + " A"
	' -------------------------------------------------------- ELEC. POWER
	ElectricalWatt = Slot(S_ActualVoltage) * Current
	Slot(s_ElecW) = ElectricalWatt
	'Button B_PowerElec Text "Pow Elec. " + Format(ElectricalWatt, "0") + " W"
	' -------------------------------------------------------- MEC. POWER
	MechanicalWatt = NewtonMeters / 41 * 2 * Math_PI * Slot(S_ActualSpeed) / 60
	MechanicalWatt = Max(MechanicalWatt, 0)
	Slot(s_MecW) = MechanicalWatt
	'Button B_PowerMec Text "Pow Mecc. " + Format(MechanicalWatt, "0") + " W"
	' -------------------------------------------------------- EFFICIENCY
	Efficiency = 100 * MechanicalWatt / ElectricalWatt
	Slot(s_Efficiency) = Efficiency
	'Button B_Efficiency Text "Efficiency " + Format(Efficiency, "0") + " %"
    ' -------------------------------------------------------- Factor NmW
	FactorNmW = Max(Abs(NewtonMeters) / ElectricalWatt, 0)
	Slot(s_FactorNmW) = FactorNmW
	'Button B_FactorNmW Text Format(FactorNmW, "0.000") + " Nm/W"
	' -------------------------------------------------------- TEMPERATURE
	Temperature = Slot(S_ActualTemp)
	'Button B_Temperature Text "Temp. " + Temperature + " °C"
Return


' ===============================================
'  DECELERATE AND STOP
' ===============================================
Label DecelerateAndStop
	Destination = Position
	DecelerateAndStopFlag = 1
Return

Variable Numeric DecelerateAndStopFlag

Label DecelerateAndStopTest
	If DecelerateAndStopFlag = 1
		If Abs(Slot(S_ActualSpeed)) < 10
			DecelerateAndStopFlag = 0
			Destination = Position
		EndIf
	EndIf
Return

' ===============================================
'  TEST FUNCTIONS
' ===============================================

' ------------------------------------- TEST CONTROL
Variable Numeric TestNm

Label LongTest
	For v1 = 0 To 100
	Next
Return


'BOOKMARKS 

'EDITEDLINES 

'EXEC K_Newton = 1.01
