I have been trying to use a PCA9685 to controll muliple servos without using any specific library for servocontroll, but I cant seem to get it to work and I dont know what I am doing wrong. To my believe, I did all that I needed to do. I configured the right prescaler to get a frequency of 50 Hz und set the ON and OFF time accordingly for servos, but nothing is moving. I would be really grateful, if someone would be able to help me out.
Here is my current code for perspective:
program pca9685_servocontroll
dim pot As Integer
dim angle As Float
dim pulse As Integer
dim channel as Integer
dim txt as string[16]
dim txt1 as string[16]
dim txt2 as string[16]
dim txt3 as string[16]
' I2C-Adresse des PCA9685-Treibers
Const PCA9685_ADDRESS = 0x40
' Registeradressen des PCA9685-Treibers
Const MODE_1 = 0x00
Const MODE_2 = 0x01
Const PRE_SCALE = 0xFE
Const SLEEP = 0x10
Const MODE_1_RESTART = 0x80
Const MODE1_AI = 0x20
Const LED0_ON_L = 0x06
Const LED0_ON_H = 0x07
Const LED0_OFF_L = 0x08
Const LED0_OFF_H = 0x09
' Servomotor-Konfiguration
Const SERVO_MIN_PULSE = 409 ' Minimale Pulsbreite (in Ticks)
Const SERVO_MAX_PULSE = 819 ' Maximale Pulsbreite (in Ticks)
Const SERVO_MIN_ANGLE = 0 ' Minimale Servomotor-Position (in Grad)
Const SERVO_MAX_ANGLE = 180 ' Maximale Servomotor-Position (in Grad)
sub procedure Control_Servo(dim channel as integer)
dim pot As Integer
dim angle As Float
dim pulse As Integer
dim txt as string[16]
dim txt1 as string[16]
dim txt2 as string[16]
dim txt3 as string[16]
' Potentiometerwert lesen
pot = ADC_Get_Sample(0)
InttoStr(pot,txt3)
Glcd_Write_Text(txt3,0,4,1)
' Servo-Position basierend auf Potentiometerwert berechnen
angle = (pot-1)*180/1022
FloattoStr(angle,txt)
Glcd_Write_Text(txt,0,1,1)
' Pulsweite basierend auf Servo-Position berechnen
pulse = (SERVO_MAX_PULSE - SERVO_MIN_PULSE) * (angle - SERVO_MIN_ANGLE) / (SERVO_MAX_ANGLE - SERVO_MIN_ANGLE) + SERVO_MIN_PULSE
InttoStr(pulse,txt1)
Glcd_Write_Text(txt1,0,2,1)
InttoStr(channel,txt2)
Glcd_Write_Text(txt2,0,3,1)
' Servo-Position setzen
I2C1_Start ' I2C-Kommunikation starten
I2C1_Wr(PCA9685_ADDRESS) ' PCA9685-Adresse senden
I2C1_Wr(LED0_ON_L + (4 * channel)) ' Register LEDn_ON_L für den angegebenen Kanal auswählen
I2C1_Wr(0) ' Niedrigere 8 Bits des Pulse-Werts senden
I2C1_Wr(LED0_ON_H + (4 * channel))
I2C1_Wr(0 >>
' Höhere 8 Bits des Pulse-Werts sendenI2C1_Wr(LED0_OFF_L + (4 * channel))
I2C1_Wr(pulse) ' Niedrigere 8 Bits des Pulse-Werts senden
I2C1_Wr(LED0_OFF_H + (4 * channel))
I2C1_Wr(pulse >>
' Höhere 8 Bits des Pulse-Werts sendenI2C1_Stop ' I2C-Kommunikation beenden
end sub
main:
Board_Init()
Board_Adc_Init()
Board_Glcd_Init()
' Initialisierung der I2C-Schnittstelle
I2C1_Init(100000) ' I2C1 mit einer Geschwindigkeit von 100 kHz initialisieren
goto Config_PCA9685
' Konfiguration des PCA9685-Treibers
Config_PCA9685:
I2C1_Start ' I2C-Kommunikation starten
I2C1_Wr(PCA9685_ADDRESS) ' PCA9685-Adresse senden
I2C1_Wr(MODE_1) ' Register MODE1 auswählen
I2C1_Wr(MODE_1_RESTART) ' MODE1 Restart
I2C1_Stop ' I2C-Kommunikation beenden
Delay_ms(1)
I2C1_Start ' I2C-Kommunikation starten
I2C1_Wr(PCA9685_ADDRESS) ' PCA9685-Adresse senden
I2C1_Wr(PRE_SCALE) ' Register PRE_SCALE auswählen
I2C1_Wr(0x79) ' Voreinstellung für eine PWM-Frequenz von 50 Hz bei 8 MHz Clock-Frequenz
I2C1_Stop ' I2C-Kommunikation beenden
Delay_ms(1)
I2C1_Start ' I2C-Kommunikation starten
I2C1_Wr(PCA9685_ADDRESS) ' PCA9685-Adresse senden
I2C1_Wr(MODE_1) ' Register MODE1 auswählen
I2C1_Wr(0x10) ' SLEEP beenden
I2C1_Stop ' I2C-Kommunikation beenden
Delay_ms(1)
goto Hauptprogramm
' Servomotor ansteuern basierend auf Potentiometerwert
Hauptprogramm:
While (SW1=SW_OFF)
wend
goto Start
Start:
'I2C1_Init(100000)
goto Servo0
Servo0:
Control_Servo(0) ' Ersten Servomotor basierend auf dem Potentiometerwert steuern
If (SW2 = SW_ON) then
Delay_ms(100)
goto Servo1
end If
goto Servo0
Servo1:
Control_Servo(1) ' Zweiten Servomotor basierend auf dem Potentiometerwert steuern
If (SW2 = SW_ON) then
Delay_ms(100)
goto Servo0
end If
goto Servo1
end.
