//This program demonstrates the stall detection that can be used when
//the module is equipped with TMC246 motor drivers.
//Motor 0 will run until it is stalled, then it will run in the other direction after one second
//until it is stalled, and so on.
//With the potentiometer it is possible to adjust the stall value. The adjusting loops are "one", "two", ..., "six".

//Values that have to be adapted
StallValue=6  //1=highest sensitivity ... 7=lowest sensitivity (0=off)
Velocity=750
Acceleration=250
current=500
microsteps=6

//Initialization
        SAP 205, 0, StallValue
        SAP 5, 0, Acceleration
        SAP 203, 0, 2048
        SIO 0, 2, 0
        SAP 6, 0, current
        SAP 140, 0, microsteps
        CSUB BlinkBlink

//Main Loop
Loop:   ROR 0, Velocity
        CSUB WaitUntilStalled
        MST 0
        SIO 0, 2, 1
        WAIT TICKS, 0, 100
        SIO 0, 2, 0
        ROL 0, Velocity
        CSUB WaitUntilStalled
        MST 0
        SIO 0, 2, 1
        WAIT TICKS, 0, 100
        SIO 0, 2, 0
        JA Loop


WaitUntilStalled:
       GIO 0, 1
       COMP 1020
       JC GT, six
       GIO 0, 1
       COMP 770
       JC GT, five
       GIO 0, 1
       COMP 520
       JC GT, four
       GIO 0, 1
       COMP 270
       JC GT, three
       GIO 0, 1
       COMP 20
       JC GT, two
       GIO 0, 1
       COMP 20
       JC LE, one
       RSUB

one:
       SAP 205, 0, 3
       JA ContinueStalled

two:
       SAP 205, 0, 4
       JA ContinueStalled

three:
       SAP 205, 0, 5
       JA ContinueStalled

four:
       SAP 205, 0, 6
       JA ContinueStalled
       
five:
       SAP 205, 0, 7
       JA ContinueStalled

six:
       SAP 205, 0, 0
       JA ContinueStalled

ContinueStalled:
       GAP 3, 0
       COMP 0
       JC NE, WaitUntilStalled
       RSUB
       
BlinkBlink:
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       WAIT TICKS, 0, 10
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       WAIT TICKS, 0, 10
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       WAIT TICKS, 0, 10
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       WAIT TICKS, 0, 10
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       WAIT TICKS, 0, 10
       SIO 0, 2, 1
       WAIT TICKS, 0, 10
       SIO 0, 2, 0
       RSUB