- 12V Akku mit 280 Ah bauen         
Seite 4 von 4 ErsteErste ... 234
Ergebnis 31 bis 36 von 36

Thema: Frage zu RP6 I2C Library: Funktionen der Lib von Peter Fleury?

  1. #31
    Anzeige

    Powerstation Test
    Gut, haben wir wieder zurückgeändert.
    Wenn no_rep_start in TWI_REP_START auf 1 gesetzt wird, können wir das MSB nun abfragen, ohne TWINT zu deaktivieren, allerdings stürzt die Kommunikation immer noch ab (SDA wird immer noch auf Low gezogen), zwei Bytes können wir abfragen, allerdings nur einmal, danach stürzt die Kommunikation auch ab.

    Wir kommen der Sache aber näher!
    Vielen Dank für weitere Tipps!

    EDIT:
    MYSTERIÖS:
    Anscheinend ist des Rätsels Lösung gefunden. Wenn man drei Bytes hintereinander empfängt, stürzt nichts ab. Naja. Egal. Jetzt läuft es!

    Vielen, vielen, vielen Dank an alle, die uns hier geholfen haben uns ganz besonders an SlyD. Es ist nicht selbstverständlich, dass der Entwickler einer solchen Library in so einem Forum ist und dann auch noch so freundlich und mit so viel Ausdauer hilft!
    Geändert von teamohnename (27.02.2012 um 19:22 Uhr)

  2. #32
    Erfahrener Benutzer Roboter Genie Avatar von SlyD
    Registriert seit
    27.11.2003
    Ort
    Paderborn
    Alter
    39
    Beiträge
    1.516
    Ah sehr gut - gern geschehen!

    Es wäre natürlich noch gut, wenn Ihr für andere hier im Forum die evtl. mal was ähnliches brauchen noch die funktionierende Lösung posten würdet.

  3. #33
    Klar. Hier unsere RP6I2CmasterTWI.c (RP6Lib/RP6common/RP6I2CmasterTWI.c):

    Code:
    /* ****************************************************************************
     *                           _______________________
     *                           \| RP6  ROBOT SYSTEM |/
     *                            \_-_-_-_-_-_-_-_-_-_/         >>> BASE CONTROLLER
     * ----------------------------------------------------------------------------
     * ------------------- [c]2006 / 2007 - AREXX ENGINEERING ---------------------
     * -------------------------- http://www.arexx.com/ ---------------------------
     * ****************************************************************************
     * File: RP6I2CmasterTWI.h
     * Version: 1.0
     * Target: RP6 Base & Processor Expansion - ATMEGA32 @8.00 or 16.00MHz
     * Author(s): Dominik S. Herwald
     * ****************************************************************************
     * Description:
     * For functional description of the TWI Master lib s. RP6I2CmasterTWI.c!
     *
     * ****************************************************************************
     * CHANGELOG AND LICENSING INFORMATION CAN BE FOUND AT THE END OF THIS FILE!
     * ****************************************************************************
     */
    
    #ifdef RP6I2C_SLAVE_TWI_H
    	#error YOU CAN NOT INCLUDE TWI I2C MASTER AND SLAVE ROUTINES AT THE SAME TIME!
    #else
    #ifndef RP6I2C_MASTER_TWI_H
    #define RP6I2C_MASTER_TWI_H
    
    /*****************************************************************************/
    // Includes:
    
    #include <stdint.h>					
    #include <avr/interrupt.h>
    #include <avr/io.h>
    
    /*****************************************************************************/
    
    union TWI_statusReg {
        uint8_t all;
        struct {
            volatile unsigned lastTransOK:1;
            unsigned unusedBits:7;
        };
    };
    
    extern volatile union TWI_statusReg TWI_statusReg;
    
    extern uint8_t i2c_req_adr;
    extern uint8_t TWI_operation;
    
    extern uint8_t no_rep_start;
    
    #define I2CTWI_isBusy() ((TWCR & (1<<TWIE))) 
    
    
    // Sample TWI transmission states, used in the main application.
    #define I2CTWI_NO_OPERATION			       0
    #define I2CTWI_SEND_REGISTER           1
    #define I2CTWI_REQUEST_BYTES           2
    #define I2CTWI_READ_BYTES_FROM_BUFFER  3
    
    #define I2CTWI_BUFFER_SIZE 16   // Set this to the largest message size that will be sent including address byte.
    #define I2CTWI_BUFFER_REC_SIZE 48 // Set this to the largest message size that will be received including address byte.
    
    #define I2CTWI_initMaster(__FREQ__) __I2CTWI_initMaster((uint8_t)((F_CPU/(2000UL*__FREQ__))-8))
    void __I2CTWI_initMaster(uint8_t twi_bitrate);
    
    void I2CTWI_setRequestedDataReadyHandler(void (*requestedDataReadyHandler)(uint8_t));
    void I2CTWI_setTransmissionErrorHandler(void (*transmissionErrorHandler)(uint8_t));
    
    void task_I2CTWI(void);
    uint8_t I2CTWI_getState(void);
    
    void I2CTWI_requestDataFromDevice(uint8_t requestAdr, uint8_t requestID, uint8_t numberOfBytes);
    void I2CTWI_requestRegisterFromDevice(uint8_t targetAdr, uint8_t requestID, uint8_t reg, uint8_t numberOfBytes);
    void I2CTWI_getReceivedData(uint8_t *msg, uint8_t msgSize);
    
    void I2CTWI_readBytes(uint8_t targetAdr, uint8_t * messageBuffer, uint8_t numberOfBytes);
    uint8_t I2CTWI_readByte(uint8_t targetAdr);
    void I2CTWI_readRegisters(uint8_t targetAdr, uint8_t reg, uint8_t * messageBuffer, uint8_t numberOfBytes);
    
    void I2CTWI_transmitByte(uint8_t adr, uint8_t data);
    void I2CTWI_transmitByte_RepeatedStart(uint8_t adr, uint8_t data);
    void I2CTWI_transmit2Bytes(uint8_t adr, uint8_t data1, uint8_t data2);
    void I2CTWI_transmit3Bytes(uint8_t targetAdr, uint8_t data1, uint8_t data2, uint8_t data3);
    void I2CTWI_transmit4Bytes(uint8_t targetAdr, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4);
    void I2CTWI_transmitBytes(uint8_t targetAdr, uint8_t *msg, uint8_t numberOfBytes);
    
    #define TWI_READ  1
    #define TWI_GEN_CALL 0 
    
    /*****************************************************************************/
    // TWI Status Codes:
    // The TWI status codes were taken from ATMEL AN315!
    
    // General TWI Master staus codes
    #define TWI_START                  0x08  // START has been transmitted
    #define TWI_REP_START              0x10  // Repeated START has been transmitted
    #define TWI_ARB_LOST               0x38  // Arbitration lost
    
    // TWI Master Transmitter staus codes
    #define TWI_MTX_ADR_ACK            0x18  // SLA+W has been transmitted and ACK received
    #define TWI_MTX_ADR_NACK           0x20  // SLA+W has been transmitted and NACK received
    #define TWI_MTX_DATA_ACK           0x28  // Data byte has been transmitted and ACK received
    #define TWI_MTX_DATA_NACK          0x30  // Data byte has been transmitted and NACK received
    
    // TWI Master Receiver staus codes
    #define TWI_MRX_ADR_ACK            0x40  // SLA+R has been transmitted and ACK received
    #define TWI_MRX_ADR_NACK           0x48  // SLA+R has been transmitted and NACK received
    #define TWI_MRX_DATA_ACK           0x50  // Data byte has been received and ACK transmitted
    #define TWI_MRX_DATA_NACK          0x58  // Data byte has been received and NACK transmitted
    
    // TWI Slave Transmitter staus codes
    #define TWI_STX_ADR_ACK            0xA8  // Own SLA+R has been received; ACK has been returned
    #define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0  // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned
    #define TWI_STX_DATA_ACK           0xB8  // Data byte in TWDR has been transmitted; ACK has been received
    #define TWI_STX_DATA_NACK          0xC0  // Data byte in TWDR has been transmitted; NOT ACK has been received
    #define TWI_STX_DATA_ACK_LAST_BYTE 0xC8  // Last data byte in TWDR has been transmitted (TWEA = “0”); ACK has been received
    
    // TWI Slave Receiver staus codes
    #define TWI_SRX_ADR_ACK            0x60  // Own SLA+W has been received ACK has been returned
    #define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68  // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned
    #define TWI_SRX_GEN_ACK            0x70  // General call address has been received; ACK has been returned
    #define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78  // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned
    #define TWI_SRX_ADR_DATA_ACK       0x80  // Previously addressed with own SLA+W; data has been received; ACK has been returned
    #define TWI_SRX_ADR_DATA_NACK      0x88  // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned
    #define TWI_SRX_GEN_DATA_ACK       0x90  // Previously addressed with general call; data has been received; ACK has been returned
    #define TWI_SRX_GEN_DATA_NACK      0x98  // Previously addressed with general call; data has been received; NOT ACK has been returned
    #define TWI_SRX_STOP_RESTART       0xA0  // A STOP condition or repeated START condition has been received while still addressed as Slave
    
    // TWI Miscellaneous status codes
    #define TWI_NO_STATE               0xF8  // No relevant state information available; TWINT = “0”
    #define TWI_BUS_ERROR              0x00  // Bus error due to an illegal START or STOP condition
    
    
    #endif
    #endif
    
    /******************************************************************************
     * Additional info
     * ****************************************************************************
     * Changelog:
     * - v. 1.0 (initial release) 16.05.2007 by Dominik S. Herwald
     *
     * ****************************************************************************
     * Bugs, feedback, questions and modifications can be posted on the AREXX Forum
     * on http://www.arexx.com/forum/ !
     * Of course you can also write us an e-mail to: info@arexx.nl
     * AREXX Engineering may publish updates from time to time on AREXX.com!
     * ****************************************************************************
     * - LICENSE -
     * GNU GPL v2 (http://www.gnu.org/licenses/gpl.txt, a local copy can be found
     * on the RP6 CD in the RP6 sorce code folders!)
     * This program is free software. You can redistribute it and/or modify
     * it under the terms of the GNU General Public License version 2 as published
     * by the Free Software Foundation.
     * ****************************************************************************
     */
    
    /*****************************************************************************/
    // EOF
    und unsere RP6I2CmasterTWI.h (RP6Lib/RP6common/RP6I2CmasterTWI.h):

    Code:
    /* ****************************************************************************
     *                           _______________________
     *                           \| RP6  ROBOT SYSTEM |/
     *                            \_-_-_-_-_-_-_-_-_-_/         >>> BASE CONTROLLER
     * ----------------------------------------------------------------------------
     * ------------------- [c]2006 / 2007 - AREXX ENGINEERING ---------------------
     * -------------------------- http://www.arexx.com/ ---------------------------
     * ****************************************************************************
     * File: RP6I2CmasterTWI.c
     * Version: 1.0
     * Target: RP6 Base & Processor Expansion - ATMEGA32 @8.00 or 16.00MHz
     * Author(s): Dominik S. Herwald
     * ****************************************************************************
     * Description:
     * This is the I2C Bus Master Library. 
     *
     * ****************************************************************************
     * CHANGELOG AND LICENSING INFORMATION CAN BE FOUND AT THE END OF THIS FILE!
     * ****************************************************************************
     */
     
    /*****************************************************************************/
    // Includes:
    
    #include "RP6I2CmasterTWI.h"
    
    /*
     * This function initializes the TWI interface! You need
     * to call this first before you use the TWI interface!
     * You should better use the macro I2CTWI_initMaster without __ at the
     * beginning. There you can specify the SCL frequency in kHz!
     * Example: 
     * I2CTWI_initMaster(100); // I2C Master mode with 100kHz SCL frequency
     *						   // This calculates TWBR value automatically.
     *
     * __I2CTWI_initMaster(32); // I2C Master mode also with 100kHz SCL frequency
     * 							// but directly calculated with the formula in the
     *							// MEGA32 datasheet.
     */
    void __I2CTWI_initMaster(uint8_t twi_bitrate)
    {
    	cli();
    	TWBR = twi_bitrate;
    	TWSR = 0x00;      // DO NOT USE PRESCALER! Otherwise you need to mask the
    	TWDR = 0xFF;      // TWSR Prescaler bits everywhere TWSR is read!  	      
    	TWCR = (1<<TWEN);
    	TWI_statusReg.lastTransOK = 1;
        sei();                      
    }
    
    /*****************************************************************************/
    // TWI Event handlers
    // These functions are used to receive Data or react on errors.
    
    void I2CTWI_requestedDataReady_DUMMY(uint8_t requestID){}
    static void (*I2CTWI_requestedDataReadyHandler)(uint8_t) = I2CTWI_requestedDataReady_DUMMY;
    void I2CTWI_setRequestedDataReadyHandler(void (*requestedDataReadyHandler)(uint8_t))
    {
    	I2CTWI_requestedDataReadyHandler = requestedDataReadyHandler;
    }
    
    void I2CTWI_transmissionError_DUMMY(uint8_t requestID){}
    static void (*I2CTWI_transmissionErrorHandler)(uint8_t) = I2CTWI_transmissionError_DUMMY;
    void I2CTWI_setTransmissionErrorHandler(void (*transmissionErrorHandler)(uint8_t))
    {
    	I2CTWI_transmissionErrorHandler = transmissionErrorHandler;
    }
    
    /*****************************************************************************/
    // Delay
    
    /**
     * A small delay that is required between some transfers. Without this delay
     * the transmission may fail.
     */
    void I2CTWI_delay(void)
    {
    	volatile uint8_t dly = 150;
    	while(dly--);
    }
    
    /*****************************************************************************/
    // Control task
    
    static uint8_t I2CTWI_buf[I2CTWI_BUFFER_SIZE];
    static uint8_t I2CTWI_recbuf[I2CTWI_BUFFER_REC_SIZE];
    static uint8_t TWI_msgSize;
    volatile union TWI_statusReg TWI_statusReg = {0}; 
    uint8_t TWI_TWSR_state = 0;
    
    uint8_t I2CTWI_request_adr = 0;
    uint8_t I2CTWI_request_reg = 0;
    uint8_t I2CTWI_request_size = 0;
    int16_t I2CTWI_requestID = 0;
    uint8_t TWI_operation = I2CTWI_NO_OPERATION;
    
    
    /**
     * You have to call this functions frequently out of the
     * main loop. It calls the event handlers above automatically if data has been received.
     */
    void task_I2CTWI(void)
    {
    	if (!I2CTWI_isBusy()) {
    		if (TWI_statusReg.lastTransOK) {
    			if(TWI_operation) {
    				if(TWI_operation == I2CTWI_SEND_REGISTER) {
    					I2CTWI_delay();
    					TWI_msgSize = 2;
    					I2CTWI_buf[0] = I2CTWI_request_adr;
    					I2CTWI_buf[1] = I2CTWI_request_reg;
    					TWI_statusReg.all = 0;
    					TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    					TWI_operation = I2CTWI_REQUEST_BYTES;
    				}
    				else if (TWI_operation == I2CTWI_REQUEST_BYTES) {
    					I2CTWI_delay();
    					TWI_msgSize = I2CTWI_request_size + 1;
    					I2CTWI_request_adr = I2CTWI_request_adr | TWI_READ;
    					I2CTWI_buf[0]  = I2CTWI_request_adr | TWI_READ; 
    					TWI_statusReg.all = 0;
    					TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);	
    					TWI_operation = I2CTWI_READ_BYTES_FROM_BUFFER;
    				}
    				else if (TWI_operation == I2CTWI_READ_BYTES_FROM_BUFFER) { 
    					TWI_operation = I2CTWI_NO_OPERATION;
    					if(I2CTWI_requestID!=-1)
    						I2CTWI_requestedDataReadyHandler(I2CTWI_requestID);
    				}
    			}
    		}
    		else {
    			uint8_t errState = I2CTWI_getState();
    			if(errState != 0) {
    				TWI_operation = I2CTWI_NO_OPERATION;
    				TWI_statusReg.lastTransOK = 1;
    				I2CTWI_request_adr = 0;
    				I2CTWI_requestID = 0;
    				I2CTWI_request_size = 0;
    				I2CTWI_transmissionErrorHandler(errState);
    			}
    		}
    	}
    }
    
    
    /*****************************************************************************/
    // Request functions - you need to use requestedDataReadyHandler to receive 
    // the requested data...
    
    /**
     * Requests a number of Bytes from the target device. You need to set a requestID 
     * to be able to identify the request after the data has been received and the 
     * "requestedDataReady" event handler is called.
     */
    void I2CTWI_requestDataFromDevice(uint8_t targetAdr, uint8_t requestID, uint8_t numberOfBytes)
    {	
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	TWI_operation = I2CTWI_REQUEST_BYTES;
    	I2CTWI_request_adr = targetAdr;
    	I2CTWI_requestID = (int16_t)requestID;
    	I2CTWI_request_size = numberOfBytes;
    }
    
    /**
     * Same as requestDataFromDevice, but this function first sets the register
     * that has to be read and transmits the register number before! 
     * This is neccessary for the reaction on interrupt requests of slave devices as this
     * function is non-blocking and thus very well suited for calls directly from ISRs. 
     */
    void I2CTWI_requestRegisterFromDevice(uint8_t targetAdr, uint8_t requestID, uint8_t reg, uint8_t numberOfBytes)
    {	
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	TWI_operation = I2CTWI_SEND_REGISTER;
    	I2CTWI_requestID = (int16_t)requestID;
    	I2CTWI_request_adr = targetAdr;
    	I2CTWI_request_reg = reg;
    	I2CTWI_request_size = numberOfBytes;
    }
    
    /**
     * This function can be used in the requestedDataReady Handler to get the
     * received data from the TWI Buffer. You need to provide a pointer to a 
     * buffer which is large enough to hold all received data. 
     * You can specify the number of bytes you want to read out of the buffer
     * with the msgSize parameter.
     * It does not make sense to use this function anywhere else as you 
     * can not guarantee what is in the reception buffer...
     */
    void I2CTWI_getReceivedData(uint8_t *msg, uint8_t msgSize)
    {
    	while(I2CTWI_isBusy());
    	uint8_t i = 0;
    	if(TWI_statusReg.lastTransOK)
    		for(; i < msgSize; i++)
    			msg[i] = I2CTWI_recbuf[i+1];
    }
    
    /**
     * This function returns the last TWI State / Error State. It waits until
     * TWI Module has completed last operation! 
     */
    uint8_t I2CTWI_getState(void)
    {
    	while(I2CTWI_isBusy()); // Wait until TWI has completed the transmission.
    	return (TWI_TWSR_state); // Return error state.
    }
    
    /*****************************************************************************/
    // Read functions:
    
    /**
     * Reads "numberOfBytes" from "targetAdr" Register "reg" into "messageBuffer". 
     * If the slave device supports auto increment, then it reads all subsequent registers of course! 
     */
    void I2CTWI_readRegisters(uint8_t targetAdr, uint8_t reg, uint8_t * messageBuffer, uint8_t numberOfBytes)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	TWI_operation = I2CTWI_SEND_REGISTER;
    	I2CTWI_request_adr = targetAdr;
    	I2CTWI_requestID = -1;
    	I2CTWI_request_reg = reg;
    	I2CTWI_request_size = numberOfBytes;
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	if (TWI_statusReg.lastTransOK) 
    		I2CTWI_getReceivedData(&messageBuffer[0], numberOfBytes+1);
    }
    
    /**
     * Same as readRegisters, but you need to make sure which register to read yourself - if there
     * are any registers at all in your slave device.  
     * 
     */
    void I2CTWI_readBytes(uint8_t targetAdr, uint8_t * messageBuffer, uint8_t numberOfBytes)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_operation = I2CTWI_REQUEST_BYTES;
    	I2CTWI_request_adr = targetAdr;
    	I2CTWI_requestID = -1;
    	I2CTWI_request_size = numberOfBytes;
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	if (TWI_statusReg.lastTransOK) 
    		I2CTWI_getReceivedData(&messageBuffer[0], numberOfBytes+1);
    }
    
    /**
     * Reads a single byte from the slave device. 
     */
    uint8_t I2CTWI_readByte(uint8_t targetAdr)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_operation = I2CTWI_REQUEST_BYTES;
    	I2CTWI_request_adr = targetAdr;
    	I2CTWI_requestID = -1;
    	I2CTWI_request_size = 1;
    	while(TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	if (TWI_statusReg.lastTransOK)
    		return I2CTWI_recbuf[1];
    	else
    		return 0;
    }
    /*****************************************************************************/
    // Transmission functions
    
    /**
     * Transmits a single byte to a slave device. It waits until the last 
     * TWI operation is finished (it blocks the normal program flow!) but
     * it does NOT wait until this transmission is finished! 
     * This allows you to perform other things while the transmission is 
     * in progress! 
     */
    void I2CTWI_transmitByte(uint8_t targetAdr, uint8_t data)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_msgSize = 2;
    	I2CTWI_buf[0] = targetAdr;
    	I2CTWI_buf[1] = data;
    	TWI_statusReg.all = 0;
    	no_rep_start = 1;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    //With Sending Repeated Start
    void I2CTWI_transmitByte_RepeatedStart(uint8_t targetAdr, uint8_t data)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_msgSize = 2;
    	I2CTWI_buf[0] = targetAdr;
    	I2CTWI_buf[1] = data;
    	TWI_statusReg.all = 0;
    	no_rep_start = 0;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    /**
     * This is just the same as transmitByte, but you can pass 2 Bytes to
     * this function which are then transferred. 
     */
    void I2CTWI_transmit2Bytes(uint8_t targetAdr, uint8_t data1, uint8_t data2)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_msgSize = 3;
    	I2CTWI_buf[0] = targetAdr;
    	I2CTWI_buf[1] = data1;
    	I2CTWI_buf[2] = data2;
    	TWI_statusReg.all = 0;
    	no_rep_start = 1;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    /**
     * Transmits 3 Bytes to the slave.
     */
    void I2CTWI_transmit3Bytes(uint8_t targetAdr, uint8_t data1, uint8_t data2, uint8_t data3)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_msgSize = 4;
    	I2CTWI_buf[0] = targetAdr;
    	I2CTWI_buf[1] = data1;
    	I2CTWI_buf[2] = data2;
    	I2CTWI_buf[3] = data3;
    	TWI_statusReg.all = 0;
    	no_rep_start = 1;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    /**
     * Transmits 4 Bytes to the slave.
     */
    void I2CTWI_transmit4Bytes(uint8_t targetAdr, uint8_t data1, uint8_t data2, uint8_t data3, uint8_t data4)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	TWI_msgSize = 5;
    	I2CTWI_buf[0] = targetAdr;
    	I2CTWI_buf[1] = data1;
    	I2CTWI_buf[2] = data2;
    	I2CTWI_buf[3] = data3;
    	I2CTWI_buf[4] = data4;
    	TWI_statusReg.all = 0;
    	no_rep_start = 1;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    
    /**
     * Transmits "numberOfBytes" Bytes to the Slave device. The Bytes need to be
     * in the Buffer "msg". Otherwise it is just the same as the other transmit functions.
     */
    void I2CTWI_transmitBytes(uint8_t targetAdr, uint8_t *msg, uint8_t numberOfBytes)
    {
    	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
    	I2CTWI_delay();
    	numberOfBytes++; 
    	TWI_msgSize = numberOfBytes;
    	I2CTWI_buf[0]  = targetAdr;
    	uint8_t i = 0;
    	for(; i < numberOfBytes; i++)
    		I2CTWI_buf[i+1] = msg[i];
    	TWI_statusReg.all = 0;
    	no_rep_start = 1;
    	TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO);
    }
    
    
    /*****************************************************************************/
    // ISR:
    
    /*
     * TWI ISR
     */
    uint8_t no_rep_start;  //Wird ein Repeated start vom Slave benötigt?
    
    ISR (TWI_vect)
    {
    	static uint8_t TWI_bufPos = 0;
    	switch (TWSR)
    	{
    		case TWI_START:           // START has been transmitted  
    		case TWI_REP_START:         // Repeated START has been transmitted
    		  no_rep_start = 1;
    			TWI_bufPos = 0;          // Set buffer pointer to the TWI Address location
    		case TWI_MTX_ADR_ACK:       // SLA+W has been transmitted and ACK received
    		case TWI_MTX_DATA_ACK:      // Data byte has been transmitted and ACK received
    		 if (TWI_bufPos < TWI_msgSize) {
    			TWDR = I2CTWI_buf[TWI_bufPos++];  
    			TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				   (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interupt and clear the flag to send byte
    				   (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           //
    				   (0<<TWWC);                                 //  
    		  } else {                   // Send STOP after last byte
    				TWI_statusReg.lastTransOK = 1;                 // Set status bits to completed successfully. 
    				if(no_rep_start == 1){
    					TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				  	 (0<<TWIE)|(0<<TWINT)|                      // Disable TWI Interrupt and clear the flag
    				  	 (0<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|           // Initiate a STOP condition.
    				  	 (0<<TWWC);                                //
    				}
    				else{
    					TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    					   (0<<TWIE)|(0<<TWINT)|                      // Disable TWI Interrupt and clear the flag
    					   (0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|           // Initiate a REPEATED START condition.
    					   (0<<TWWC);                                //
    		  	}
    			}
    		  break;
    		case TWI_MRX_DATA_ACK:     // Data byte has been received and ACK transmitted
    		  I2CTWI_recbuf[TWI_bufPos++] = TWDR;
    		case TWI_MRX_ADR_ACK:      // SLA+R has been transmitted and ACK received
    		  if (TWI_bufPos < (TWI_msgSize-1) ) {                 // Detect the last byte to NACK it.
    			TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				   (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interupt and clear the flag to read next byte
    				   (1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // Send ACK after reception
    				   (0<<TWWC);                                 //  
    		  } else {                 // Send NACK after next reception
    			TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				   (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interupt and clear the flag to read next byte
    				   (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // Send NACK after reception
    				   (0<<TWWC);                                 // 
    		  }    
    		  break; 
    		case TWI_MRX_DATA_NACK:     // Data byte has been received and NACK transmitted
    		  I2CTWI_recbuf[TWI_bufPos] = TWDR;
    		  TWI_statusReg.lastTransOK = 1;                 	// Set status bits to completed successfully. 		 
    		  TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				 (0<<TWIE)|(1<<TWINT)|                      // Disable TWI Interrupt and clear the flag
    				 (0<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|           // Initiate a STOP condition.
    				 (0<<TWWC);                                 //
    		  break;      
    		case TWI_ARB_LOST:         // Arbitration lost
    		  TWI_TWSR_state = TWSR;  							// Store TWSR 	
    		  TWCR = (1<<TWEN)|                                 // TWI Interface enabled
    				 (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interupt and clear the flag
    				 (0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|           // Initiate a (RE)START condition.
    				 (0<<TWWC);                               //
    		  break;
    		default:     
    		  TWI_TWSR_state = TWSR;                     		// Store TWSR 					
    		  TWCR = (1<<TWEN)|                                 // Enable TWI-interface and release TWI pins
    				 (0<<TWIE)|(1<<TWINT)|                      // Disable Interupt
    				 (0<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|           // No Signal requests
    				 (0<<TWWC);  
    		break;
    	}
    }
    
    /******************************************************************************
     * Additional info
     * ****************************************************************************
     * Changelog:
     * - v. 1.0 (initial release) 16.05.2007 by Dominik S. Herwald
     *
     * ****************************************************************************
     * Bugs, feedback, questions and modifications can be posted on the AREXX Forum
     * on http://www.arexx.com/forum/ !
     * Of course you can also write us an e-mail to: info@arexx.nl
     * AREXX Engineering may publish updates from time to time on AREXX.com!
     * ****************************************************************************
     * - LICENSE -
     * GNU GPL v2 (http://www.gnu.org/licenses/gpl.txt, a local copy can be found
     * on the RP6 CD in the RP6 sorce code folders!)
     * This program is free software. You can redistribute it and/or modify
     * it under the terms of the GNU General Public License version 2 as published
     * by the Free Software Foundation.
     * ****************************************************************************
     */
    
    /*****************************************************************************/
    // EOF
    Mehr ist, soweit ich weiß, nicht geändert.
    Die Abfolge von Befehlen zum Abfragen der Werte aus dem Sensor kommt noch, da müssen wir noch etwas basteln (damit die Umrechnung in °C funktioniert).
    Bei weiteren Fragen könnt ihr gerne diesen Thread weiterverwenden!

    EDIT:
    So könnt ihr den Sensor selbst abfragen. Ihr bekommt zum Schluss z.B. für 22,53°C den Wert 2253 (damit ihr nicht mit riesigen Variablen oder Floats arbeiten müsst):

    Code:
    int16_t getMLX90614(void)
    {
    	I2CTWI_transmitByte_RepeatedStart(0x5A<<1,0x07);
    	I2CTWI_readBytes(0x5A<<1, sensorBuf, 3);
    
    	// This masks off the error bit of the high byte, then moves it left 8 bits and adds the low byte.
            tempdata = (((sensorBuf[1] & 0x007F) << 8) + sensorBuf[0]);
    	tempdata = (tempdata * 2)-1;
    	tempdata = tempdata - 27315;
    
            return tempdata;
    }
    Geändert von teamohnename (28.02.2012 um 17:22 Uhr)

  4. #34
    Hallo,
    es gibt ein neues Problem...
    Viele move Funktionen funktionieren nicht mehr. Wir benötigen nur moveAtSpeed und stop, wobei moveAtSpeed teilweise funktioniert und stop gar nicht. Anscheinend hängt das Programm sich an den whiles und waitForTransmit Funktionen auf. Eventuell liegt das aber auch an den I2CTWI_transmitXBytes Funktionen. Kann man das irgendwie mit dem normalen I2CTWI_transmitByte lösen?
    So langsam ist das alles echt zum Verzweifeln... Ein Problem nach dem anderen...

    Vielen Dank für Eure Hilfe und
    Viele Grüße
    teamohnename

  5. #35
    Erfahrener Benutzer Roboter Genie Avatar von SlyD
    Registriert seit
    27.11.2003
    Ort
    Paderborn
    Alter
    39
    Beiträge
    1.516
    > So langsam ist das alles echt zum Verzweifeln... Ein Problem nach dem anderen...

    Wenn man auf der low-level Ebene in der Lib was ändert kann das natürlich an anderer Stelle Probleme bereiten also soweit völlig normal
    --> Erstmal mit minimal Programm testen also nur initialisierung, pause, betroffene Funktionen aufrufen und endlosschleife.
    Dann in die betroffenen Funktionen debug ausgaben rein machen (vermutlich schon passiert?)

    Evtl. fehlt ja irgendwo das no_rep_start = 1 o.ä.

  6. #36
    Haben wir alles schon gemacht.
    Nur die Stop Funktion macht Probleme, von daher wird es wohl an dem waitForTransmit liegen...
    Ist erstmal egal, es gibt ein noch viel größeres Problem: Wenn man in der Hauptschleife die Infrarotsensoren abfragt und moveAtSpeed benutzt, funktionieren die Infrarotsensoren nicht mehr. Das hat garantiert nichts damit zu tun, dass die Motoren an sind.
    Vielleicht wäre es doch einfacher, komplett auf die Fleury Library umzusteigen...

    EDIT:
    So, mit der Library von Peter Fleury auf der M32 Platine und dem Slave Programm aus den RP6 Examples auf der Base funktioniert alles. Wir werden, sobald wir alles, was mit der neuen Library nicht benötigt wird, aus dem Programm entfernt haben, hier alles posten.

    EDIT2:
    So:
    1) Im Makefile Zeile 74 (dort, wo die Standard I²C Library eingebunden wird) entfernen oder auskommentieren
    2) Die I²C Master Library von Peter Fleury herunterladen
    3) Library (nur die Dateien twimaster.c und i2cmaster.h) in den Ordner einfügen, in den auch das Hauptprogramm und das Makefile ist
    4) In twimaster.c I²C Geschwindigkeit und ggf. CPU Takt (16MHz, also 16000000Hz) festlegen
    5) Ins Hauptprogramm zu den anderen includes schreiben:
    Code:
    #include "i2cmaster.h"
    6) Ins Hauptprogramm die Zeilen
    Code:
    I2CTWI_initMaster(100);  
    I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
    durch das:
    Code:
    i2c_init();
    ersetzen.
    7) Ggf. RP6Control_I2CMasterLib.c durch das ersetzen:
    (ACHTUNG: move, rotate und setLEDs kann man dann nicht mehr benutzen, nur noch moveAtSpeed, changeDirection und stop. Natürlich kann man das alles ergänzen, wenn man will, wir benötigen das aber nicht, deshalb haben wir uns keine Arbeit damit gemacht)

    Code:
    // Includes:
    
    #include "RP6Control_I2CMasterLib.h" 		
    
    /*****************************************************************************/
    // Sensors/ADCs: 
    
    // ADCs:
    uint16_t adcBat;
    uint16_t adcMotorCurrentLeft;
    uint16_t adcMotorCurrentRight;
    uint16_t adcLSL;
    uint16_t adcLSR;
    uint16_t adc0;
    uint16_t adc1;
    
    //Accelerometer:
    int16_t threedbs_x;
    int16_t threedbs_y;
    int16_t threedbs_z;
    
    //IR (MLX90614)
    int16_t mlx90614_l;
    int16_t mlx90614_r;
    
    // Measured Speed:
    uint8_t mleft_speed;
    uint8_t mright_speed;
    
    // Distance
    uint16_t mleft_dist;
    uint16_t mright_dist;
    
    // Desired Speed:
    uint8_t mleft_des_speed;
    uint8_t mright_des_speed;
    
    // Power
    uint8_t mleft_power;
    uint8_t mright_power;
    
    uint8_t sensorBuf[24]; 
    
    /**
     * In order to use the same register names as in the RP6Lib, this
     * function reads all ADC channels and all motor parameters into
     * the same values as in the RP6Lib. 
     * Of course this function needs some time to read all these
     * 24 registers via the I2C Bus. 
     */
    void getAllSensors(void)
    {
    	i2c_start(I2C_RP6_BASE_ADR+0);
    	i2c_write(I2C_REG_POWER_LEFT);
    	i2c_rep_start(I2C_RP6_BASE_ADR+1);
    
    	for(uint8_t i = 0; i<23; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[23] = i2c_readNak();
    
    	i2c_stop();
    
    	mleft_power = sensorBuf[0];
    	mright_power = sensorBuf[1];
    	mleft_speed = sensorBuf[2];
    	mright_speed = sensorBuf[3];
    	mleft_des_speed = sensorBuf[4];
    	mright_des_speed = sensorBuf[5];
    	mleft_dist = sensorBuf[6] + (sensorBuf[7]<<8);
    	mright_dist = sensorBuf[8] + (sensorBuf[9]<<8);
    	adcLSL = sensorBuf[10] + (sensorBuf[11]<<8);
    	adcLSR = sensorBuf[12] + (sensorBuf[13]<<8);
    	adcMotorCurrentLeft = sensorBuf[14] + (sensorBuf[15]<<8);
    	adcMotorCurrentRight = sensorBuf[16] + (sensorBuf[17]<<8);
    	adcBat = sensorBuf[18] + (sensorBuf[19]<<8);
    	adc0 = sensorBuf[20] + (sensorBuf[21]<<8);
    	adc1 = sensorBuf[22] + (sensorBuf[23]<<8);
    }
    
    /**
     *
     */
    void getLightSensors(void)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(I2C_REG_ADC_LSL_L);
    	i2c_rep_start(I2C_RP6_BASE_ADR+1);
    
    	for(uint8_t i = 0; i<3; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[3] = i2c_readNak();
    
    	i2c_stop();
    
    	adcLSL = sensorBuf[0] + (sensorBuf[1]<<8);
    	adcLSR = sensorBuf[2] + (sensorBuf[3]<<8);
    }
    
    /**
     *
     */
    void getADCs(void)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(I2C_REG_ADC_ADC0_L);
    	i2c_rep_start(I2C_RP6_BASE_ADR+1);
    
    	for(uint8_t i = 0; i<3; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[3] = i2c_readNak();
    
    	i2c_stop();
    
    	adc0 = sensorBuf[0] + (sensorBuf[1]<<8);
    	adc1 = sensorBuf[2] + (sensorBuf[3]<<8);
    }
    
    /**
     *
     */
    void get3DBS(void)
    {
    	i2c_start(I2C_3DBS_ADR);
    	i2c_write(I2C_REG_3DBS);
    	i2c_rep_start(I2C_3DBS_ADR+1);
    
    	for(uint8_t i = 0; i<5; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[5] = i2c_readNak();
    
    	i2c_stop();
    
    	threedbs_x = sensorBuf[1];//i2c_readAck() + (i2c_readAck()<<8);
    	threedbs_y = sensorBuf[3];//i2c_readAck() + (i2c_readAck()<<8);
    	threedbs_z = sensorBuf[5];//i2c_readAck() + (i2c_readNak()<<8);
    }
    
    /*
     *
     */
    void getIR(void)
    {
    	i2c_start_wait(I2C_MLX90614_L);
    	i2c_write(I2C_REG_MLX90614);
    	i2c_rep_start((I2C_MLX90614_L)+1);
    
    	for(uint8_t i = 0; i<2; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[2] = i2c_readNak();
    
    	i2c_stop();
    
    	// This masks off the error bit of the high byte, then moves it left 8 bits and adds the low byte.
      int16_t tempdata = (((sensorBuf[1] & 0x007F) << 8) + sensorBuf[0]);
    	tempdata = (tempdata * 2)-1;
    	mlx90614_l = tempdata - 27315;
    
    
    	i2c_start_wait(I2C_MLX90614_R);
    	i2c_write(I2C_REG_MLX90614);
    	i2c_rep_start((I2C_MLX90614_R)+1);
    
    	for(uint8_t i = 0; i<2; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[2] = i2c_readNak();
    
    	i2c_stop();
    
    	// This masks off the error bit of the high byte, then moves it left 8 bits and adds the low byte.
      tempdata = (((sensorBuf[1] & 0x007F) << 8) + sensorBuf[0]);
    	tempdata = (tempdata * 2)-1;
    	mlx90614_r = tempdata - 27315;
    }
    
    /*
     *
     */
    void getDists(void)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(I2C_REG_DIST_LEFT_L);
    	i2c_rep_start(I2C_RP6_BASE_ADR+1);
    
    	for(uint8_t i = 0; i<3; i++)
    	{
    		sensorBuf[i] = i2c_readAck();
    	}
    	sensorBuf[3] = i2c_readNak();
    
    	i2c_stop();
    
    	mleft_dist = sensorBuf[0] + (sensorBuf[1]<<8);
    	mright_dist = sensorBuf[2] + (sensorBuf[3]<<8);
    }
    
    /*****************************************************************************/
    /*****************************************************************************/
    
    // Movement functions:
    
    /**
     * Move at speed function - just the same as with RP6Lib!
     */
    void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(0);
    	i2c_write(CMD_MOVE_AT_SPEED);
    	i2c_write(desired_speed_left);
    	i2c_write(desired_speed_right);
    	i2c_stop();
    }
    
    /**
     * Change direction function - just the same as with RP6Lib!
     */
    void changeDirection(uint8_t dir)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(0);
    	i2c_write(CMD_CHANGE_DIR);
    	i2c_write(dir);
    	i2c_stop();
    }
    
    /**
     * Stop function - just the same as with RP6Lib!
     */
    void stop(void)
    {
    	i2c_start_wait(I2C_RP6_BASE_ADR);
    	i2c_write(0);
    	i2c_write(CMD_STOP);
    	i2c_stop();
    }
    und RP6Control_I2CMasterLib.h durch das ersetzen:

    Code:
    #ifndef RP6CONTROL_I2CMASTERLIB_H
    #define RP6CONTROL_I2CMASTERLIB_H
    
    
    /*****************************************************************************/
    // Includes:
    
    #include "RP6ControlLib.h"
    
    // define the RP6 Base address here:
    #define I2C_RP6_BASE_ADR 10
    
    //other I2C adresses:
    #define I2C_3DBS_ADR 0x70
    #define I2C_MLX90614_L 0x55<<1
    #define I2C_MLX90614_R 0x5A<<1
    
    /*****************************************************************************/
    // These are the same command definitions as you can find them in the 
    // I2C Bus Slave Example program for RP6Base:
    
    #define I2C_REG_STATUS1 		 0
    #define I2C_REG_STATUS2 		 1
    #define I2C_REG_MOTION_STATUS 	 2
    #define I2C_REG_POWER_LEFT 		 3
    #define I2C_REG_POWER_RIGHT 	 4
    #define I2C_REG_SPEED_LEFT 		 5
    #define I2C_REG_DES_SPEED_LEFT 	 7
    #define I2C_REG_DES_SPEED_RIGHT  8
    #define I2C_REG_DIST_LEFT_L 	 9
    #define I2C_REG_DIST_LEFT_H 	 10
    #define I2C_REG_DIST_RIGHT_L     11
    #define I2C_REG_DIST_RIGHT_H 	 12
    #define I2C_REG_ADC_LSL_L 		 13
    #define I2C_REG_ADC_LSL_H 		 14
    #define I2C_REG_ADC_LSR_L 		 15
    #define I2C_REG_ADC_LSR_H 		 16
    #define I2C_REG_ADC_MOTOR_CURL_L 17
    #define I2C_REG_ADC_MOTOR_CURL_H 18
    #define I2C_REG_ADC_MOTOR_CURR_L 19
    #define I2C_REG_ADC_MOTOR_CURR_H 20
    #define I2C_REG_ADC_UBAT_L 		 21
    #define I2C_REG_ADC_UBAT_H 		 22
    #define I2C_REG_ADC_ADC0_L 		 23
    #define I2C_REG_ADC_ADC0_H 		 24
    #define I2C_REG_ADC_ADC1_L 		 25
    #define I2C_REG_ADC_ADC1_H 		 26
    #define I2C_REG_RC5_ADR	 		 27
    #define I2C_REG_RC5_DATA	 	 28
    #define I2C_REG_LEDS	 		 29
    
    //Commands od other I2C devices:
    #define I2C_REG_3DBS	 0x02
    #define I2C_REG_MLX90614 0x07
    
    #define CMD_POWER_OFF 		0
    #define CMD_POWER_ON 		1
    #define CMD_CONFIG 			2
    #define CMD_SETLEDS 		3
    #define CMD_STOP   			4
    #define CMD_MOVE_AT_SPEED   5
    #define CMD_CHANGE_DIR	    6
    #define CMD_MOVE 			7
    #define CMD_ROTATE 			8
    #define CMD_SET_ACS_POWER	9 
    #define CMD_SEND_RC5		10 
    #define CMD_SET_WDT			11
    #define CMD_SET_WDT_RQ		12
    
    /*****************************************************************************/
    
    #define getMotorRight() (OCR1A)
    #define getMotorLeft() (OCR1B)
    
    //Encoders:
    extern uint8_t mleft_speed;
    extern uint8_t mright_speed;
    
    // Distance
    extern uint16_t mleft_dist;
    extern uint16_t mright_dist;
    
    // Desired Speed:
    extern uint8_t mleft_des_speed;
    extern uint8_t mright_des_speed;
    
    // Power
    extern uint8_t mleft_power;
    extern uint8_t mright_power;
    
    #define getLeftSpeed() (mleft_speed)
    #define getRightSpeed() (mright_speed)
    #define getLeftDistance() (mleft_dist)
    #define getRightDistance() (mright_dist)
    
    #define getDesSpeedLeft() (mleft_des_speed)
    #define getDesSpeedRight() (mright_des_speed)
    
    //Direction:
    
    #define FWD 0
    #define BWD 1
    #define LEFT 2
    #define RIGHT 3
    
    #define getDirection() (drive_status.direction)
    
    
    #define INT0_STATUS_CHECK 0
    
    
    
    /*****************************************************************************/
    // 
    void initI2C_RP6Lib(void);
    uint8_t checkRP6Status(uint8_t dataRequestID);
    
    void I2C_transmissionError(uint8_t errorState);
    
    void task_checkINT0(void);
    
    #define NON_BLOCKING 0
    #define BLOCKING 1
    
    void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking);
    void move(uint8_t desired_speed, uint8_t dir, uint16_t distance, uint8_t blocking);
    void moveAtSpeed(uint8_t desired_speed_left, uint8_t desired_speed_right);
    void changeDirection(uint8_t dir);
    void stop(void);
    
    void getADCs(void);
    void getDists(void);
    void get3DBS(void);
    void getIR(void);
    
    #define isMovementComplete() (drive_status.movementComplete)
    	
    extern uint8_t bumper_left;
    extern uint8_t bumper_right;
    
    extern uint8_t obstacle_left;
    extern uint8_t obstacle_right;
    #define isObstacleLeft() (obstacle_left)
    #define isObstacleRight() (obstacle_right)
    
    extern uint16_t adcBat;
    extern uint16_t adcMotorCurrentLeft;
    extern uint16_t adcMotorCurrentRight;
    extern uint16_t adcLSL;
    extern uint16_t adcLSR;
    extern uint16_t adc0;
    extern uint16_t adc1;
    
    //Accelerometer:
    extern int16_t threedbs_x;
    extern int16_t threedbs_y;
    extern int16_t threedbs_z;
    
    //IR (MLX90614)
    extern int16_t mlx90614_l;
    extern int16_t mlx90614_r;
    
    #define TOGGLEBIT 32
    
    typedef union {
    	uint16_t data;
    	struct {
    		uint8_t key_code:6;
    		uint8_t device:5;
    		uint8_t toggle_bit:1;
    		uint8_t reserved:3;
    	};
    } RC5data_t;
    
    
    void IRCOMM_sendRC5(uint8_t adr, uint8_t data);
    
    void IRCOMM_setRC5DataReadyHandler(void (*rc5Handler)(RC5data_t));
    void ACS_setStateChangedHandler(void (*acsHandler)(void));
    void MOTIONCONTROL_setStateChangedHandler(void (*motionControlHandler)(void));
    void BUMPERS_setStateChangedHandler(void (*bumperHandler)(void));
    void WDT_setRequestHandler(void (*requestHandler)(void));
    
    void getAllSensors(void);
    void getLightSensors(void);
    
    #endif
    nun sollte alles funktionieren. Bei Fragen einfach fragen!
    Geändert von teamohnename (05.03.2012 um 18:19 Uhr)

Seite 4 von 4 ErsteErste ... 234

Ähnliche Themen

  1. LCD library von Peter Fleury ÄÖÜ fehlt
    Von Woftschik im Forum C - Programmierung (GCC u.a.)
    Antworten: 21
    Letzter Beitrag: 18.04.2009, 14:31
  2. LCD an Mega8 mit Lib von Peter Fleury
    Von Mr Bean im Forum C - Programmierung (GCC u.a.)
    Antworten: 6
    Letzter Beitrag: 04.10.2007, 08:01
  3. 4x20 LCD und Peter Fleury
    Von hansbausn im Forum C - Programmierung (GCC u.a.)
    Antworten: 11
    Letzter Beitrag: 27.01.2006, 17:06
  4. Anfängerproblem mit i2c und Peter Fleury
    Von hansbausn im Forum C - Programmierung (GCC u.a.)
    Antworten: 5
    Letzter Beitrag: 20.11.2005, 17:26
  5. Peter Fleury LCD Lib Problem mit LCD
    Von Cybrix im Forum C - Programmierung (GCC u.a.)
    Antworten: 13
    Letzter Beitrag: 30.09.2005, 10:05

Berechtigungen

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

fchao-Sinus-Wechselrichter AliExpress