Code
Const // Konstanten
_PCAFrequency : Float = 25000000; // PCA9685-Frequenz; bei intern 25MHz
_ServoMinTime : Float = 0.800; // in Milli-Sekunden; normal 1-2ms
_ServoMaxTime : Float = 2.200; // in Milli-Sekunden
_ServoRange : Float = 1000; // Bewegungsbereich 1-1000
// 1 bei _ServoMinTime
// 1000 bei _ServoMaxTime
Var // Variablen
_PCAResolution : Float; // Auflösung 1/f/4096
_PCAMinValue : Float; // PCA min.Wert bei ServoMinTime
_PCAMaxValue : Float; // PCA max.Wert bei ServoMaxTime
_PCARange : Float; // PCA-Differenz
PCA9685_OE[@PortC,7] : Bit; // /OE Servo-Controller
PCATest : Byte;
SHi, SLo : Word; // Servo Hi & Lo
Servo : Array[0..3] of Byte; // Lo_on, Hi_on, Lo_off, Hi_off
Transmit : Boolean; // Fehlervariable
Sequenzauswahl : Byte; // 0..15 --> div. Servo-Tests
{ Calculation of the registry values of the servo output }
{ PWM-Frequency: Register $FE prescaler for PWM output frequency }
{ prescale-value = Round(25000000/(4096*f))-1 }
{ e.g. 50Hz --> (25000000/(4096*50Hz))-1 = 121 }
{ }
{ PWM-on-time: Register $06-$09 output0, Register $0A-$0D output1, ..... }
{ Register $FA-$FD all output }
{ t=1/f e.g. t=1/50Hz=0.02s }
{ resolution=t/4096 e.g. 0.02s/4096=0.0000048828125s }
{ first two values=0 --> start-value lo/hi-Byte }
{ value 3&4 --> stop-value lo/hi-Byte }
{ stop-value=on-time*(4096/t) }
{ e.g. 1.8ms: stop-value=0.0018s/(0.02s/4096)=368 }
{ values for on-time 1-2ms @ 50Hz --> 205-410 }
{ }
{ Note: An external clock would be more precise! }
{ enable external clock: TWIOut(PCA9685_Ad,$00,%00110001); }
{ TWIOut(PCA9685_Ad,$00,%01110001); }
{ This bit (6) is a ‘sticky bit’. It cannot be cleared by }
{ writing a logic 0 to it. The EXTCLK bit can only be cleared }
{ by a power cycle or software reset. }
Function Init_PCA9685(Freq:Float):Boolean; // Frequency; range 24-1526Hz
Var PCA : Boolean;
PCAPrescFloat : Float;
PCAPrescale : Byte;
Begin
If Freq in [24..1526]
then
_PCAResolution:=1000000/(Freq*4096); // 1 Bit corresponds x µSeconds
// register value PCA minimum --> Integer!
_PCAMinValue:=Float(Round(_ServoMinTime/_PCAResolution*1000));
// register value PCA maximum --> Integer!
_PCAMaxValue:=Float(Round(_ServoMaxTime/_PCAResolution*1000));
_PCARange:=_PCAMaxValue-_PCAMinValue; // difference max-min
// PCA-divider =(25000000/(4096*f))-1
PCAPrescFloat:=Float(Round((_PCAFrequency/(4096*Freq))-1));
PCAPrescale:=Lo(Trunc(PCAPrescFloat)); // calculated value --> register value
PCA:=TWIOut(PCA9685_Ad,$00,%00110001); // mode reg.1: Bit7: restart disabled
// 6: use internal clock
// 5: register auto-increment
// 4: sleep mode enabled
// 3: does not respond to I2C-bus subaddress 1
// 2: does not respond to I2C-bus subaddress 2
// 1: does not respond to I2C-bus subaddress 3
// 0: responds to LED all call I2C-bus address
mDelay(10);
PCA:=TWIOut(PCA9685_Ad,$FE,PCAPrescale); // prescale-register: 50Hz=121=$79 | 60Hz=100=$65 |100Hz=61=$3C
// value=(25000000/(4096*f))-1
mDelay(10);
PCA:=TWIOut(PCA9685_Ad,$00,%00100001); // mode reg.1: Bit7: restart disabled
// 6: use internal clock
// 5: register auto-increment
// 4: normal mode enabled
// 3: does not respond to I2C-bus subaddress 1
// 2: does not respond to I2C-bus subaddress 2
// 1: does not respond to I2C-bus subaddress 3
// 0: responds to LED all call I2C-bus address
mDelay(10);
PCA:=TWIOut(PCA9685_Ad,$01,%00001100); // mode reg.2: Bit 7-5: reserved
// 4: output logic state not inverted
// 3: outputs change on ACK
// 2: LED outputs are configured with totem pole structure
// 1-0: when OE=1 LEDn = 0
mDelay(10);
PCA9685_OE:=0;
Servo[0]:=0; // PCA-register start value 0
Servo[1]:=0; // PCA-register start value 1
else
PCA:=false;
EndIf;
Return(PCA);
End Init_PCA9685;
Function ServoPos(ServoNr:Byte;Position:Integer):Boolean; // Procedure moves servo to position
// ServoNr port 0-15; >15=all ports
// Position value in 1.._ServoRange
Var ServoPos : Integer;
RetVal : Boolean;
Begin
If Position in [1..Trunc(_ServoRange)]
then
ServoPos:=Trunc(((Float(Position)/_ServoRange)*_PCARange)+_PCAMinValue);
Servo[2]:=Lo(ServoPos);
Servo[3]:=Hi(ServoPos);
If ServoNr in [0..15]
then
RetVal:=TWIOut(PCA9685_Ad,$06+(4*ServoNr),Servo); // $06 --> port 0
else
RetVal:=TWIOut(PCA9685_Ad,$FA,Servo); // $FA-$FD --> all call/ports address
EndIf;
else
RetVal:=false;
EndIf;
Return(RetVal);
End ServoPos;
// Procedure without exit! Only a test ;-)
// Moves servo continuously from MinPos to MaxPos and
// back with step-value in time interval (in ms)
Procedure MoveServo(ServoNr:Byte;MinPos,MaxPos,Step:Integer;Interval:Word);
Var ActPos : Integer;
FWD : Boolean;
Works : Boolean;
Begin
If ((MinPos in [1.._ServoRange]) and (MaxPos in [1.._ServoRange]))
then
FWD:=true;
ActPos:=MinPos;
Loop
ServoPos(ServoNr,ActPos);
If FWD=true
then
FWD:=IncToLim(ActPos,_ServoRange,Step);
else
FWD:=not DecToLim(ActPos,1,Step);
EndIf;
mDelay(Interval);
EndLoop;
EndIf;
End MoveServo;
{------------------------------------------------------------------------------}
{ Main Program }
{$IDATA}
Begin
EnableInts;
InitPorts;
mDelay(1000);
Sequenzauswahl:=PinD and %00001111; // nur die unteren 4 Bits des Ports sind relevant (DIP-Schalter)
Case Sequenzauswahl of
0..3:Transmit:=Init_PCA9685(50); // 50Hz
|
4..7:Transmit:=Init_PCA9685(150); // 150Hz
|
8..11:Transmit:=Init_PCA9685(200); // 200Hz
|
12..15:Transmit:=Init_PCA9685(250); // 250Hz
|
EndCase;
Case (Sequenzauswahl mod 4) of
0:ServoPos($FF,1); // Min-Postition
|
1:ServoPos($FF,1000); // Max-Position
|
2:MoveServo($FF,1,1000,100,500); // Min zu Max alle 500ms 1/10 der Position
|
3:MoveServo($FF,1,1000,999,1000); // Min zu Max jede Sekunde wechselnd
|
EndCase;
- Verbesserung der Auflösung durch 2 Monoflops am PCA (hab ich oben mal erwähnt)
