E-LAB Computers
 

Servo-Treiber

  • 1
  • 2
  • Seite 2 von 2
Harry
Moderator
Avatar
Geschlecht:
Herkunft: zwischen Augsburg und Ulm
Beiträge: 2185
Dabei seit: 03 / 2003
Betreff:

Re: Servo-Treiber

 · 
Gepostet: 09.06.2020 - 14:23 Uhr  ·  #13
Ich habe jetzt mal die Software etwas verbessert:
- die Konfigurationswerte des PCA9685 werden berechnet
- die Servoauflösung wird immer auf 1000 Schritte hoch gerechnet

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;


Was noch fehlt:
- Konfiguration des PCA mit externem Oszillator (viel genauer!)
- Verbesserung der Auflösung durch 2 Monoflops am PCA (hab ich oben mal erwähnt)

Gruss
Harry
Harry
Moderator
Avatar
Geschlecht:
Herkunft: zwischen Augsburg und Ulm
Beiträge: 2185
Dabei seit: 03 / 2003
Betreff:

Re: Servo-Treiber

 · 
Gepostet: 30.06.2020 - 19:18 Uhr  ·  #14
Mein vermutlich letzter Post in diesem Thread ...... Ich habe die schon erwähnte und einfachere Schaltung aufgebaut und getestet und sie funktioniert. Der PCA triggert mit seinem 1. Port die erste Stufe eines 2-stufigen Monoflops. Diese erste Stufe mit 2.5ms triggert nach Ablauf die 2. Stufe mit 17ms und diese sperrt über den OE den PCA9685. Theoretisch und wenn man beide Monoflops genau eingestellt hat, sollte es möglich sein, den PCA auf mindestens 500Hz zu konfigurieren und hat somit >2000 Stufen für die Servobewegung.

Gruss
Harry
Anhänge an diesem Beitrag
Dateiname: ServoBoard-Zusatz.zip
Dateigröße: 59.81 KB
Titel:
Heruntergeladen: 13
  • 1
  • 2
  • Seite 2 von 2
Gewählte Zitate für Mehrfachzitierung:   0

Registrierte in diesem Topic

Aktuell kein registrierter in diesem Bereich

Die Statistik zeigt, wer in den letzten 5 Minuten online war. Erneuerung alle 90 Sekunden.