-         
Ergebnis 1 bis 3 von 3

Thema: malthy Omni 3 Raeder Antrieb von Bascom nach C

  1. #1
    Neuer Benutzer Öfters hier Avatar von ces
    Registriert seit
    06.10.2018
    Beiträge
    11

    Beitrag malthy Omni 3 Raeder Antrieb von Bascom nach C

    Anzeige

    @malthy

    Hallo,
    baue das hier nach.
    Habs aus diesem Forum, moechte was beitragen und hoffe erstmal das meine Uebersetzung des Bascom Quellcode:
    http://www.mtahlers.de/download/omniVehicle_bc.html

    nach ANSI-C 89/90 dazu taugt. Bin ewiger Anfaenger nach etlichen Anlaeufen.

    Edit:

    Erste Zeile in void main(void) geändert:
    vorher:
    // WDTE = 0; // Disable Watchdog timer
    nachher:
    PCA0MD &= ~0x40; // Disable Watchdog timer ()

    Ist der Watchdog timer enabled, führt while(1); nach kurzer Untätigkeit zum Reset der MCU.
    Ist der Watchdog timer disabled, wartet while(1); auf den Interrupt von Timer 0

    Kommentar korrigiert: while(1); // Infinite while loop waiting for
    // an interrupt from Timer0


    Klicke auf die Grafik für eine größere Ansicht

Name:	omni.jpeg
Hits:	12
Größe:	10,3 KB
ID:	33978

    Code:
    // C51 COMPILER V9.55
    // Ansi-C 89/90
    // MCU C8051F410(DK)
    // Quellcode Bascom-AVR: http://www.mtahlers.de/index.php/robotik/omnivehicle
    // Steuerung per Joystick über PC über RS232 nicht vorgesehen, dieser Teil fehlt.
    // Status: Compiler = 0 ERROR(S)
    // Todo: "th" ausrechnen. Radius und Steps/U anpassen.
    //       Routine für Testlauf ohne Joystick.
    //       Das SRAM ? verwenden, limitierte Programmgröße umgehen.		 
    //       http://www.keil.com/support/man/docs/c51/c51_noaregs.htm
    //       Timer0 preload, Init reload value (155) anpassen.
    		 
    // PSM35BYG104 https://www.pollin.de/productdownloads/D310455D.PDF 
    
    //-----------------------------------------------------------------------------
    // Includes
    //-----------------------------------------------------------------------------
    
    #include <compiler_defs.h>
    #include <C8051F410_defs.h>                 // SFR declarations
    
    //-----------------------------------------------------------------------------
    // Pin Declarations
    //-----------------------------------------------------------------------------
    
    sbit Mot_en = P2^0;
    sbit Mot1_stp = P1^3;
    sbit Mot1_dir = P2^1;
    sbit Mot2_stp = P1^4;
    sbit Mot2_dir = P2^2;
    sbit Mot3_stp = P1^5;
    sbit Mot3_dir = P2^3;
    
    signed int Vx; // Dim Vx As Integer // 16 	2 	-32768 to +32767
    signed int Vy;  // Dim Vy As Integer
    signed int V1;  // Dim V1 As Integer
    signed int V2;  // Dim V2 As Integer
    signed int V3;  // Dim V3 As Integer
    signed int V_rot;  // Dim V_rot As Integer
    
    unsigned int Mot1_cnt; // Dim Mot1_cnt As Word //	16 	2 	0 to 65535
    unsigned int Mot1_cnt_th; // Dim Mot1_cnt_th As Word
    unsigned int Mot2_cnt; // Dim Mot2_cnt As Word
    unsigned int Mot2_cnt_th; // Dim Mot2_cnt_th As Word
    unsigned int Mot3_cnt; // Dim Mot3_cnt As Word
    unsigned int Mot3_cnt_th; // Dim Mot3_cnt_th As Word
    
    float frc_part;
    float int_part;
    float Tmp_sng_1; // Dim Tmp_sng_1 As Single // 32 	4 	±1.175494E-38 to ±3.402823E+38
    float Tmp_sng_2; // Dim Tmp_sng_2 As Single
    signed int Tmp_int_1; // Dim Tmp_int_1 As Integer
    
    unsigned char R26OLD;
    unsigned char R27OLD;
    
    //-----------------------------------------------------------------------------
    // Global CONSTANTS
    //-----------------------------------------------------------------------------
    
    float T0_dt = 1.08e-4;  // Const T0_dt = 1.08e-4  
    float Sin_p120 = 0.8660;  // Const Sin_p120 = 0.8660
    float Cos_p120 = -0.5;  // Const Cos_p120 = -0.5
    float Sin_m120 = -0.8660; // Const Sin_m120 = -0.8660
    float Cos_m120 = -0.5;  // Const Cos_m120 = -0.5
    
    //-----------------------------------------------------------------------------
    // Function Prototypes
    //-----------------------------------------------------------------------------
    
    void SYSCLK_Init (void);
    void CLKMUL_Init (void);
    void Oscillator_Init (void);           // Configure the system clock
    void Port_Init (void);                 // Configure the Crossbar and GPIO
    void Ext_Interrupt_Init (void);        // Configure External Interrupts (/INT0
                                           // and /INT1)
    void Timer0_Init (void);
    void Timer0_ISR (void);
    void Set_motor_speed (unsigned char Mot_nr , signed int V);
    int round(float f);
    
    //-----------------------------------------------------------------------------
    // main() Routine
    //-----------------------------------------------------------------------------
    
    void main (void)
    {
    PCA0MD &= ~0x40;  // Disable Watchdog timer
    
       SYSCLK_Init ();
       CLKMUL_Init ();
       Oscillator_Init();                  // Initialize the system clock
       Port_Init ();                       // Initialize crossbar and GPIO
       Ext_Interrupt_Init();               // Initialize External Interrupts
       Timer0_Init ();
    
       Mot_en = 0; // Reset ?
       Set_motor_speed (1 , 0);
       Set_motor_speed (2 , 0); 
       Set_motor_speed (3 , 0);
    
    do{
    //   If New_uart_msg = 1 then UART isr
     
          Vx = 0;
          Vy = 0;
          V_rot = 0; 
          //
          V1 = Vx + V_rot;
          //
          Tmp_sng_1 = Cos_p120 * Vx;
          Tmp_sng_2 = Sin_p120 * Vy;
          Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2;
          Tmp_sng_1 = Tmp_sng_1 + V_rot;
          V2 = round (Tmp_sng_1);
          //
          Tmp_sng_1 = Cos_m120 * Vx;
          Tmp_sng_2 = Sin_m120 * Vy;
          Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2;
          Tmp_sng_1 = Tmp_sng_1 + V_rot;
          V3 = round (Tmp_sng_1);
          //
          Set_motor_speed (1 , V1);
          Set_motor_speed (2 , V2);
          Set_motor_speed (3 , V3);
    
       
    }  
          while(1);                        // Infinite while loop waiting for
                                           // an interrupt from Timer0
    }
    	
    //-----------------------------------------------------------------------------
    // Initialization Subroutines
    //-----------------------------------------------------------------------------
    
    //-----------------------------------------------------------------------------
    // SYSCLK_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This routine initializes the system clock to use the internal 24.5 MHz / 8
    // oscillator as its clock source.  Also enables missing clock detector reset.
    //
    // RSTSRC bit 2 --> MCDRSF: Missing Clock Detector Flag.
    // 0: Read: Source of last reset was not a Missing Clock Detector timeout.
    // Write: Missing Clock Detector disabled.
    // 1: Read: Source of last reset was a Missing Clock Detector timeout.
    // Write: Missing Clock Detector enabled; triggers a reset if a missing clock condition is
    // detected.
    //-----------------------------------------------------------------------------
    
    void SYSCLK_Init (void)
    {
       OSCICN = 0x84;                      // Configure internal oscillator for
                                           // (24.5 / 8 MHz) = 3.062
    
       RSTSRC = 0x04;                      // Enable missing clock detector
    }
    
    //-----------------------------------------------------------------------------
    // CLKMUL_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This routine initializes the Clock Multiplier using the Internal Oscillator
    // as the source.
    //
    //-----------------------------------------------------------------------------
    
    void CLKMUL_Init (void)
    {
       char i;
    
       CLKMUL = 0x00;                      // Reset the Multiplier by writing 0x00
                                           // to register CLKMUL.
    
       CLKMUL |= 0x00;                     // Select the Multiplier input source
                                           // via the MULSEL bits.
                                           // '00'b = Internal Oscillator/2
    
       CLKMUL |= 0x18;                     // Select the Multiplier output scaling
                                           // factor via the MULDIV bits
                                           // '110'b = scale by 2/6
                                           // Clock Multiplier output =
                                           // (IntOsc/2) * 2/6 = 16.33 MHz
    
       CLKMUL |= 0x80;                     // Enable the Multiplier with the MULEN
                                           // bit (CLKMUL | = 0x80).
    
       for (i = 12; i > 0; i--);           // Delay for >5 µs.
                                           // At 3.0625 MHz SYSCLK ~ 326 ns, so
                                           // 5 us is 16 SYSCLK counts.
                                           // DJNZ = 2 clocks.
    
       CLKMUL |= 0xC0;                     // Initialize the Multiplier with the
                                           // MULINIT bit (CLKMUL | = 0xC0).
    
       while ((CLKMUL & 0x20) != 0x20);    // Poll for MULRDY => 1.
    }
    
    //-----------------------------------------------------------------------------
    // OSCILLATOR_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function initializes the system clock to use the internal oscillator
    //
    //-----------------------------------------------------------------------------
    
    void OSCILLATOR_Init (void)
    {
       OSCICN |= 0x02;                     // Configure internal oscillator for
                                           // 24.5/8 Mhz (~3 Mhz)
    								   
    }
    
    //-----------------------------------------------------------------------------
    // PORT_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function configures the crossbar and ports pins.
    //
    //-----------------------------------------------------------------------------
    // CROSSBAR
    //-----------------------------------------------------------------------------
    /*
    Pin Signal/Function
    P0.0    TX0, set to digital, pushpull
    P0.1    RX0, set to digital, pushpull 
    P0.2    SCK, set to digital, pushpull
    P0.3    MISO, set to digital, pushpull
    P0.4    MOSI, set to digital, pushpull
    P0.5    NSS, set to digital, pushpull
    P0.6    /INT0, set to digital, pushpull, skipped
    P0.7    /INT1, set to digital, pushpull, skipped
    
    P1.0 - P1.2    ????, set to digital, pushpull, skipped
    P1.3 - P1.7    GPIO, set to digital, pushpull, skipped
    
    P2.0 - P2.7    GPIO, set to digital, pushpull, skipped
    
    
    Bitwise operations, example
    {
    P2 = 0x19  // assignment operation
    P2 |= 0xC1 // bitwise OR
    P2 &= ~0x33 // bitwise AND
    }
    */
    //-----------------------------------------------------------------------------
    
    void PORT_Init (void)
    {
       P0MDIN = 0xFF; // set to "no analog input" = digital
       P1MDIN = 0xFF; // set to "no analog input" = digital
       P2MDIN = 0xFF; // set to "no analog input" = digital
    
       P0MDOUT = 0xFF; // set to pushpull
       P1MDOUT = 0xFF; // set to pushpull
       P2MDOUT = 0xFF; // set to pushpull
    
       P0SKIP = 0xC0;
       P1SKIP = 0xF8;
       P2SKIP = 0xFF;
    
       XBR0 = 0x03; // SPI (P0.0 - P0.3) enabled, UART (P0.4 - P0.5) enabled
       XBR1 = 0xC0; // WeakPUD disabled, Crossbar enabled
    }
    
    //-----------------------------------------------------------------------------
    // Ext_Interrupt_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function configures and enables /INT0 and /INT1 (External Interrupts)
    // as negative edge-triggered.
    //
    //-----------------------------------------------------------------------------
    
    void Ext_Interrupt_Init (void)
    {
       TCON = 0x05; // /INT 0 and /INT 1 are edge triggered
    
       IT01CF = 0x76; // /INT0 active low; /INT0 on P0.6
                      // /INT1 active low; /INT1 on P0.7
    
       EIE1 = 0x00; // Extended Interrupt disabled
    
    // SFR IE
    
       EX0 = 1; // /INT0 interrupt enabled
       ET0 = 1; // masking Timer 0 interrupt enabled 
       EX1 = 1; // /INT1 interrupt enabled
       ET1 = 1; // masking of Timer 1 interrupt enabled
       ES0 = 1; // UART0 interrupt enabled
       ET2 = 1; // masking of Timer 2 interrupt enabled 
    //   ESPIO = 1; // masking of SPIO interrupt enabled  
       EA = 1; // Global Interrupt enabled
    }
    
    //-----------------------------------------------------------------------------
    // Timer0_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // Configure Timer0 to 8-bit auto-reload and generate an interrupt after the
    // maximum possible time (TMR0RL = 0x0000).
    //
    //-----------------------------------------------------------------------------
    
    void Timer0_Init (void)
    {
       TMOD = 0xb2;			   
       CKCON = 0xfd;                    // Timer0 clocked based on SYSCLK;
       TL0 = 0x00;                   // Init reload value
       TH0 = 0xff;                   // set to reload immediately
       ET0     = 1;                        // Enable Timer0 interrupts
       TR0     = 1;                        // Start Timer0
    }
    
    //-----------------------------------------------------------------------------
    // Interrupt Service Routines
    //-----------------------------------------------------------------------------
    
    //-----------------------------------------------------------------------------
    // /INT0 ISR
    //-----------------------------------------------------------------------------
    //
    // Whenever a negative edge appears on P0.6, ... is toggled.
    // The interrupt pending flag is automatically cleared by vectoring to the ISR
    //
    //-----------------------------------------------------------------------------
    
    void INT0_ISR (void) interrupt 0
    {
    ; //   Steuerung, z.B. Joystick mit CY7C63001A-PC 
    }
    
    //-----------------------------------------------------------------------------
    // /INT1 ISR
    //-----------------------------------------------------------------------------
    //
    // Whenever a negative edge appears on P0.7, ... is toggled.
    // The interrupt pending flag is automatically cleared by vectoring to the ISR
    //
    //-----------------------------------------------------------------------------
    
    void INT1_ISR (void) interrupt 2
    {
    ;
    }
    
    //-----------------------------------------------------------------------------
    // Timer0_ISR
    //-----------------------------------------------------------------------------
    //
    // This routine changes the state of the ... whenever Timer0 overflows.
    //
    //-----------------------------------------------------------------------------
    
    void Timer0_ISR (void) interrupt 1 
    {
    TF0 = 0; // Clear Timer0 interrupt flag
    
    
    // timer0 preload
    TL0 = 0x73;                   // Init reload value
    
    // alle stp auf 0
    Mot1_stp = 0;
    Mot2_stp = 0;
    Mot3_stp = 0;
    
    //--V1
    R26OLD = Mot1_cnt_th;
    R27OLD = Mot1_cnt;
    
    if (R26OLD == 255) 
    {
    goto V1_incr; 
    }
    
    if (R27OLD < R26OLD) 
    {
    goto V1_incr; 
    }
    
    Mot1_stp = 1;
    R27OLD = 0;
    goto V1_setcnt;
    
    V1_incr:
    R27OLD = R27OLD + 1;
    V1_setcnt:
    Mot1_cnt = R27OLD;
    
    //-- V2
    R26OLD = Mot2_cnt_th;
    R27OLD = Mot2_cnt;
    
    if (R26OLD == 255) 
    {
    goto V2_incr; 
    }
    
    if (R27OLD < R26OLD) 
    {
    goto V2_incr; 
    }
    
    Mot2_stp = 1;
    R27OLD = 0;
    goto V2_setcnt;
    
    V2_incr:
    R27OLD = R27OLD + 1;
    
    V2_setcnt:
    Mot2_cnt = R27OLD;
    
    //-- V3
    R26OLD = Mot3_cnt_th;
    R27OLD = Mot3_cnt;
    
    if (R26OLD == 255) 
    {
    goto V3_incr; 
    }
    
    if (R27OLD < R26OLD) 
    {
    goto V3_incr; 
    }
    
    Mot3_stp = 1;
    R27OLD = 0;
    goto V3_setcnt;
    
    V3_incr:
    R27OLD = R27OLD + 1;
    
    V3_setcnt:
    Mot3_cnt = R27OLD;  
    
    }    
    
    //-----------------------------------------------------------------------------
    // Subroutines
    //-----------------------------------------------------------------------------
    
    void Set_motor_speed (unsigned char Mot_nr , signed int V){
     
       switch (Mot_nr) {
          case 1:
             if (V < 0) {
                Mot1_dir = 1;
                V = V * -1; }
             else {
                Mot1_dir = 0; }
             
             if (V == 0) {
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V * T0_dt;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                Tmp_int_1 = round (Tmp_sng_1); }
             
             if (Tmp_int_1 < 5) {
    			 Tmp_int_1 = 5; }
    
             if (Tmp_int_1 > 255) {
                 Tmp_int_1 = 255; }
             Mot1_cnt_th = Tmp_int_1;
          break;         
          case 2:
             if (V < 0) {
                Mot2_dir = 1;
                V = V * -1; }
             else {
                Mot2_dir = 0; }
             
             if (V == 0) {
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V * T0_dt;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                Tmp_int_1 = round (Tmp_sng_1); }
             
             if (Tmp_int_1 < 5) {
    			 Tmp_int_1 = 5; }
    
             if (Tmp_int_1 > 255) {
                Tmp_int_1 = 255;
                Mot2_cnt_th = Tmp_int_1; }
          break;
          case 3:
             if (V < 0) {
                Mot3_dir = 1;
                V = V * -1; }
             else {
                Mot3_dir = 0; }
             
             if (V == 0) { 
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V * T0_dt;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                Tmp_int_1 = round (Tmp_sng_1); }
             if (Tmp_int_1 < 5) {
    			 Tmp_int_1 = 5; }
    			 
             if (Tmp_int_1 > 255) {
    			 Tmp_int_1 = 255; }
             Mot3_cnt_th = Tmp_int_1;
           break;        
           default:
           ;
           break;        
           
    	   } 
    }
    
    signed int round (float f)
    {
      if (f > 0) 
      {
      return (int)(f + 0.5);
      }
    
      return (int)(f - 0.5);
    }
    
    // End Subroutines
    //-----------------------------------------------------------------------------
    // End Of File
    //-----------------------------------------------------------------------------
    Geändert von ces (02.03.2019 um 12:41 Uhr)

  2. #2
    Neuer Benutzer Öfters hier Avatar von ces
    Registriert seit
    06.10.2018
    Beiträge
    11
    Update: Functions round(void), Set_motor_speed(void). Keine Konstanten.
    Denke das ich so eventuelle Probleme mit Registern umschifft habe.
    Auf Vollschritt umgestellt.

    Muß die Motoren oft anschubsen, Motor 2 rotiert nie, sobald ich den Timer Preload verwende singen sie nur noch.
    Verkabelung stimmt und Pololu 4988 Besonderheiten beachtet. Werde die Strombregrezung nochmal einstellen.
    Mach noch ein Foto.
    Kann es ohne Steuerung wohl nicht richtig testen, nur werte für Vx, Vy, Vrot vorzugeben klappt wohl nicht.
    Ausserdem hab ich immer noch das Problem die Motoren zu befestigen. In Heisskleber mit Winkel gießen oder verwenden von Schellen und Kabelbinder fehlgeschlagen.
    Nächster Versuch ist mit einem 320kg Alleskleber.

    Edit: Ohne Timer Preload funzt es jetzt mit dem Update soweit ich es ausprobieren konnte. Motor 2 geht auch.
    Muß immer noch anschubsen. Sind wohl bei meinem ersten Versuch Register überschrieben worden.

    Mein Rechenweg:
    Code:
    Phasen                      p = 2
    Schrittwinkel               1,8 [Grad] Vollschrittbetrieb
    Schrittanzahl               n = 200/U Vollschrittbetrieb
    Mikroschritte               4/Vollschritt
    Schrittwinkel               0,9 [Grad] Halbschrittbetrieb
    Schrittanzahl               n = 400/U Halbschrittbetrieb 
    Mikroschritte               8/Halbschritt
    
    Schrittwinkel = Mikroschrittwinkel?
    
    V = Schritt-Geschwindigkeit
    V = 1 / t 
    T0_dt = Aufrufintervall
    t = (th + 1) * Aufrufintervall
    V = 1 / ((th + 1) * Aufrufintervall)
    th = 0 bis 255 #th ist vom Typ Byte
    th = ((1 / (V * Aufrufintervall)) - 1
    sysclock = 24 [MHz]
    r = Radius
    r = 120 [mm]
    ω = Winkelgeschwindigkeit
    ω = 0,5 * pi [rad/sec] 
    ω * r = Bahngeschwindigkeit
    V_rot [0-255] = Bahngeschwindigkeit 
    Rad Durchmesser 48 [mm]
    Rad Umfang 151 [mm]
    Mindestabstand pro Mikroschritt = 0,00003 [sec] = 30 [usec]
    
    //-----------------------------------------------------------------------------
    // Vollschrittbetrieb
    //-----------------------------------------------------------------------------
    
    Berechnung in Vollschritten [FS]:
    200 [FS/U] * 4 [Mikroschritte/FS] = 800 [Mikroschritte/U]
    
    800 Mikroschritte * 0,00003 [sec] = 0,024 [sec/U]
    4 Mikroschritte * 0,00003 [sec] = 0,00012 [sec/FS]
    
    th = Abstand zwischen den Pulsen [sec/FS] am Pololu Eingang "step". 
    Mindestabstand  zwischen den Pulsen = 0,00012 [sec/FS]
    
    Aufrufintervall muß >= 0,00012 [sec/FS] sein. => 0,0001275 [sec/FS]
    200 [FS/U] * 0,00012 [sec/FS] = 0,024 [sec/U] 
    
    1 / 0,024 [sec/U] = 42 [U/sec] 
    41,67 [U/sec] * 60 = 2500 [U/min]
    1 / 0,00012 [sec/FS] = 8333 [FS/sec]
    
    (360 [Grad] / 200 [FS]) = 1,8 [Grad/FS]
    1,8 [Grad/FS] * 8333 [FS/sec] = 15000 [Grad/sec]
    
    = 41,67 [U/sec]
    = 2500 [U/min]
    
    
    Maximalgeschwindigkeit Vollschritt:
    th = (1 / (8333 [FS/sec] * 0,00012 [sec/FS])) -1 = 0
    Vmax. = 8333 [FS/sec]
    1th = 8333 [FS/sec] / 255 = 32,69 [FS/sec]
    
    Robot soll sich in 2 [sec] 1x um sich selbst drehen.
    Bahngeschwindigkeit = ω * r = 1 * pi [rad/sec] * 0,12 [m] = 0,37699 [m/sec] #eine halbe Robot [U/sec]
    0,37699 [m/sec]  / 0,151 [m/U] = 2,4966 [U/sec] #Rad 
    2,4966 [U/sec] * 200 [FS/U] = 499,32 [FS/sec]
    V_rot = 255 - 499,32 [FS/sec] / 32,69 [FS/sec] = 239,73  
    
    ISR:
    Aufrufintervall T0_dt >= 0,00012 [sec/FS]
    
    Timer Frequenz:
    TF = 24[MHz] / 12 = 2 [MHz]
    Timer sec/Cycle: 
    TPuls = 1 / 2 [MHz] = 0,0000005 [sec] = 0,5 [usec]
    
    T0_dt = TPuls * 255 = 0,0001275 [sec] = 127,5 [usec] = 12.75e-5 [sec]
    
    
    Abarbeitung_Timer0_ISR [sec]:
    
    Befehle ASM = 564
    ORL		93  x1
    MOV		55  x1 
    XRL		96	x1
    JNB		05  x2 = 10
    ANL		57  x1
    ADD		04  x1
    JB		73  x2 = 146
    INC           77  x1
    AJMP         01  x2 = 2
    ADDC	        65  x1
    RETI	        08  x2 = 16
    ACALL        02  x2 = 4
    JMP           13  x2 = 26
    		---------
    		651 Cycle
    
    1 / 24 [MHz] = 4,166666666666667e-8 [sec]
    Abarbeitung_Timer0_ISR = 651 Cycle * 4,166666666666667e-8 [sec] = 0,000027125 [sec] = 271.25e-7 [sec] = 27.125 [usec]
    Abarbeitung_Timer0_ISR [usec] / TPuls [usec]:
    27.125 [usec] / 0,5 [usec] = 54,25 TPulse
    
    Timer Preload = 255 + 1 - Abarbeitung_Timer0_ISR [usec] / TPuls [usec]
    Timer Preload = 256 - 54,25 = 201,75 ~ 202 = 0xCA

    Code:
    // C51 COMPILER V9.55 COPYRIGHT KEIL ELEKTRONIK GmbH 1987 - 2009
    // Ansi-C 89/90
    // MCU C8051F410(DK) 24MHz
    // Quellcode Bascom-AVR: http://www.mtahlers.de/index.php/robotik/omnivehicle
    // Steuerung per Joystick über PC über RS232 nicht vorgesehen, dieser Teil fehlt.
    // Status: Program Size: data=53.0 xdata=0 code=2081
    //         LINK/LOCATE RUN COMPLETE.  0 WARNING(S),  0 ERROR(S)
    //         RS(256) PL(68) PW(78)
    // Todo: "th" ausrechnen. Radius und Steps/U anpassen. -Erledigt
    //       Routine für Testlauf ohne Joystick.
    //       http://www.keil.com/support/man/docs/c51/c51_noaregs.htm -Register mit "void" ok?	 
    //       Timer0 preload, Init reload value ausrechnen. -Erledigt
    // Schrittmotor: PSM35BYG104 https://www.pollin.de/productdownloads/D310455D.PDF 
    //               Bei einem Rad Durchmesser von nur 4,8 cm der einzig mögliche (3,5 x 3,5 cm).
    // Räder: https://www.interroll.de/fileadmin/products/de/Resources_pdf_9007199597364747.pdf
    // Autor: ces @
    //-----------------------------------------------------------------------------
    // Includes
    //-----------------------------------------------------------------------------
    
    #include <compiler_defs.h>
    #include <C8051F410_defs.h>                 // SFR declarations
    
    //-----------------------------------------------------------------------------
    // Pin Declarations
    //-----------------------------------------------------------------------------
    
    sbit Mot_en = P2^0;
    sbit Mot1_stp = P1^3;
    sbit Mot1_dir = P2^1;
    sbit Mot2_stp = P1^4;
    sbit Mot2_dir = P2^2;
    sbit Mot3_stp = P1^5;
    sbit Mot3_dir = P2^3;
    
    signed int Vx;  // Dim Vx As Integer // 16 	2 	-32768 to +32767
    signed int Vy;  // Dim Vy As Integer
    signed int V1;  // Dim V1 As Integer
    signed int V2;  // Dim V2 As Integer
    signed int V3;  // Dim V3 As Integer
    signed int V_rot;  // Dim V_rot As Integer
    
    unsigned int Mot1_cnt;    // Dim Mot1_cnt As Word //	16 	2 	0 to 65535
    unsigned int Mot1_cnt_th; // Dim Mot1_cnt_th As Word
    unsigned int Mot2_cnt;    // Dim Mot2_cnt As Word
    unsigned int Mot2_cnt_th; // Dim Mot2_cnt_th As Word
    unsigned int Mot3_cnt;    // Dim Mot3_cnt As Word
    unsigned int Mot3_cnt_th; // Dim Mot3_cnt_th As Word
    
    float frc_part;
    float int_part;
    float Tmp_sng_1; // Dim Tmp_sng_1 As Single // 32 	4       ±1.175494E-38 to ±3.402823E+38
    float Tmp_sng_2; // Dim Tmp_sng_2 As Single
    signed int Tmp_int_1; // Dim Tmp_int_1 As Integer
    
    unsigned char R26OLD;
    unsigned char R27OLD;
    
    
    //-----------------------------------------------------------------------------
    // Function Prototypes
    //-----------------------------------------------------------------------------
    
    void SYSCLK_Init (void);
    void CLKMUL_Init (void);
    void Oscillator_Init (void);           // Configure the system clock
    void Port_Init (void);                 // Configure the Crossbar and GPIO
    void Ext_Interrupt_Init (void);        // Configure External Interrupts (/INT0
                                           // and /INT1)
    void Timer0_Init (void);
    void Timer0_ISR (void);
    void Set_motor_speed_1 (void); 
    void Set_motor_speed_2 (void); 
    void Set_motor_speed_3 (void); 
    void round (void);
    
    //-----------------------------------------------------------------------------
    // main() Routine
    //-----------------------------------------------------------------------------
    
    void main (void) {
    
       PCA0MD &= ~0x40;                 // Disable Watchdog timer ()
    
       Oscillator_Init();               // Initialize the Oscillator 
       CLKMUL_Init ();                  // Initialize the Clock Multiplier
       SYSCLK_Init ();                  // Initialize the System Clock
       Port_Init ();                    // Initialize crossbar and GPIO
       Ext_Interrupt_Init();            // Initialize External Interrupts
       Timer0_Init ();
    
    // Mot_en = 0;                      // Pololu 4988: Brücke zwischen Reset und Sleep = Enable
       V1 = 0;
       Set_motor_speed_1 ();
       V2 = 0;
       Set_motor_speed_2 ();
       V3 = 0;
       Set_motor_speed_3 ();
    
    
    
        do{
                                      
          // Steuerung: If INT0 = 1 then INT0_ISR
    
          Vx = 0;
          Vy = 0;
          V_rot = 0; 
     
          V1 = Vx + V_rot;
    
          Tmp_sng_1 = -0.5 * Vx;
          Tmp_sng_2 = 0.8660 * Vy;
          Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2;
          Tmp_sng_1 = Tmp_sng_1  + V_rot;
    
          if (Tmp_sng_1 > 0) {
          V2 = (int)(Tmp_sng_1 + 0.5); }
          else {
          V2 = (int)(Tmp_sng_1 - 0.5); }
    
          Tmp_sng_1 = -0.5 * Vx;
          Tmp_sng_2 = -0.8660 * Vy;
          Tmp_sng_1 = Tmp_sng_1 - Tmp_sng_2;
          Tmp_sng_1 = Tmp_sng_1 + V_rot;
    
          if (Tmp_sng_1 > 0) {
          V3 = (int)(Tmp_sng_1 + 0.5); }
          else {
          V3 = (int)(Tmp_sng_1 - 0.5); }
    
          Set_motor_speed_1 (); 
          Set_motor_speed_2 (); 
          Set_motor_speed_3 ();
          
          }  
          while(1);  // Infinite while loop waiting for an interrupt from Timer0
    }
    	
    //-----------------------------------------------------------------------------
    // Initialization Subroutines
    //-----------------------------------------------------------------------------
    
    //-----------------------------------------------------------------------------
    // SYSCLK_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This routine initializes the system clock to use the internal 24 MHz / 1
    // oscillator as its clock source.  Also enables missing clock detector reset.
    //
    // RSTSRC bit 2 --> MCDRSF: Missing Clock Detector Flag.
    // 0: Read: Source of last reset was not a Missing Clock Detector timeout.
    // Write: Missing Clock Detector disabled.
    // 1: Read: Source of last reset was a Missing Clock Detector timeout.
    // Write: Missing Clock Detector enabled; triggers a reset if a missing clock condition is
    // detected.
    //-----------------------------------------------------------------------------
    
    void SYSCLK_Init (void) 
    {
       CLKSEL = 0x00;                          // Clock Select Internal Oscillator
    
       RSTSRC = 0x04;                      // Enable missing clock detector
    }
    
    //-----------------------------------------------------------------------------
    // CLKMUL_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This routine initializes the Clock Multiplier using the Internal Oscillator
    // as the source.
    //
    //-----------------------------------------------------------------------------
    
    void CLKMUL_Init (void)
    {
    //   char i;
    
       CLKMUL = 0x00;                      // Reset the Multiplier by writing 0x00
                                           // to register CLKMUL and disable.
    /*
       CLKMUL |= 0x00;                     // Select the Multiplier input source
                                           // via the MULSEL bits.
                                           // '00'b = Internal Oscillator/2
    
       CLKMUL |= 0x18;                     // Select the Multiplier output scaling
                                           // factor via the MULDIV bits
                                           // '110'b = scale by 2/6
                                           // Clock Multiplier output =
                                           // (IntOsc/2) * 2/6 = 16.33 MHz
    
       CLKMUL |= 0x80;                     // Enable the Multiplier with the MULEN
                                           // bit (CLKMUL | = 0x80).
    
       for (i = 12; i > 0; i--);           // Delay for >5 µs.
                                           // At 3.0625 MHz SYSCLK ~ 326 ns, so
                                           // 5 us is 16 SYSCLK counts.
                                           // DJNZ = 2 clocks.
    
       CLKMUL |= 0xC0;                     // Initialize the Multiplier with the
                                           // MULINIT bit (CLKMUL | = 0xC0).
    
       while ((CLKMUL & 0x20) != 0x20);    // Poll for MULRDY => 1.
    */
    
    }
    
    //-----------------------------------------------------------------------------
    // OSCILLATOR_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function initializes the system clock to use the internal oscillator
    //
    //-----------------------------------------------------------------------------
    
    void OSCILLATOR_Init (void)
    {
       OSCICN = 0xC7;                     // Configure internal oscillator for
                                           // 24 MHz / 1
    								   
    }
    
    //-----------------------------------------------------------------------------
    // PORT_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function configures the crossbar and ports pins.
    //
    //-----------------------------------------------------------------------------
    // CROSSBAR
    //-----------------------------------------------------------------------------
    /*
    Pin Signal/Function
    P0.0    TX0, set to digital, pushpull
    P0.1    RX0, set to digital, pushpull 
    P0.2    SCK, set to digital, pushpull
    P0.3    MISO, set to digital, pushpull
    P0.4    MOSI, set to digital, pushpull
    P0.5    NSS, set to digital, pushpull
    P0.6    /INT0, set to digital, pushpull, skipped
    P0.7    /INT1, set to digital, pushpull, skipped
    
    P1.0 - P1.2    ????, set to digital, pushpull, skipped
    P1.3 - P1.7    GPIO, set to digital, pushpull, skipped
    
    P2.0 - P2.7    GPIO, set to digital, pushpull, skipped
    */
    //-----------------------------------------------------------------------------
    
    void PORT_Init (void)
    {
       P0MDIN = 0xFF; // set to "no analog input" = digital
       P1MDIN = 0xFF; // set to "no analog input" = digital
       P2MDIN = 0xFF; // set to "no analog input" = digital
    
       P0MDOUT = 0xFF; // set to pushpull
       P1MDOUT = 0xFF; // set to pushpull
       P2MDOUT = 0xFF; // set to pushpull
    
       P0SKIP = 0xC0;
       P1SKIP = 0xF8;
       P2SKIP = 0xFF;
    
       XBR0 = 0x03; // SPI (P0.0 - P0.3) enabled, UART (P0.4 - P0.5) enabled
       XBR1 = 0xC0; // WeakPUD disabled, Crossbar enabled
    }
    
    //-----------------------------------------------------------------------------
    // Ext_Interrupt_Init
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // This function configures and enables /INT0 and /INT1 (External Interrupts)
    // as negative edge-triggered.
    //
    //-----------------------------------------------------------------------------
    
    void Ext_Interrupt_Init (void)
    {
       TCON = 0x05; // /INT 0 and /INT 1 are edge triggered
    
       IT01CF = 0x76; // /INT0 active low; /INT0 on P0.6
                      // /INT1 active low; /INT1 on P0.7
    
       EIE1 = 0x00; // Extended Interrupt disabled
    
    // SFR IE
    
       EX0 = 1;   // /INT0 interrupt enabled
       ET0 = 0;   // masking Timer 0 interrupt enabled 
       EX1 = 1;   // /INT1 interrupt enabled
       ET1 = 0;   // masking of Timer 1 interrupt enabled
       ES0 = 1;   // UART0 interrupt enabled
       ET2 = 0;   // masking of Timer 2 interrupt enabled 
       EA = 1;    // Global Interrupt enabled
    }
    
    //-----------------------------------------------------------------------------
    // Timer0_Init ()
    //-----------------------------------------------------------------------------
    //
    // Return Value : None
    // Parameters   : None
    //
    // Configure Timer0 to 8-bit auto-reload and generate an interrupt after the
    // maximum possible time (TMR0RL = 0x0000).
    //
    //-----------------------------------------------------------------------------
    
    void Timer0_Init (void)
    {
       TMOD = 0x32;	// Timer0: Mode 2: 8-bit counter/timer with auto-reload
                    // Timer1: Mode 3: inactive		   
    
       CKCON = 0xfc;                 // Timer0/1 clocked based on SYSCLK
       TH0 = 0x00;                   // set to reload immediately
       ET0     = 1;                  // Enable Timer0 interrupts
       TR0     = 1;                  // Start Timer0
    }
    
    //-----------------------------------------------------------------------------
    // Interrupt Service Routines
    //-----------------------------------------------------------------------------
    
    //-----------------------------------------------------------------------------
    // /INT0 ISR
    //-----------------------------------------------------------------------------
    //
    // Whenever a negative edge appears on P0.6, ... is toggled.
    // The interrupt pending flag is automatically cleared by vectoring to the ISR
    //
    //-----------------------------------------------------------------------------
    
    void INT0_ISR (void) interrupt 0
    {
    ; //   Steuerung
    }
    
    //-----------------------------------------------------------------------------
    // /INT1 ISR
    //-----------------------------------------------------------------------------
    //
    // Whenever a negative edge appears on P0.7, ... is toggled.
    // The interrupt pending flag is automatically cleared by vectoring to the ISR
    //
    //-----------------------------------------------------------------------------
    
    void INT1_ISR (void) interrupt 2
    {
    ;
    }
    
    //-----------------------------------------------------------------------------
    // Timer0_ISR
    //-----------------------------------------------------------------------------
    //
    // This routine changes the state of the motor whenever Timer0 overflows.
    //
    //-----------------------------------------------------------------------------
    
    void Timer0_ISR (void) interrupt 1 
    {
    // TF0 = 0;     // Clear Timer0 interrupt flag 
    // TL0 = 0xCA;  // timer0 preload. 
    
    // alle stp auf 0
    Mot1_stp = 0;
    Mot2_stp = 0;
    Mot3_stp = 0;
    
    //--V1
    R26OLD = Mot1_cnt_th;
    R27OLD = Mot1_cnt;
    
    if (R26OLD == 255) {
    goto V1_incr; }
    
    if (R27OLD < R26OLD) {
    goto V1_incr; }
    
    Mot1_stp = 1;
    R27OLD = 0;
    goto V1_setcnt;
    
    V1_incr:
    R27OLD = R27OLD + 1;
    V1_setcnt:
    Mot1_cnt = R27OLD;
    
    //-- V2
    R26OLD = Mot2_cnt_th;
    R27OLD = Mot2_cnt;
    
    if (R26OLD == 255) {
    goto V2_incr; }
    
    if (R27OLD < R26OLD) {
    goto V2_incr; }
    
    Mot2_stp = 1;
    R27OLD = 0;
    goto V2_setcnt;
    
    V2_incr:
    R27OLD = R27OLD + 1;
    
    V2_setcnt:
    Mot2_cnt = R27OLD;
    
    //-- V3
    R26OLD = Mot3_cnt_th;
    R27OLD = Mot3_cnt;
    
    if (R26OLD == 255) {
    goto V3_incr; }
    
    if (R27OLD < R26OLD) {
    goto V3_incr; }
    
    Mot3_stp = 1;
    R27OLD = 0;
    goto V3_setcnt;
    
    V3_incr:
    R27OLD = R27OLD + 1;
    
    V3_setcnt:
    Mot3_cnt = R27OLD;  
    
    }    
    
    //-----------------------------------------------------------------------------
    // Subroutines
    //-----------------------------------------------------------------------------
    
    void Set_motor_speed_1 (void) { 
       
             if (V1 < 0) {
                Mot1_dir = 1;
                V1 = V1 * -1; }
             else {
                Mot1_dir = 0; }
             
             if (V1 == 0) {
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V1 * 12.75e-5;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                round (); }
             
             if (Tmp_int_1 < 5) {
                 Tmp_int_1 = 5; }
    
             if (Tmp_int_1 > 255) {
                 Tmp_int_1 = 255; }
             Mot1_cnt_th = Tmp_int_1;
    }        
    
    void Set_motor_speed_2 (void) { 
    
             if (V2 < 0) {
                Mot2_dir = 1;
                V2 = V2 * -1; }
             else {
                Mot2_dir = 0; }
             
             if (V2 == 0) {
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V2 * 12.75e-5;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                round (); }
                 
             if (Tmp_int_1 < 5) {
                 Tmp_int_1 = 5; }
    
             if (Tmp_int_1 > 255) {
                Tmp_int_1 = 255; }
             Mot2_cnt_th = Tmp_int_1; 
    }
    
    void Set_motor_speed_3 (void) { 
    
             if (V3 < 0) {
                Mot3_dir = 1;
                V3 = V3 * -1; }
             else {
                Mot3_dir = 0; }
             
             if (V3 == 0) { 
                Tmp_int_1 = 255; }
             else {
                Tmp_sng_1 = V3 * 12.75e-5;
                Tmp_sng_1 = 1 / Tmp_sng_1;
                Tmp_sng_1 = Tmp_sng_1 - 1;
                round (); }
    
             if (Tmp_int_1 < 5) {
                 Tmp_int_1 = 5; }
    			 
             if (Tmp_int_1 > 255) {
                 Tmp_int_1 = 255; }
             Mot3_cnt_th = Tmp_int_1;
    }
    
    void round (void) {
    
             if (Tmp_sng_1 > 0) {
             Tmp_int_1 = (int)(Tmp_sng_1 + 0.5); }
             else {
             Tmp_int_1 = (int)(Tmp_sng_1 - 0.5); }
    }
    
    
    // End Subroutines
    //-----------------------------------------------------------------------------
    // End Of File
    //-----------------------------------------------------------------------------
    Klicke auf die Grafik für eine größere Ansicht

Name:	SAM_1475.jpg
Hits:	8
Größe:	64,9 KB
ID:	34051

    Klicke auf die Grafik für eine größere Ansicht

Name:	SAM_1478.jpg
Hits:	7
Größe:	102,6 KB
ID:	34052
    Geändert von ces (17.03.2019 um 10:53 Uhr)

  3. #3
    Neuer Benutzer Öfters hier Avatar von ces
    Registriert seit
    06.10.2018
    Beiträge
    11
    Neuer Ansatz mit KI. So wie ich es verstehe gehts ums sammeln von mundgerechte Daten und deren Verwendung.
    Warum nicht das Programm sehr klein halten und die immer selben Werte nicht berechnen lassen sondern gleich
    zur Verfügung stellen.

    Entweder durch das füllen von 3 Arrays mit den Delays (Vorzeichen als Drehrichtung) durch 3 Formeln
    oder das Einfügen der Daten in den Code oder Einlesen einer Datei.
    Einlesen wird wohl nicht gehen mit einer MCU.

    Hier meine für diesen Zweck erstellte Librecalc Tabelle: OmniDelay360.zip

    Edit:
    SRAM bei 8051 und AVR zu klein für sowas.
    Probiers jetzt mal mit einem ESP8266 und esp8266_basic_interpreter
    So stelle ich mir das vor:

    REM 363*3*2 = 2178 BYTE

    REM CONFIG SINGLE = SCIENTIFIC , DIGITS = 3
    CONFIG BASE=0

    DIM M1(363) AS INTEGER
    DIM M2(363) AS INTEGER
    DIM M3(363) AS INTEGER
    REM 361+362 ROTATE L -45:-45:-45 R 45:45:45
    REM 363 ROBOT STOP 90:90:90

    REM INPUT RADIUS
    RAD=90

    FOR I = 0 TO 360
    IF I>=1 AND I<=180
    THEN M1(I)=INT((COS(I)*-1-RAD)*1000)
    ELSE M1(I)=INT((RAD-COS(I)*RAD)*1000)
    END IF

    IF I>=61 AND I<=260
    THEN M2(I)=INT(((COS(I)*RAD*COS(120)-SIN(I)*RAD*SIN(120))-RAD)*1000)
    ELSE M2(I)=INT(((COS(I)*RAD*COS(120)-SIN(I)*RAD*SIN(120))*-1-RAD)*1000)
    END IF

    IF I>=300 OR I<=119
    THEN M3(I)=INT(((COS(I)*RAD*COS(120)-SIN(I)*RAD*SIN(120))-RAD)*1000)
    ELSE M3(I)=INT(((COS(I)*RAD*COS(-120)-SIN(I)*RAD*SIN(-120))*-1-RAD)*1000)
    END IF
    NEXT I

    M1(361)=RAD/2*-1:M2(361)=RAD/2*-1:M3(361)=RAD/2*-1
    M1(362)=RAD/2:M2(362)=RAD/2:M3(362)=RAD/2
    M1(363)=RAD:M2(363)=RAD:M3(363)=RAD
    Geändert von ces (17.04.2019 um 13:49 Uhr)

Ähnliche Themen

  1. Von Bascom nach C
    Von Zwerwelfliescher im Forum Sonstige Roboter- und artverwandte Modelle
    Antworten: 5
    Letzter Beitrag: 11.03.2011, 21:15

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •