'Constants
symbol min_step_delay = 3   'through trial and error 3 milliseconds is the smallest delay
symbol step_delay = 3

'Variables
Port_Mask             var byte
Step_direction1       var byte
Step_direction2       var byte
Step_direction3       var byte
Step_direction4       var byte

'Inputs
Left_Button_Forward   var PORTA.4
Left_Button_Stop      var PORTA.5
Left_Button_Backward  var PORTE.0
Right_Button_Forward  var PORTD.6
Right_Button_Stop     var PORTD.5
Right_Button_Backward var PORTD.4

input Right_Button_Forward  
input Right_Button_Stop     
input Right_Button_Backward 
input Left_Button_Forward  
input Left_Button_Stop     
input Left_Button_Backward

'Outputs
Left_LED_Forward      var PORTA.1
Left_LED_Stop         var PORTA.2
Left_LED_Backward     var PORTA.3
Right_LED_Forward     var PORTB.5
Right_LED_Stop        var PORTB.4
Right_LED_Backward    var PORTB.2

TRISC = %00000000
'Right_Step1          var PORTC.7
'Right_Step2          var PORTC.6
'Right_Step3          var PORTC.5
'Right_Step4          var PORTC.4

'Left_Step1           var PORTC.0
'Left_Step2           var PORTC.1
'Left_Step3           var PORTC.2
'Left_Step4           var PORTC.3

'The stepper motors are run off of Port C.  They can both be run simutaneously
'by setting the port to a certain value instead of individually setting the pins


output Left_LED_Forward  
output Left_LED_Stop     
output Left_LED_Backward 
output Right_LED_Forward  
output Right_LED_Stop     
output Right_LED_Backward

ADCON1 = %0000110    'Be sure to turn off the ADC in order to use 
                     'Port A and Port D as digital I/O

'Initialize the LEDs
Left_LED_Forward = 0    
Left_LED_STOP = 1         
Left_LED_Backward = 1     
Right_LED_Forward = 0    
Right_LED_STOP = 1        
Right_LED_Backward = 1

'Initialize the stepper states
PORTC = 0

'Initialize Variables
Port_mask = %00000000
step_direction1 = %10001000
Step_direction2 = %01000100
Step_direction3 = %00100010
Step_direction4 = %00010001

MainLoop:
'--------------------Stop Button code -----------------------
if left_button_stop = 0 then 
Port_mask = port_mask & %11110000
left_led_stop = 0
endif

if left_button_stop = 1 then
port_mask = port_mask | %00001111
left_led_stop = 1
endif

if right_button_stop = 0 then 
Port_mask = port_mask & %00001111
right_led_stop = 0
endif

if right_button_stop = 1 then
port_mask = port_mask | %11110000
right_led_stop = 1
endif
'-------------------------------------------------------------

'-----------------Direction Button Code-----------------------
if right_button_forward = 0 then
step_direction1 = (step_direction1 & %00001111) | %10000000
Step_direction2 = (step_direction2 & %00001111) | %01000000
Step_direction3 = (step_direction3 & %00001111) | %00100000
Step_direction4 = (Step_direction4 & %00001111) | %00010000
right_led_backward = 1
right_led_forward = 0
endif

if right_button_backward = 0 then
step_direction1 = (step_direction1 & %00001111) | %00010000
Step_direction2 = (step_direction2 & %00001111) | %00100000
Step_direction3 = (step_direction3 & %00001111) | %01000000
Step_direction4 = (Step_direction4 & %00001111) | %10000000
right_led_backward = 0
right_led_forward = 1
endif

if left_button_forward = 0 then
step_direction1 = (step_direction1 & %11110000) | %00001000
Step_direction2 = (step_direction2 & %11110000) | %00000100
Step_direction3 = (step_direction3 & %11110000) | %00000010
Step_direction4 = (Step_direction4 & %11110000) | %00000001
left_led_backward = 1
left_led_forward = 0
endif

if left_button_backward = 0 then
step_direction1 = (step_direction1 & %11110000) | %00000001
Step_direction2 = (step_direction2 & %11110000) | %00000010
Step_direction3 = (step_direction3 & %11110000) | %00000100
Step_direction4 = (Step_direction4 & %11110000) | %00001000
left_led_backward = 0
left_led_forward = 1
endif
'-------------------------------------------------------------

'--------------------- Step Code -----------------------------
PORTC = step_direction1 & Port_mask
pause step_delay
PORTC = Step_direction2 & Port_mask
pause step_delay
PORTC = Step_direction3 & Port_mask
pause step_delay
PORTC = Step_direction4 & Port_mask
pause step_delay
'-------------------------------------------------------------

goto mainloop