Commit a580555e authored by Ad Bos's avatar Ad Bos
Browse files

Initial commit

parents
//TMCL disassembly at 09-06-2019 14:12:24
ROR 3, 0
Lbl1: SIO 0, 0, 0
GGP 0, 2
COMP 0
JC ZE, Lbl11
JC GT, Lbl6
Lbl6: JC LT, Lbl9
CSUB Lbl13
SGP 0, 2, 0
Lbl9: JA Lbl11
CSUB Lbl23
Lbl11: SGP 0, 2, 0
JA Lbl1
Lbl13: STOP
GAP 154, 0
COMP 0
JC EQ, Lbl22
CALC ADD, -1
AAP 154, 0
GAP 3, 0
CALC DIV, 2
AAP 2, 0
Lbl22: AAP 3, 0
Lbl23: RSUB
GAP 154, 0
COMP 13
JC GE, Lbl32
CALC ADD, 1
AAP 154, 0
GAP 3, 0
CALC MUL, 2
AAP 2, 0
Lbl32: AAP 3, 0
RSUB
GGP 9, 2
CALC MUL, -1
AGP 9, 2
COMP 0
JC LT, Lbl45
GGP 2, 2
CALC SUB, 1
AGP 4, 2
GGP 3, 2
CALC DIV, 2
AGP 5, 2
Lbl45: JA Lbl49
GGP 2, 2
AGP 4, 2
GGP 3, 2
Lbl49: AGP 5, 2
GGP 4, 2
AAP 154, 0
GGP 5, 2
AAP 2, 0
COMP 0
JC GE, Lbl56
Lbl56: CALC MUL, -1
AAP 4, 0
GGP 6, 2
COMP 0
JC NE, Lbl62
GGP 5, 2
Lbl62: AAP 3, 0
RETI
STOP
// Program for Azimuth axis
// User variable 0 = position setpoint in 0,01 degree
// User variable 1 = actual position in 0,01 degree
// User variable 2 = position setpoint Change of state register in 0,01 degree
// User variable 3 = position setpoint in steps
// User variable 5 = operation mode select 0 1 2 3 4 5
// User variable 6 = buffered mode select Change of state register 0 1 2 3 4 5
// User variable 11 = Reference position Known flag
// User variable 12 = homing sequence register
SIO 0, 0, 0 // disable pull-up on all inputs
SIO 255, 2, 0 // Set all outputs to ZERO
SAP 140, 0, 5 // set Microstep Resolution to 32 steps
SAP 210, 0, 12801 // set Encoder prescaler to match
// Encoder direction reversed because motorwiring has been reversed
SAP 12, 0, 1 // set Right limit switch disable to 1 ( disable )
SAP 13, 0, 1 // set Left limit switch disable to 1 ( disable )
SAP 149, 0, 1 // set Soft stop flag to 1 ( no hard stop )
VECT 27, Capture // Define Capture position routine
EI 255 // Enable global interrupts
SGP 5, 2, 0 // set User variable 5 Start up in mode 0
SGP 11, 2, 0 // set User variable 11 to Zero ( Ref. Known flag )
SGP 12, 2, 0 // set User variable 12 to Zero ( homing sequence )
CSUB Convert_uSteps_to_Degree
AGP 0, 2 // accu to User variable 0 position setpoint in 0,01 degree
// make sure position setpoint is equal to actual position
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MainLoop:
CSUB Convert_uSteps_to_Degree
GGP 5, 2 // get User variable 5 ( Mode selection )
CALCX LOAD // Store user variable 5 in register X
GGP 6, 2 // get User variable 6 ( Mode select change of state )
CALCX SUB // Subtract variable 5 from variable 6
COMP 0 // Check if result = zero
JC ZE , No_Change // when zero end routine
MST 0 // Stop motor when mode selection changes
No_Change:
CALCX SWAP // Copy X register into Accu ( user variable 5 )
AGP 6, 2 // Copy User Var 5 to User variable 6
COMP 2 // Check if we are busy finding the reference position
JC EQ, CONTINUE
SGP 12, 2, 0 // set User variable 12 to Zero homing sequence
CONTINUE:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 0
JC NE, Check_1
CSUB MODE_0 // Stop motor and set the brake
Check_1:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 1
JC NE, Check_2
CSUB MODE_1 // Release the brake
GIO 1, 2 // READ Status Output 1 (Brake )
COMP 1
JC NE , Complete // If brake not free then preform no further action
// Modes 2 , 3 and 4 are not allowed when the brake is not released
Check_2:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 2
JC NE, Check_3
SAP 154, 0, 2 // set Pulse divisor to 2
SAP 4, 0, 1573 // set Maximum speed [int] to 900 RPM = 10 degree/sec
SAP 153, 0, 11 // set Ramp divisor to 11
SAP 5, 0, 1648 // set Maximum acceleration [int] to 1.0 sec
CSUB MODE_2 // Find reference position
Check_3:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 3
JC NE, Check_4
SAP 154, 0, 3 // set Pulse divisor to 3
SAP 4, 0, 1573 // set Maximum speed [int] to 450 RPM = 5 degree/sec
SAP 153, 0, 12 // set Ramp divisor to 12
SAP 5, 0, 1648 // set Maximum acceleration [int] to 2.0 sec
CSUB MODE_3 // Manual control ( Jog forward and Jog reverse )
Check_4:
GGP 11, 2 // get User variable 11 Reference position Known flag
COMP 1
JC NE , Complete // mode 4 operation is not allowed when reference is unknown
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 4
JC NE, NEQ_4
SAP 154, 0, 2 // set Pulse divisor to 2
SAP 4, 0, 1573 // set Maximum speed [int] to 900 RPM = 10 degree/sec
SAP 153, 0, 11 // set Ramp divisor to 11
SAP 5, 0, 1648 // set Maximum acceleration [int] to 1.0 sec
CSUB MODE_4 // Move to position as specified in 0,01 degres in user variable 0
JA Check_5
NEQ_4:
GGP 0, 2 // get User variable 0 ( Setpoint in 0,01 degree )
AGP 2, 2 // Setpoint change of state = Setpoint
Check_5:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 5
JC NE, Complete
CSUB MODE_5 // ?????????????????????????
Complete:
JA MainLoop
STOP
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 00000000000000000000000000000000000000000000000000000000000000000000
MODE_0: // Stops Motor and sets the brake
GIO 1, 2 // READ Status output 1 (Brake )
COMP 1
JC NE , End_0
MST 0
GAP 3, 0 // get Actual speed [int]
COMP 0
JC NE , END_0
SIO 1, 2, 0 // change Output 1 to zero ( Brake )
END_0:
RSUB
// 111111111111111111111111111111111111111111111111111111111111111111111
MODE_1: // Releases the brake
GIO 1, 2 // READ Status output 1 (Brake )
COMP 1
JC EQ , Brake_Free
SIO 1, 2, 1 // Set Output 1 to 1 ( Release Brake )
WAIT TICKS, 0, 30 // wait 300 msec
Brake_Free:
RSUB
// 2222222222222222222222222222222222222222222222222222222222222222222222
MODE_2: // Find the reference position
GGP 12, 2 // get User variable 12 homing sequence
COMP 20
JC GE , No_Action_2
COMP 15
JC EQ , Step_15
COMP 10
JC EQ , Step_10
COMP 9
JC EQ , Step_9
COMP 8
JC EQ , Step_8
COMP 7
JC EQ , Step_7
COMP 6
JC EQ , Step_6
COMP 5
JC EQ , Step_5
COMP 4
JC EQ , Step_4
COMP 3
JC EQ , Step_3
COMP 2
JC EQ , Step_2
COMP 1
JC Eq , Step_1
// Initialising reference search parameters
SCO 0, 0, 0 // set Coordinate 0 to 0 stores high-low transition
SCO 1, 0, 0 // set Coordinate 1 to 0 stores low-high transition
SGP 11, 2, 0 // set User variable 11 to Zero ( Ref. Known flag )
SGP 12, 2, 1 // set User variable 12 homing sequence to 1
CSUB Add_Switch_Status // Result in variable 12 will be 1 or 2
RSUB
Step_1: // Limit switch is Off. So find it within 360 degrees
MVP REL, 0, -3456000 // incremental move - 360 degrees
SGP 12, 2, 3 // set User variable 12 homing sequence to 3
RSUB
Step_2: // Limit switch is On. Move away 15 degrees
MVP REL, 0, 144000 // incremental move 15 degrees
SGP 12, 2, 15 // set User variable 12 homing sequence to 15
RSUB
Step_3: // Check if we are at speed ( Limit switch must always be activated at same speed )
GAP 4, 0 // get Maximum speed [int]
CALCX LOAD // Store Maximum speed in X register
GAP 3, 0 // get Actual speed [int]
COMP 0
JC GE , Positive_Speed
CALC MUL , -1 // Reverse sign of actual speed
Positive_Speed:
CALCX SUB // Subtract maximum speed from actual speed
COMP 0 // zero difference means motor is at requested speed
JC NE, No_Action_2 // when not at speed continue checking
SGP 12, 2, 4 // set User variable 12 homing sequence to 4
CSUB Add_Switch_Status // Result in variable 12 will be 4 or 5
RSUB
Step_4: // Enable the switch interrupt
SGP 27, 3, 3 // set Left limitswitch trigger transition to low-high high-low
EI 27 // Capture the transitions of the Left limit switch
SGP 12, 2, 6 // set User variable 12 homing sequence to 6
RSUB
Step_5: // While at speed the Limit switch is On. So stop and continue in step 2
MST 0
GAP 3, 0 // get Actual speed [int]
COMP 0 // motor stopped
JC NE, No_Action_2
SGP 12, 2, 1 // set User variable 12 homing sequence to 1
CSUB Add_Switch_Status // Result in variable 12 will be 1 or 2
RSUB
// The capture interupt routine will increment the homing sequence register
Step_6: // Wait for transition Low to High
Step_7: // Wait for transition High to Low
GAP 8, 0 // get Position reached flag
COMP 1
JC EQ , Error // when at end of move goto error
RSUB
Step_8: // Calculate midpoint between transitions and move to that position
DI 27 // Disable interupt 27 for left limit switch
GCO 0, 0 // Get coordinate 0 High - Low transition\
CALCX SWAP // Store High - Low transition in X register
GCO 1, 0 // Get coordinate 1 Low - High transition
CALCX ADD // Calculate sum of transitions
CALC DIV , 2 // Calculate MIDpoint between transitions
Calc ADD , 12345 // Hardcoded offset to referenceposition
ACO 2, 0 // Store result in Coordinate 2
MVP COORD, 0, 2 // Move to Coordinate 2
SGP 12, 2, 9 // set User variable 12 homing sequence to 9
RSUB
Step_9: // wait for the position reached flag
GAP 8, 0 // get Position reached flag
COMP 1
JC NE , No_Action_2 // wait for end of motion
SGP 12, 2, 10 // set User variable 12 homing sequence to 10
RSUB
Step_10: // Update position registers with the reference position
SAP 138, 0, 2 // set Ramp mode to 2 ( velocity mode ) to prevent any
// motion occurring when modifying the position registers
SAP 1, 0, 0 // set Actual position [steps]
SAP 0, 0, 0 // set Target position [steps]
SAP 209, 0, 0 // set Encoder position [steps]
SGP 11, 2, 1 // set User variable 11 to 1 ( Ref. Known flag )
SGP 12, 2, 20 // set User variable 12 to 20 homing sequence
RSUB
Step_15: // While moving off the limitswitch wait for position reached flag
GAP 8, 0 // get Position reached flag
COMP 1
JC NE , No_Action_2
GAP 11, 0 // get Left limit switch status
COMP 1
JC EQ , Error
SGP 12, 2, 1 // set User variable 12 homing sequence to 1
RSUB
ERROR:
DI 27 // Disable interupt 27 for left limit switch
SGP 12, 2, 50 // set User variable 12 to 50 ERROR homing sequence
CSUB Add_Switch_Status
// 50 = Left switch always off 51 = Left switch always on
RSUB
No_Action_2:
RSUB
// 3333333333333333333333333333333333333333333333333333333333333333333333
MODE_3: // Manual operation
// Input 0 and Input 1 are both two digital aswell as two analog inputs
// they are interconnected on the board.
// When using 24 V digital signals the analog values will simply saturate at 1023
GIO 0, 1 // read Analog Input 0
AGP 0, 2 // accu to User variable 0
COMP 20 // No input active ( use minimum level of 20 to filter out noise )
JC GT, Jog_Reverse
GIO 1, 1 // read Analog Input 1
AGP 0, 2 // accu to User variable 0
COMP 20 // No input active ( use minimum level of 20 to filter out noise )
JC GT, Jog_Forward
GAP 3, 0 // get Actual speed [int]
COMP 0
JC EQ , No_Action_3
MST 0 // Stop the motor
JA No_Action_3
Jog_Forward:
CSUB Calc_Speed
MVP ABS, 0, 2592000 // move to max. position of 270,00 degree
JA No_Action_3
Jog_Reverse:
CSUB Calc_Speed
MVP ABS, 0, -2592000 // move to min. position of -270,00 degree
No_Action_3:
RSUB
// 4444444444444444444444444444444444444444444444444444444444444444444444
MODE_4: // Move to Position as specified in user variable 0
GGP 0, 2 // get User variable 0 ( Setpoint in 0,01 degree )
CALCX LOAD // Store user variable 0 in register X
GGP 2, 2 // get User variable 2 ( Setpoint change of state )
CALCX SUB // Subtract variable 0 from variable 2
COMP 0 // Check if result = zero
JC ZE , Not_Changed // when zero end routine
CSUB Convert_Degree_to_uSteps
Not_Changed:
RSUB
// 55555555555555555555555555555555555555555555555555555555555555555555555
MODE_5: // ??????????????????????????????????
RSUB
// =========================================================================
Convert_uSteps_to_Degree:
GAP 209, 0 // get Encoder position [steps]
CALC DIV, 96 // 9600 steps per degree
AGP 1, 2 // accu to User variable 1 position in 0,01 degree
RSUB
Convert_Degree_to_uSteps:
GGP 0, 2 // get User variable 0 ( Setpoint in 0,01 degree )
CALC MOD , 36000 // Limit the input to + - 360 degrees
COMP 27000 // upper limit of 270,00 degree
JC LE, Check_Low_Limit
CALC SUB, 36000 // subtract 360,00 degree
AGP 0, 2 // set User variable 0 to desired setpoint - 360,00 degree
JA CONVERT
Check_Low_Limit:
COMP -27000 // lower limit of -270,00 degree
JC GE, CONVERT // If value >=
CALC ADD, 36000 // add 360,00 degree
AGP 0, 2 // set User variable 0 to desired setpoint + 360,00 degree
CONVERT:
GGP 0, 2 // get User variable 0 ( Setpoint in 0,01 degree )
AGP 2, 2 // Setpoint change of state = Setpoint
CALC MUL, 96 // 9600 steps per degree
ACO 10, 0 // accu to User Coordinate 10 position in steps
AGP 3, 2 // accu to User variable 3 position in steps
MVP COORD, 0, 10 // move to position in coordinate 10
RSUB
Add_Switch_Status:
GGP 12,2 // Load user variable 12 into accumulator
CALCX LOAD // Load user variable 12 into X register
GAP 11, 0 // get Left limit switch status
CALCX ADD // Add status from left limitswitch
AGP 12, 2 // accu to User variable 12
RSUB
Calc_Speed:
GAP 4, 0 // get Maximum speed [int]
CALCX LOAD // Load maximum speed into X register
GGP 0, 2 // get User variable 0 ( stored analog value of input 0 or 1 )
CALCX MUL // Multiply max speed with stored analog value
CALC div , 1023 // Analog value ranges from 0 to 1023 ( 0 to 10.7 Volt )
AAP 4, 0 // accu to Maximum speed [int]
RSUB
Calc_Power_2:
SGP 132, 2, 0 // set User variable 132 to 0 Ticktimer
SGP 20, 2, 1 // set User variable 20 to 1 ( 2 to power of 0 )
Loop:
GGP 21, 2 // get User variable 21 ( power )
COMP 0
JC EQ , Ready
CALC SUB , 1
GGP 20, 2 // get User variable 20
CALC MUL , 2
JA Loop
Ready:
GGP 132, 2 // get User variable 132 to 0 Ticktimer
AGP 22,2 //
RSUB
Capture:
GGP 12, 2 // get User variable 12 homing sequence
CALC ADD ,1 // Add 1 to User variable 12
AGP 12, 2 // accu to User variable 12
GAP 11, 0 // get Left limit switch status
COMP 0
JC NE , High
CCO 0, 0 // Store position from high to low transition in Coordinate 0
RETI
High:
CCO 1, 0 // Store position from low to high transition in Coordinate 1
RETI
\ No newline at end of file
TMCL Symbol File (Azimuth_6a.sym 09-06-2019 18:00:14)
Label Address (dec)
------------------------------------------------------
MAINLOOP: 16
NO_CHANGE: 24
CONTINUE: 29
CHECK_1: 33
CHECK_2: 40
CHECK_3: 44
CHECK_4: 48
NEQ_4: 56
MODE_0: 60
END_0: 68
MODE_1: 69
BRAKE_FREE: 74
MODE_2: 75
STEP_1: 107
STEP_2: 110
STEP_3: 113
POSITIVE_SPEED: 119
STEP_4: 125
STEP_5: 129
STEP_6: 136
STEP_7: 136
STEP_8: 140
STEP_9: 151
STEP_10: 156
STEP_15: 163
ERROR: 171
NO_ACTION_2: 175
MODE_3: 176
JOG_FORWARD: 190
JOG_REVERSE: 193
NO_ACTION_3: 195
MODE_4: 196
NOT_CHANGED: 204
CONVERT_STEPS_TO_DEGREE: 205
CONVERT_DEGREE_TO_STEPS: 209
CHECK_LOW_LIMIT: 215
CONVERT: 219
ADD_SWITCH_STATUS: 226
CALC_SPEED: 232
CAPTURE: 239
HIGH: 247
// Program for Azimuth axis
// User variable 0 = position setpoint in 0,01 degree
// User variable 1 = actual position in 0,01 degree
// User variable 2 = position setpoint Change of state register in 0,01 degree
// User variable 3 = position setpoint in steps
// User variable 5 = operation mode select 0 1 2 3 4
// User variable 6 = buffered mode select Change of state register 0 1 2 3 4
// User variable 11 = Reference position Known flag
// User variable 12 = homing sequence register
SIO 255, 2, 0 // Set all outputs to ZERO
SAP 140, 0, 5 // set Microstep Resolution to 32 steps
SAP 210, 0, 12801 // set Encoder prescaler to match
// Encoder direction reversed because motorwiring has been reversed
SAP 154, 0, 2 // set Pulse divisor to 2
SAP 153, 0, 11 // set Ramp divisor to 11
SAP 5, 0, 412 // set Maximum acceleration [int] to 4 sec
SIO 0, 0, 0 // disable pull-up on all inputs
SAP 12, 0, 1 // set Right limit switch disable to 1 ( disable )
SAP 13, 0, 1 // set Left limit switch disable to 1 ( disable )
VECT 27, Capture // Define Capture position routine
EI 255 // Enable global interrupts
SGP 5, 2, 0 // set User variable 5 Start up in mode 0
SGP 11, 2, 0 // set User variable 11 to Zero ( Ref. Known flag )
SGP 12, 2, 0 // set User variable 12 to Zero ( homing sequence )
CSUB Convert_Steps_to_Degree
AGP 0, 2 // accu to User variable 0 position setpoint in 0,01 degree
// make sure position setpoint is equal to actual position
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
MainLoop:
CSUB Convert_Steps_to_Degree
GGP 5, 2 // get User variable 5 ( Mode selection )
CALCX LOAD // Store user variable 5 in register X
GGP 6, 2 // get User variable 6 ( Mode select change of state )
CALCX SUB // Subtract variable 5 from variable 6
COMP 0 // Check if result = zero
JC ZE , No_Change // when zero end routine
MST 0 // Stop motor when mode selection changes
No_Change:
CALCX SWAP // Copy X register into Accu ( user variable 5 )
AGP 6, 2 // Copy User Var 5 to User variable 6
COMP 2 // Check if we are busy finding the reference position
JC EQ, CONTINUE
SGP 12, 2, 0 // set User variable 12 to Zero homing sequence
CONTINUE:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 0
JC NE, Check_1
CSUB MODE_0 // Stop motor and set the brake
Check_1:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 1
JC NE, Check_2
CSUB MODE_1 // Release the brake
GIO 1, 2 // READ Status Output 1 (Brake )
COMP 1
JC NE , MainLoop // If brake not free then preform no further action
// Modes 2 , 3 and 4 are not allowed when the brake is not released
Check_2:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 2
JC NE, Check_3
CSUB MODE_2 // Find reference position
Check_3:
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 3
JC NE, Check_4
CSUB MODE_3 // Manual control ( Jog forward and Jog reverse )
Check_4:
GGP 11, 2 // get User variable 11 Reference position Known flag
COMP 1
JC NE , Mainloop // mode 4 operation is not allowed when reference is unknown
GGP 6, 2 // get User variable 6 ( buffered Mode selection )
COMP 4
JC NE, NEQ_4
CSUB MODE_4 // Move to position as specified in 0,01 degres in user variable 0
JA MainLoop
NEQ_4:
GGP 0, 2 // get User variable 0 ( Setpoint in 0,01 degree )
AGP 2, 2 // Setpoint change of state = Setpoint
JA MainLoop
STOP
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 00000000000000000000000000000000000000000000000000000000000000000000
MODE_0: // Stops Motor and sets the brake
GIO 1, 2 // READ Status output 1 (Brake )
COMP 1
JC NE , End_0
MST 0
GAP 3, 0 // get Actual speed [int]
COMP 0
JC NE , END_0
SIO 1, 2, 0 // change Output 1 to zero ( Brake )
END_0:
RSUB
// 111111111111111111111111111111111111111111111111111111111111111111111
MODE_1: // Releases the brake
GIO 1, 2 // READ Status output 1 (Brake )
COMP 1
JC EQ , Brake_Free
SIO 1, 2, 1 // Set Output 1 to 1 ( Release Brake )
WAIT TICKS, 0, 30 // wait 300 msec
Brake_Free:
RSUB
// 2222222222222222222222222222222222222222222222222222222222222222222222
MODE_2: // Find the reference position
GGP 12, 2 // get User variable 12 homing sequence
COMP 20
JC GE , No_Action_2
COMP 15
JC EQ , Step_15
COMP 10
JC EQ , Step_10
COMP 9
JC EQ , Step_9
COMP 8
JC EQ , Step_8
COMP 7
JC EQ , Step_7
COMP 6