8 Servo Control via CAN and UART

General discussion on mikroBasic PRO for PIC.
Post Reply
Author
Message
marcolino7
Posts: 32
Joined: 09 Dec 2010 00:51

8 Servo Control via CAN and UART

#1 Post by marcolino7 » 09 Jun 2012 00:09

Hi,
I found this into old post by Xor:

http://www.mikroe.com/forum/viewtopic.php?f=10&t=5979

I need to command 8 servos connected onto 8 different port group.
For Example:

Code: Select all

symbol servo_pt0 = LATB.5
symbol servo_pt1 = LATB.4
symbol servo_pt2 = LATC.3
symbol servo_pt3 = LATC.2
symbol servo_pt4 = LATC.1
symbol servo_pt5 = LATC.0
symbol servo_pt6 = LATA.5
symbol servo_pt7 = LATA.4
Each servo has His own port not consecutive.
I run on PIC18F2580 and 20Mhz Crystal for oscillator.
Someone can help me to develop my code?
Many thanks

Marco

marcolino7
Posts: 32
Joined: 09 Dec 2010 00:51

Re: 8 Servo Control via CAN and UART

#2 Post by marcolino7 » 12 Jun 2012 14:43

Hi,
I'm trying to port this code to my project.
It's a Serial/CAN 8 servo controller. Problem is that servo is not connected to same port, so I cannot use excellent code by XOR por quick servos positioning.
PIC is 18F2580 running at 20Mhz. Compiler is Mikrobasic PRO 5.60
I got the code from this topic and edited like this to avoid compiling errors:

Code: Select all

program CAN_Servo

'Config Variabiles
symbol T1_Switch = T1CON.TMR1ON
symbol T1_Int_Flag = PIR1.TMR1IF
symbol T1_Int_En = PIE1.TMR1IE
symbol T0_Int_Flag = INTCON.TMR0IF
symbol T0_Int_En = INTCON.TMR0IE


' Firmware release
const fw as string[4] = "0.03"

' Declarations section
dim uart_rd as byte
dim sread as string[8]
dim delimiter as string[1]


' EEPROM Position
const init_pos as byte[8]      = (0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07)
const low_offset as byte[8]    = (0x08,0x09,0x0A,0x0B,0x0C,0x0D,0x0E,0x0F)
const high_offset as byte[8]   = (0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17)

' Servo Variabiles
const IsEnabled = 1
const IsDisabled = 0
const IsSet = 1
const IsRst = 0
const IsOn = 1
const IsOff = 0
dim intflag as boolean
dim servo as byte
dim T1 as word absolute $0E
dim T0 as byte absolute $01
dim pos as word[8]


'CAN BUS Varibiles
dim Can_Init_Flags, Can_Send_Flags, Can_Rcv_Flags as byte ' CAN flags
    Rx_Data_Len as byte                                    ' received data length in bytes
    RxTx_Data as byte[8]                                   ' can rx/tx data buffer
    Msg_Rcvd as byte                                       ' reception flag
    ID_1st, ID_2nd as longint                              ' node IDs
    Rx_ID as longint


sub procedure CR()
    UART1_Write(10)   ' Line Feed
    UART1_Write(13)   ' Carriage Return
end sub

sub Procedure ImpostaUscita(dim valore as byte, dim servo_num as byte)

    'symbol servo_pt0 = LATB.5
    'symbol servo_pt1 = LATB.4
    'symbol servo_pt2 = LATC.3
    'symbol servo_pt3 = LATC.2
    'symbol servo_pt4 = LATC.1
    'symbol servo_pt5 = LATC.0
    'symbol servo_pt6 = LATA.5
    'symbol servo_pt7 = LATA.4
    if valore = 0 then
        select case servo_num
               case 0
                    ClearBit(LATB,5)
               case 1
                    ClearBit(LATB,4)
               case 2
                    ClearBit(LATC,3)
               case 3
                    ClearBit(LATC,2)
               case 4
                    ClearBit(LATC,1)
               case 5
                    ClearBit(LATC,0)
               case 6
                    ClearBit(LATA,5)
               case 7
                    ClearBit(LATA,4)
        end select
    end if
    if valore = 1 then
        select case servo_num
               case 0
                    SetBit(LATB,5)
               case 1
                    SetBit(LATB,4)
               case 2
                    SetBit(LATC,3)
               case 3
                    SetBit(LATC,2)
               case 4
                    SetBit(LATC,1)
               case 5
                    SetBit(LATC,0)
               case 6
                    SetBit(LATA,5)
               case 7
                    SetBit(LATA,4)
        end select
    end if



end sub

sub Procedure interrupt
           If T0_Int_Flag = IsSet Then      ' if Timer0 interrupt
              T0 = 196                     ' load Frame 196 x 12.8us = 2508us interrupt cycle
              'Clearbit(PORTB, servo)        ' force servo off...in case value was wrong
              ImpostaUscita(0,servo)
              servo = inc(servo) And 7      ' next servo
              'Setbit(PORTB, servo)          ' turn on next servo
              ImpostaUscita(1,servo)
              T1 = pos[servo]           ' load servo position...each count is 0.2us
              T1_Switch = IsOn              ' turn Timer1 On
              T1_Int_Flag = IsRst           ' reset Timer1 Interrupt Flag
              T0_Int_Flag = IsRst           ' reset Timer0 Interrupt Flag
           Else                             ' if Timer1 interrupt
              'Clearbit(PORTB, servo)        ' turn off servo
              ImpostaUscita(0,servo)
              T1_Switch = IsOff             ' turn Timer1 Off
           End If
           intflag = not(intflag)           ' interrupt flag status
end sub

sub Procedure CAN_init

  Can_Send_Flags = _CAN_TX_PRIORITY_0 and                   ' form value to be used
                   _CAN_TX_XTD_FRAME and                    '     with CANWrite
                   _CAN_TX_NO_RTR_FRAME

  Can_Init_Flags = _CAN_CONFIG_SAMPLE_THRICE and            ' form value to be used
                   _CAN_CONFIG_PHSEG2_PRG_ON and            ' with CANInit
                   _CAN_CONFIG_XTD_MSG and
                   _CAN_CONFIG_DBL_BUFFER_ON and
                   _CAN_CONFIG_VALID_XTD_MSG and
                   _CAN_CONFIG_LINE_FILTER_OFF

  ID_1st = 12111
  ID_2nd = 3
  RxTx_Data[0] = 9                                           ' set initial data to be sent


  CANSetOperationMode(_CAN_MODE_CONFIG,0xFF)                 ' set CONFIGURATION mode
  CANSetMask(_CAN_MASK_B1,-1,_CAN_CONFIG_XTD_MSG)            ' set all mask1 bits to ones
  CANSetMask(_CAN_MASK_B2,-1,_CAN_CONFIG_XTD_MSG)            ' set all mask2 bits to ones
  CANSetFilter(_CAN_FILTER_B2_F3,ID_1st,_CAN_CONFIG_XTD_MSG) ' set id of filter B2_F3 to 1st node ID

  CANSetOperationMode(_CAN_MODE_NORMAL,0xFF)                 ' set NORMAL mode

end sub

sub Procedure init

    PORTA = %00000000
    PORTB = %00000000
    PORTC = %00000000

    LATA = %00000000
    LATB = %00000000
    LATC = %00000000

    RCON.IPEN = 0           'Disabilito la priorità degli interrupt. Compatibilità 16F
    INTCON.RBIF = 0
    INTCON.INT0IF = 0
    INTCON.TMR0IF = 1
    INTCON.RBIE = 0
    INTCON.INT0IE = 0
    INTCON.TMR0IE = 1       'Interrupt sul timer 0
    INTCON.PEIE = 1
    INTCON.GIE = 1          'Global Interrupt


    INTCON2.TMR0IP = 0      'Timer0 è low priority


    '---Timer0 Control ------
    T0CON.T0PS2 = 1
    T0CON.T0PS1 = 0
    T0CON.T0PS0 = 1         'Prescaler a 1:64
    T0CON.T08BIT = 1        'TMR0 a 8bit
    T0CON.T0SE = 0          'Increment on low-to-high transition on T0CKI pin
    T0CON.T0CS = 0          'Internal instruction cycle clock (CLKO)
    T0CON.PSA = 0           'Timer0 prescaler is assigned. Timer0 clock input comes from prescaler output.
    T0CON.TMR0ON = 1        'Attivo il timer 0

    '---Timer0 Control ------
    T1CON = 0

    TRISA = %00000000
    TRISB = %00000000
    TRISC = %00000000

    ADCON1 = 0x0F                           ' Configure all ports with analog function as digital



    UART1_Init(38400)                     ' Initialize UART module at 38400 bps
    Delay_ms(200)                         ' Wait for UART module to stabilize

    'Inizializzo il bus CAN
    CAN_init

    delimiter = 13         'delimitatore per la conferma del comando seriale

    '---- SERVO INIT ------
    For servo = 0 to 7
      pos[servo] = 7500             ' set for 1500us mid-point
    Next servo

    intflag = False
    servo = 0
    T1 = 4500                    ' load Timer1
    T1_Int_Flag = IsRst              ' reset Timer1 Interrupt Flag
    T1_Int_En = IsEnabled            ' Timer1 Interrupt Enabled
    T1_Switch = IsOn                 ' Timer1 On
    T0 = 196                        ' load TMR0 for 2500us Interrupt Cycle Count
    T0_Int_En = IsEnabled            ' reset Timer0 Interrupt Flag
    'Setbit(PORTB, 0)                 ' turn on first servo
    'servo_pt0 = 1                     'turn on first servo my way


    'Posizioni Iniziali Dei Servi
    'Servo_arr[0]=EEPROM_Read(0x00)
    'Servo_arr[1]=EEPROM_Read(0x01)
    'Servo_arr[2]=EEPROM_Read(0x02)
    'Servo_arr[3]=EEPROM_Read(0x03)
    'Servo_arr[4]=EEPROM_Read(0x04)
    'Servo_arr[5]=EEPROM_Read(0x05)
    'Servo_arr[6]=EEPROM_Read(0x06)
    'Servo_arr[7]=EEPROM_Read(0x07)

end sub

sub Procedure set_init_pos(dim byref cmd as string[4])
   'scrive nella eeprom il valore iniziale della posizione del servo
   dim i as integer
   i = strtoint("0"+cmd[0])
   EEPROM_Write(init_pos[i],strtoint(cmd[1]+cmd[2]+cmd[3]))
   UART1_Write_Text("Pos.Iniz. Servo_"+cmd[0]+"="+cmd[1]+cmd[2]+cmd[3])
   CR
end sub

sub Procedure set_low_offset(dim byref cmd as string[4])
   'scrive nella eeprom il valore offset basso
   dim i as byte
   i = strtoint("0"+cmd[0])
   EEPROM_Write(low_offset[i],strtoint(cmd[1]+cmd[2]+cmd[3]))
   UART1_Write_Text("Offset Basso Servo_"+cmd[0]+"="+cmd[1]+cmd[2]+cmd[3])
   CR
end sub

sub Procedure set_high_offset(dim byref cmd as string[4])
   'scrive nella eeprom il valore offset alto
   dim i as byte
   i = strtoint("0"+cmd[0])
   EEPROM_Write(high_offset[i],strtoint(cmd[1]+cmd[2]+cmd[3]))
   UART1_Write_Text("Offset Alto Servo_"+cmd[0]+"="+cmd[1]+cmd[2]+cmd[3])
   CR
end sub

sub Procedure set_servo_pos(dim byref si as byte, dim byref cmd as string[6])
   'posiziono il servo
   dim ss as string[5]
       'Devo sottrarre 48 se il comando arriva dalla seriale
       'Servo_arr[si-48] = strtoint(cmd[0]+cmd[1]+cmd[2])
       pos[si-48] = strtoword(cmd[0]+cmd[1]+cmd[2]+cmd[3]+cmd[4])
       bytetostr(si-48,ss)
       ltrim(ss)
       UART1_Write_Text("Servo_"+ss+":"+cmd)
       CR
end sub

sub Procedure can_servo_pos(dim byref si as byte, dim byref cmd as byte)
   'posiziono il servo
   dim ss as string[3]
   bytetostr(si,ss)
   'Servo_arr[si] = cmd
   UART1_Write_Text("Servo_"+ss+":"+sread)
   CR
end sub

main:

     init
     'SetAllServoTo(256)
     'INTCON.GIE = 1  'abilito gli interrupt
     UART1_Write_Text("CAN Serial Servo "+fw+" Ready")
     CR

     while true
       While intflag = False
           if (UART1_Data_Ready() <> 0) then  ' If data is received,
              uart_rd = UART1_Read()
              Select Case uart_rd
                     '------ da 0 a 7 Muovo i Servi -----------------------------------------
                     case "0","1","2","3","4","5","6","7"
                          UART1_Read_Text(sread,delimiter,6)
                          set_servo_pos(uart_rd,sread)
                     '------ leggo l'help ---------------------------------------------------
                     case "a"
                          UART1_Write_Text("Servo CAN Controller "+fw)
                          CR
                          CR
                          UART1_Write_Text("- 0/7 Selezione Servo, dopo inserire il valore della posizione")
                          CR
                          UART1_Write_Text("      da 0 a 255 seguita da invio")
                          CR
                          UART1_Write_Text("-a legge l'help")
                          CR
                          UART1_Write_Text("-i imposta i valori iniziale del servo sintassi sXYYY")
                          CR
                          UART1_Write_Text("   X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                          UART1_Write_Text("-l imposta l'offset basse del servo sintassi lXYYY")
                          CR
                          UART1_Write_Text("   X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                          UART1_Write_Text("-h imposta l'offset alto del servo sintassi hXYYY")
                          CR
                          UART1_Write_Text("   X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                    '------ Gestione del valore iniziale -----------------------------------
                     case "i"
                          UART1_Write_Text("Valore iniziale del servo sintassi XYYY")
                          CR
                          UART1_Write_Text("X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                          UART1_Read_Text(sread,delimiter,5)
                          set_init_pos(sread)
                    '------ Gestione dell'offset basso -------------------------------------
                     case "l"
                          UART1_Write_Text("Offset basso del servo sintassi XYYY")
                          CR
                          UART1_Write_Text("X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                          UART1_Read_Text(sread,delimiter,5)
                          set_low_offset(sread)
                    '------ Gestione dell'offset alto  -------------------------------------
                     case "h"
                          UART1_Write_Text("Offset alto del servo sintassi XYYY")
                          CR
                          UART1_Write_Text("X=servo da 0 a 7, YYY=posizione da 0 a 255")
                          CR
                          UART1_Read_Text(sread,delimiter,5)
                          set_high_offset(sread)
                     case else  'Non faccio nulla e vado a capo
                          UART1_Write_Text("a for Help")
                          CR

              end select
           end if

           'CAN Operation
           Msg_Rcvd = CANRead(Rx_ID , RxTx_Data , Rx_Data_Len, Can_Rcv_Flags)   ' receive message
           if ((Rx_ID = ID_1st) and (Msg_Rcvd <> 0)) <> 0 then                  ' if message received check id
              if RxTx_Data[0] = ID_2nd Then 'Il messaggio è per questa periferica
                  UART1_Write_Text("CAN Rcv")
                  CR
                  'can_servo_pos(RxTx_Data[1],RxTx_Data[2])
              end if
           end if

       wend
     wend
end.
CAN and Serial Section is working.
Runnign this code onto a real circuit, I got on Oscilloscope a Frequency of 38.17 MHz, i think this is cause by my soubroutine that select the port

Code: Select all

sub Procedure ImpostaUscita(dim valore as byte, dim servo_num as byte)

    'symbol servo_pt0 = LATB.5
    'symbol servo_pt1 = LATB.4
    'symbol servo_pt2 = LATC.3
    'symbol servo_pt3 = LATC.2
    'symbol servo_pt4 = LATC.1
    'symbol servo_pt5 = LATC.0
    'symbol servo_pt6 = LATA.5
    'symbol servo_pt7 = LATA.4
    if valore = 0 then
        select case servo_num
               case 0
                    ClearBit(LATB,5)
               case 1
                    ClearBit(LATB,4)
               case 2
                    ClearBit(LATC,3)
               case 3
                    ClearBit(LATC,2)
               case 4
                    ClearBit(LATC,1)
               case 5
                    ClearBit(LATC,0)
               case 6
                    ClearBit(LATA,5)
               case 7
                    ClearBit(LATA,4)
        end select
    end if
    if valore = 1 then
        select case servo_num
               case 0
                    SetBit(LATB,5)
               case 1
                    SetBit(LATB,4)
               case 2
                    SetBit(LATC,3)
               case 3
                    SetBit(LATC,2)
               case 4
                    SetBit(LATC,1)
               case 5
                    SetBit(LATC,0)
               case 6
                    SetBit(LATA,5)
               case 7
                    SetBit(LATA,4)
        end select
    end if
Strage thing is that i got for all channels, ON period of 3,40msec, and changing the variable pos[0], channel 0 does not change. This is the behaviour of all channles.
Anyone can helpme?

Thanks

Post Reply

Return to “mikroBasic PRO for PIC General”