Friday, 21 September 2012

PIC based stepper driver controller for CNC

Here's the PIC code for our USB-HID device/stepper motor controller. Here's how it works:

Data is sent to the device through an 8-byte wide buffer. The seventh byte in the buffer is a "command byte" and (amongst other things) can be used to


  • set the number of steps to move on the x-axis
  • set the number of steps to move on the y-axis
  • start the head moving
  • plunge the drill
  • set the backlash values for x/y axis
  • set the minimum/maximum drill height
While the board is connected, you can poll the device and it will report

  • if the head is still moving (x/y direction)
  • if the drill is activated (spinning/plunging)
  • if the machine is ready to receive the next command

The code is written for Oshonsoft PIC18 Simulator/compiler and runs on an 18F2455 PIC microcontroller


Define CLOCK_FREQUENCY = 20
Define CONFIG1L = 0x24
Define CONFIG1H = 0x0c
Define CONFIG2L = 0x38
Define CONFIG2H = 0x00
Define CONFIG3L = 0x00
Define CONFIG3H = 0x03
Define CONFIG4L = 0x80
Define CONFIG4H = 0x00
Define CONFIG5L = 0x0f
Define CONFIG5H = 0xc0
Define CONFIG6L = 0x0f
Define CONFIG6H = 0xe0
Define CONFIG7L = 0x0f
Define CONFIG7H = 0x40

UsbSetVendorId 0x1234
UsbSetProductId 0x1234
UsbSetVersionNumber 0x1122
UsbSetManufacturerString "Nerd Club"
UsbSetProductString "CNC drilling machine"
UsbSetSerialNumberString "1111111111"
UsbOnIoInGosub input_report_before_sending
UsbOnIoOutGosub output_report_received
UsbOnFtInGosub feature_report_before_sending
UsbOnFtOutGosub feature_report_received

AllDigital
Dim i As Byte
Dim stepsx As Long
Dim stepsy As Long
Dim dirx As Byte
Dim diry As Byte
Dim domove As Bit
Dim doplunge As Bit
Dim isdrilling As Bit
Dim booting As Bit
Dim atdestination As Bit
Dim snapback As Bit
Dim tmp As Byte
Dim tmpw As Word
Dim tmpl As Long

Dim statex As Byte
Dim statey As Byte
Dim statez As Byte
Dim zaxisvalue As Byte
Dim drillspeedvalue As Byte
Dim zaxisspeed As Byte
Dim zservoon As Bit
Dim dservoon As Bit
Dim holdzaxis As Bit
Dim timeoutcount As Byte
Dim zmin As Byte
Dim zmax As Byte
Dim drilldir As Byte
Dim feedbackvalues As Byte

Config PORTB = Output
Config PORTC.7 = Output
Config PORTC.6 = Output

Symbol coila_1 = PORTB.0
Symbol coilb_1 = PORTB.1
Symbol coilc_1 = PORTB.2
Symbol coild_1 = PORTB.3

Symbol coila_2 = PORTB.4
Symbol coilb_2 = PORTB.5
Symbol coilc_2 = PORTB.6
Symbol coild_2 = PORTB.7

Symbol zaxis = PORTC.7
Symbol drillspeed = PORTC.6

init:
      booting = 1
      domove = 0
      doplunge = 0
      atdestination = 1
      statex = 1
      statey = 1
      statez = 0
      snapback = 1

      zservoon = 1
      zaxisvalue = zmin 'retracted
      dservoon = 1
      drillspeedvalue = 90 'midpoint=stopped
      zaxisspeed = 50
      holdzaxis = 0

      timeoutcount = 0
      zmin = 150
      zmax = 175
      drilldir = 0
      isdrilling = 0

      feedbackvalues = 1

      UsbStart

      'wait 1 second before sending any servo commands
      '(so the servo controller doesn't freak out)
      WaitMs 1000

      'set up a timer1 interrupt to occur every 1/20th second
      '(for servo control commands- particularly motor spin speed)
      '(we use this for servos)
      'enable timer1 interrupts to trigger every 1ms
      INTCON1.GIE = 1 'enable global interrupts
      PIE1.0 = 1 'enable TMR1 interupts
      INTCON.6 = 1 'enable all unmasked interrupts
      INTCON.7 = 1 'enable Global interrupts
      PIR1.0 = 0 'reset interupt flag
      T1CON.TMR1CS = 0 'use fosc/4
      T1CON.TMR1ON = 1 'turn on timer1

      'enable timer0 in 16-bit mode
      T0CON.T08BIT = 0
      T0CON.TMR0ON = 1
      T0CON.T0CS = 0 'use fosc/4

      'connect to the brushless motor controller
      '(it has a specific startup sequence)
      drillspeedvalue = 90

      'make sure the drill is retracted(~200ms)
      zservoon = 1
      zaxisvalue = zmin
      drillspeedvalue = 90


loop:
      booting = 0
     
      While domove = 1

            Select Case statex
                  Case 1
                  High coila_1
                  Low coilb_1
                  Low coilc_1
                  Low coild_1

                  Case 2
                  High coila_1
                  High coilb_1
                  Low coilc_1
                  Low coild_1

                  Case 3
                  Low coila_1
                  High coilb_1
                  Low coilc_1
                  Low coild_1

                  Case 4
                  Low coila_1
                  High coilb_1
                  High coilc_1
                  Low coild_1

                  Case 5
                  Low coila_1
                  Low coilb_1
                  High coilc_1
                  Low coild_1

                  Case 6
                  Low coila_1
                  Low coilb_1
                  High coilc_1
                  High coild_1

                  Case 7
                  Low coila_1
                  Low coilb_1
                  Low coilc_1
                  High coild_1

                  Case 8
                  High coila_1
                  Low coilb_1
                  Low coilc_1
                  High coild_1
                       
            EndSelect

           
            Select Case statey
                  Case 1
                  High coila_2
                  Low coilb_2
                  Low coilc_2
                  Low coild_2

                  Case 2
                  High coila_2
                  High coilb_2
                  Low coilc_2
                  Low coild_2

                  Case 3
                  Low coila_2
                  High coilb_2
                  Low coilc_2
                  Low coild_2

                  Case 4
                  Low coila_2
                  High coilb_2
                  High coilc_2
                  Low coild_2

                  Case 5
                  Low coila_2
                  Low coilb_2
                  High coilc_2
                  Low coild_2

                  Case 6
                  Low coila_2
                  Low coilb_2
                  High coilc_2
                  High coild_2

                  Case 7
                  Low coila_2
                  Low coilb_2
                  Low coilc_2
                  High coild_2

                  Case 8
                  High coila_2
                  Low coilb_2
                  Low coilc_2
                  High coild_2
                       
            EndSelect

                       
            If stepsx > 0 Then
                  If dirx = 1 Then
                        statex = statex + 1
                        If statex > 8 Then statex = 1
                  Else
                        statex = statex - 1
                        If statex < 1 Then statex = 8
                  Endif
                  stepsx = stepsx - 1
            Endif

            If stepsy > 0 Then
                  If diry = 1 Then
                        statey = statey + 1
                        If statey > 8 Then statey = 1
                  Else
                        statey = statey - 1
                        If statey < 1 Then statey = 8
                  Endif
                  stepsy = stepsy - 1
            Endif

            If stepsx = 0 And stepsy = 0 Then
                  'set the flag to say we've arrived at the destination
                  atdestination = 1
                  domove = 0
            Else
                  atdestination = 0
                  'we need a delay for the stepper to respond
                  WaitMs 1
            Endif

            'poll the usb just to keep it alive
            UsbService
      Wend

      'important: only plunge when NOT moving!
      If domove = 0 Then

            If statez > 0 Then
                 
                  domove = 0
                  atdestination = 0
                  isdrilling = 1

                  Select Case statez

                  '--------------------------------
                  Case 1 'start the drill spinning
                  '--------------------------------
                  drillspeedvalue = 255
                  WaitMs 100
                  statez = 2

                  '-------------------
                  Case 2 'moving down
                  '-------------------
                  If zaxisvalue < zmax Then
                        zaxisvalue = zaxisvalue + 1
                        If zaxisspeed > 0 Then WaitMs zaxisspeed
                  Else
                        zaxisvalue = zmax
                        statez = 3
                  Endif

                  '----------------------------------------
                  Case 3 'wait at the bottom of the stroke
                  '----------------------------------------
                  WaitMs 500
                  statez = 4

                  '-------------------
                  Case 4 'moving up
                  '-------------------
                  If zaxisvalue > zmin Then
                        zaxisvalue = zaxisvalue - 1
                        If snapback = 1 Then
                              'don't delay
                        Else
                              If zaxisspeed > 0 Then WaitMs zaxisspeed
                        Endif
                  Else
                        zaxisvalue = zmin
                        statez = 5
                  Endif

                  '-----------------------------------
                  Case 5 'brake (not break) the drill
                  '-----------------------------------
                  drillspeedvalue = 60
                  WaitMs 100
                  statez = 6

                  '-------------------------------
                  Case 6 'stop the drill spinning
                  '-------------------------------
                  drillspeedvalue = 90
                  statez = 0
                  WaitMs 100
                  atdestination = 1
                  isdrilling = 0

                  EndSelect
            Endif
      Endif

      'poll the usb to keep it alive
      UsbService

Goto loop

End


feature_report_received:
Return


feature_report_before_sending:
Return


output_report_received:

      Select Case UsbIoBuffer(7)
            '--------------------------------------
            Case 1 'set X step count/direction
            '--------------------------------------
            dirx = UsbIoBuffer(4)
            tmpw.HB = UsbIoBuffer(3)
            tmpw.LB = UsbIoBuffer(2)
            tmpl.HW = tmpw
            tmpw.HB = UsbIoBuffer(1)
            tmpw.LB = UsbIoBuffer(0)
            tmpl.LW = tmpw
            stepsx = tmpl
            domove = 0

            '--------------------------------------
            Case 2 'set Y step count/direction
            '--------------------------------------
            diry = UsbIoBuffer(4)
            tmpw.HB = UsbIoBuffer(3)
            tmpw.LB = UsbIoBuffer(2)
            tmpl.HW = tmpw
            tmpw.HB = UsbIoBuffer(1)
            tmpw.LB = UsbIoBuffer(0)
            tmpl.LW = tmpw
            stepsy = tmpl
            domove = 0

            '---------------------------------
            Case 240 'reset the flag buffers
            '---------------------------------
            zaxisvalue = zmin
            domove = 0
            statez = 0
            drillspeedvalue = 90
            WaitMs 300
            isdrilling = 0
            atdestination = 1

            '----------------------------------------------------------
            Case 241 'set which values you want to read back from usb
            '----------------------------------------------------------
            feedbackvalues = UsbIoBuffer(0)

            '--------------------------------------
            Case 247 'zmin value
            '--------------------------------------
            zmin = UsbIoBuffer(0)

            '--------------------------------------
            Case 248 'zmax value
            '--------------------------------------
            zmax = UsbIoBuffer(0)

            '--------------------------------------
            Case 249 'set the z-axis step speed
            '--------------------------------------
            zaxisspeed = UsbIoBuffer(0)

            '--------------------------------------
            Case 250 'set the drill speed
            '--------------------------------------
            drillspeedvalue = UsbIoBuffer(0)

            '--------------------------------------
            Case 251 'set the zaxis servo depth
            '--------------------------------------
            zaxisvalue = UsbIoBuffer(0)
            statez = 0

            '--------------------------------------
            Case 253 'plunge the drill
            '--------------------------------------
            statez = 1
            domove = 0

            '--------------------------------------
            Case 254 'start moving
            '--------------------------------------
            atdestination = 0
            domove = 1

      EndSelect
Return


input_report_before_sending:
      'tell the PC our current status
      '(so when PC sees we're at our destination, for example
      'it can move onto the next command in the script)
      tmp = 0
      tmp.0 = domove
      tmp.1 = atdestination
      tmp.2 = isdrilling
      tmp.7 = booting
     
      Select Case feedbackvalues

            Case 0
            UsbIoBuffer(0) = 0
            UsbIoBuffer(1) = 0
            UsbIoBuffer(2) = 0
            UsbIoBuffer(3) = statex
            UsbIoBuffer(4) = zaxisvalue
            UsbIoBuffer(5) = drillspeedvalue
            UsbIoBuffer(6) = tmp
            UsbIoBuffer(7) = 255

            Case 1
            tmpw = stepsx.LW
            UsbIoBuffer(0) = tmpw.LB
            UsbIoBuffer(1) = tmpw.HB
            tmpw = stepsy.LW
            UsbIoBuffer(2) = tmpw.LB
            UsbIoBuffer(3) = tmpw.HB
            UsbIoBuffer(4) = zaxisvalue
            UsbIoBuffer(5) = drillspeedvalue
            UsbIoBuffer(6) = tmp
            UsbIoBuffer(7) = 255

      EndSelect

Return


preloadtimer1:
      'pre-load to 15535 (65,535-50,000 where 1ms=5000)
      'so this causes a timeout interrupt every 10m/s
      TMR1H = 50 '216 (2ms)
      TMR1L = 175 '239 (2ms)
Return


On High Interrupt

      'if timer1 has rolled over (hit 65535) then
      If PIR1.TMR1IF = 1 Then

            'save system state/working address etc
            Save System

            'reset the timer1 interrupt flag
            PIR1.TMR1IF = 0

            'preload timer1 with a number to count to
            'so that it rolls over (hits 65535) after
            '2ms. As it happens, at 20Mhz, we need to
            'count up to 5000 for 1ms to elapse.
            Gosub preloadtimer1

            timeoutcount = timeoutcount + 1
            If timeoutcount > 1 Then
     
                  'send the servo position control
                  If zservoon = 1 Then
                        ServoOut zaxis, zaxisvalue
                  Endif

                  'send the drill speed control
                  If dservoon = 1 Then
                        ServoOut drillspeed, drillspeedvalue
                  Endif
                  timeoutcount = 0
            Endif
     
      Endif
     
Resume



CNC drill schematic