' ╔═════════════════════════════════════════════╗
' ║ TORQUE METER                                ║
' ╚═════════════════════════════════════════════╝

' ------------------------------------- TEST PARAMETERS
TestNm = 2
MaxTemp = 70
MinTemp = 40

' ------------------------------------- MOTOR PARAMETERS
MaxTORQUE = 100
MaxACC =   1000
MaxRPM =   1000
PidKP  =   50
PidKI  =   50
PidKD  =    0
DeadBand =  5
ZeroSpeedCurrent = 0

Kreduction = 41
MotorID = 1

' ------------------------------------- BUTTONS and KEYS
Button 1 Label B_Torque
Button 2 Label B_Acceleration
Button 3 Label B_Speed

Button 5 Label B_NewtonMeter
Button 6 Label Long_Test
Button Long_Test Color Gold

Button 10 Label +10_turns
Button +10_turns Color Gold
Button 11 Label -10_turns
Button -10_turns Color Gold
Button 12 Label +100_turns
Button +100_turns Color Gold
Button 13 Label -100_turns
Button -100_turns Color Gold
Button 14 Label Goto-Zero
Button Goto-Zero Color Orange
Button 15 Label DecelerateAndStop
Button DecelerateAndStop Color Orange

Button 17 Label B_ResetNewton
Button B_ResetNewton Text "Reset N/m"
Button B_ResetNewton Color LightGreen
Button 18 Label B_TorqueNm
Button 19 Label B_Velocity
Button 20 Label B_Voltage
Button 21 Label B_Current
Button 22 Label B_PowerElec
Button 23 Label B_PowerMec
Button 24 Label B_Efficiency
Button 25 Label B_FactorNmW
Button 26 Label B_Temperature

Key Up   ReadKeys
Key Down ReadKeys

' ------------------------------------- APPS
Load "Theremino_Hal.exe"
Load CloseApps "Theremino_Cobot.exe"
Load "Theremino_Motors.exe"
Load "Theremino_SlotViewer1.exe"
Load CloseApps "Theremino_SlotViewer2.exe"
Load CloseApps "Theremino_SlotViewer3.exe"
Load "Theremino_SignalScope1.exe"
Load "Theremino_SignalScope2.exe"
Load "Theremino_SignalScope3.exe"
Load "Theremino_Logger12.exe"


' ------------------------------------- START
Option Speed 9
InitSlots(MotorID)
WaitGoodCommunication

Destination = Position
UpdateMotorValues

ResetNewton
ShowTorque
ShowAcceleration
ShowSpeed
ShowNm

' ------------------------------------- LOOP
Gosub Loop
Stop

' ------------------------------------- STOP
Label EventStop
	StopMotor
End

Label StopMotor
	Destination = Position
	UpdateMotorValues
	SetWorkingMode(0)
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
	Button B_Torque Text "Torque = " + Slot(s_GoalTorque)
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
	Button B_Acceleration Text "Accel. = " + Slot(s_GoalAcc)
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
	Button B_Speed Text "RPM = " + Slot(s_GoalSpeed)
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
	Button B_NewtonMeter Text "Nm. = " + TestNm
Return


' ------------------------------------- MOTOR CONTROL
Variable Numeric MotorID
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
Variable Numeric Kreduction

Label MoveBy
	Variable Numeric MB1
	Destination = Destination + MB1
	UpdateMotorValues
Return


' ------------------------------------- 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


' ===============================================
'  LOOP
' ===============================================
Label Loop
	UpdateMotorValues
	PrintValues
	MeasureCurrentAndTorque
	Wait Seconds 0.01
	TestCommunication
	EnableDisableButtons
	DecelerateAndStopTest
Goto Loop

Label UpdateMotorValues
	Slot(s_GoalPos) = Destination
	Position = Slot(S_ActualPos)
Return

Label PrintValues
	s1 = "Destination: " + Format(Destination, "0.00")
	s1 = s1 + "   Position: " + Format(Position, "0.00")
	Print s1
Return

Label TestCommunication
	If Slot(56) < 10
		WaitGoodCommunication
	EndIf
Return

Label WaitGoodCommunication
	For v1 = 1 To Infinity
		If Slot(56) > 50
			v2 = v2 + 1
		Else
			v2 = 0
		EndIf
		If v2 > 50
			Beep "440-80,0-320,330-80,440-80,660-80"
			InitAllParams
			Exit
		EndIf
		Wait Seconds 0.01
	Next
Return

Label EnableDisableButtons
	If Abs(Destination - Position) > 1
		Button B_Torque Disabled
		Button B_Acceleration Disabled
		Button B_Speed Disabled
	Else
		Button B_Torque Enabled
		Button B_Acceleration Enabled
		Button B_Speed Enabled
	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
Variable Numeric MaxTemp
Variable Numeric MinTemp

Label LongTest
	For v1 = 0 To 100
	Next
Return


'BOOKMARKS 364 358 354 

'EDITEDLINES 

'EXEC K_Newton = 1.01
