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