-         
Ergebnis 1 bis 1 von 1

Thema: malthy Omni 3 Raeder Antrieb von Bascom nach C

  1. #1

    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.

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

Name:	omni.jpeg
Hits:	10
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)
    {
    //   WDTE = 0; // 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 /INT0 or /I
    }
    	
    //-----------------------------------------------------------------------------
    // 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 (03.02.2019 um 17:25 Uhr)

Ähnliche Themen

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

Berechtigungen

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