RP6 Sensor Board und Xtra Module: Unterschied zwischen den Versionen

Version vom 20. April 2014, 16:58 Uhr (Quelltext anzeigen) Dirk (Diskussion | Beiträge) K (→‎Demo) ← Zum vorherigen Versionsunterschied		Version vom 20. April 2014, 17:02 Uhr (Quelltext anzeigen) Dirk (Diskussion | Beiträge) K (→‎Library Header) Zum nächsten Versionsunterschied →
Zeile 663:		Zeile 663:
	======Library Header======		======Library Header======
−	<pre></pre>	+	Datei: RP6M256_Sensor_BoardLib.h
		+	<pre>/*
		+	* ****************************************************************************
		+	* RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
		+	* ****************************************************************************
		+	* Example: RP6M256 Sensor Board Library
		+	* Author(s): Dirk
		+	* ****************************************************************************
		+	* Description:
		+	* Header file for new RP6 Sensor Board library.
		+	*
		+	* ****************************************************************************
		+	*/
		+
		+	#ifndef RP6M256_SENSOR_BOARDLIB_H
		+	#define RP6M256_SENSOR_BOARDLIB_H
		+
		+
		+	/*****************************************************************************/
		+	// RP6 Sensor Board hardwired components:
		+	// - I2C I/O Port Expander (MAX7311)
		+	// - I2C Real Time Clock (RTC DS1339)
		+	// - I2C Temperature Sensor (LM75B)
		+	// - I2C Servo Controller (PCA9685)
		+	// - I2C LED Driver (TCA6507)
		+	// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
		+	// - Analog Yaw-Rate Gyroscope (LY330ALH)
		+	// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
		+	// - Button
		+	// - Status LED
		+
		+	/*****************************************************************************/
		+	// Includes:
		+
		+	#include "RP6M256Lib.h"
		+	#include "RP6I2CmasterTWI.h"
		+	#include "RP6M256_Sensor_Board.h"
		+	#include <math.h>
		+
		+	/*****************************************************************************/
		+	// RP6M256_I2CMasterLib definitions:
		+
		+	#ifndef I2C_RP6_BASE_ADR
		+	#define I2C_RP6_BASE_ADR 10
		+	#endif
		+	#ifndef I2C_REG_ADC_ADC0_L
		+	#define I2C_REG_ADC_ADC0_L 23
		+	#endif
		+	#ifndef I2C_REG_ADC_ADC1_L
		+	#define I2C_REG_ADC_ADC1_L 25
		+	#endif
		+	#ifndef CMD_POWER_OFF
		+	#define CMD_POWER_OFF 0
		+	#endif
		+	#ifndef CMD_POWER_ON
		+	#define CMD_POWER_ON 1
		+	#endif
		+
		+	/*****************************************************************************/
		+	// I2C I/O Port Expander (MAX7311):
		+
		+	// Registers:
		+	#define MAX7311_INPUT_PORT1 0
		+	#define MAX7311_INPUT_PORT2 1
		+	#define MAX7311_OUTPUT_PORT1 2
		+	#define MAX7311_OUTPUT_PORT2 3
		+	#define MAX7311_PORT1_POL_INV 4
		+	#define MAX7311_PORT2_POL_INV 5
		+	#define MAX7311_PORT1_CONFIG 6
		+	#define MAX7311_PORT2_CONFIG 7
		+	#define MAX7311_TIMEOUT 8
		+
		+	// Port 1 configuration register bitmasks:
		+	#define ST_GYRO (1 << PIN7) // I/O 0.7 (Output)
		+	#define PD_GYRO (1 << PIN6) // I/O 0.6 (Output)
		+	#define PWR_CAM_IF (1 << PIN5) // I/O 0.5 (Output)
		+	#define EN_LEDDRIV (1 << PIN4) // I/O 0.4 (Output)
		+	#define RDY_BUSBUFF (1 << PIN3) // I/O 0.3 (Input)
		+	#define PWR_SHARP_R (1 << PIN2) // I/O 0.2 (Output)
		+	#define PWR_SHARP_F (1 << PIN1) // I/O 0.1 (Output)
		+	#define PWR_I2C (1 << PIN0) // I/O 0.0 (Output)
		+
		+	// Port 2 configuration register bitmasks:
		+	#define IO2_I2C (1 << PIN7) // I/O 1.7 (Input)
		+	#define IO1_I2C (1 << PIN6) // I/O 1.6 (Output)
		+	#define DRDY_MAGACC (1 << PIN5) // I/O 1.5 (Input)
		+	#define PWR_SERVO (1 << PIN4) // I/O 1.4 (Output)
		+	#define OEN_SERVO (1 << PIN3) // I/O 1.3 (Output)
		+	#define IO_K19_1 (1 << PIN2) // I/O 1.2 (Input)
		+	#define IO_K19_3 (1 << PIN1) // I/O 1.1 (Input)
		+	#define EN_BUSBUFF (1 << PIN0) // I/O 1.0 (Output)
		+
		+	// Port 1 & 2 register 16-bit bitmasks:
		+	// (CONFIG: 1 = Input, 0 = Output)
		+	#define MAX7311_CONFIG_DEFAULT 0b1010011000001000
		+	#define MAX7311_CONFIG_USE_PD5 0b1010011100001000
		+	// (OUTPUT: 1 = High/True, 0 = Low/False)
		+	#define MAX7311_OUTPUT_DEFAULT 0b0100000100010000
		+	#define MAX7311_OUTPUT_FULLPOWER 0b0001000100110111
		+	#define MAX7311_OUTPUT_SHUTDOWN 0b0100100011000000
		+
		+	// Safe configuration:
		+	// (Ensures if activated, that I/Os 0.3, 1.5 and 1.7 cannot be configured
		+	//  as output with the function MAX7311_write_cfg(). This may protect the
		+	//  RP6 Sensor Board from hardware damage.)
		+	// -------------------
		+	#define SAFE_CONFIG
		+	// -------------------
		+
		+	typedef union {
		+	uint8_t byte;
		+	struct {
		+	unsigned int1  :1; // INT1  XBUS Pin 8
		+	unsigned int2  :1; // INT2  XBUS Pin 11
		+	unsigned int3  :1; // INT3  XBUS Pin 9
		+	unsigned intu  :1; // INTU  XBUS Pin 7
		+	unsigned unused :4;
		+	};
		+	struct {
		+	unsigned magacc1    :1; // INT1: LSM303DLHC INT1
		+	unsigned portexp    :1; // INT2: MAX7311 INT_N
		+	unsigned magacc2_rtc :1; // INT3: LSM303DLHC INT2
		+	unsigned noname      :5; //      OR DS1339 SQW/INT_N
		+	};
		+	} interrupt_RP6SBstatus_t;
		+	extern interrupt_RP6SBstatus_t interrupt_RP6SBstatus;
		+
		+	typedef union {
		+	uint16_t word;
		+	struct {
		+	unsigned pwr_i2c    :1; // I/O 0.0 (Output)
		+	unsigned pwr_sharp_f :1; // I/O 0.1 (Output)
		+	unsigned pwr_sharp_r :1; // I/O 0.2 (Output)
		+	unsigned rdy_busbuff :1; // I/O 0.3 (Input)
		+	unsigned en_leddriv  :1; // I/O 0.4 (Output)
		+	unsigned pwr_cam_if  :1; // I/O 0.5 (Output)
		+	unsigned pd_gyro    :1; // I/O 0.6 (Output)
		+	unsigned st_gyro    :1; // I/O 0.7 (Output)
		+	unsigned en_busbuff  :1; // I/O 1.0 (Output)
		+	unsigned io_k19_3    :1; // I/O 1.1 (Input)
		+	unsigned io_k19_1    :1; // I/O 1.2 (Input)
		+	unsigned oen_servo  :1; // I/O 1.3 (Output)
		+	unsigned pwr_servo  :1; // I/O 1.4 (Output)
		+	unsigned drdy_magacc :1; // I/O 1.5 (Input)
		+	unsigned io1_i2c    :1; // I/O 1.6 (Output)
		+	unsigned io2_i2c    :1; // I/O 1.7 (Input)
		+	};
		+	} max7311IOs_t;
		+	extern max7311IOs_t inport;
		+	extern max7311IOs_t outport;
		+	extern uint8_t readyBusbuff;
		+	extern uint8_t pressedButton;
		+
		+	void MAX7311_write_cfg(uint16_t);
		+	void task_checkINTs(void);
		+	void task_RP6SensorBoard(void);
		+	void MAX7311_update(void);
		+	void MAX7311_write(uint16_t);
		+	void MAX7311_init(void);
		+
		+	/*****************************************************************************/
		+	// I2C Real Time Clock (RTC DS1339):
		+
		+	// Registers:
		+	#define DS1339_SECONDS 0
		+	#define DS1339_MINUTES 1
		+	#define DS1339_HOURS 2
		+	#define DS1339_DAY 3
		+	#define DS1339_DATE 4
		+	#define DS1339_MONTH_CENTURY 5
		+	#define DS1339_YEAR 6
		+	#define DS1339_ALARM1_SECONDS 7
		+	#define DS1339_ALARM1_MINUTES 8
		+	#define DS1339_ALARM1_HOURS 9
		+	#define DS1339_ALARM1_DAY_DATE 10
		+	#define DS1339_ALARM2_MINUTES 11
		+	#define DS1339_ALARM2_HOURS 12
		+	#define DS1339_ALARM2_DAY_DATE 13
		+	#define DS1339_CONTROL 14
		+	#define DS1339_STATUS 15
		+	#define DS1339_TRICKLE_CHARGER 16
		+
		+	// Control register bitmasks:
		+	#define DS1339_CONTROL_DEFAULT 0 // Power on default
		+	#define DS1339_CONTROL_A1IE 1
		+	#define DS1339_CONTROL_A2IE 2
		+	#define DS1339_CONTROL_INTCN 4
		+	#define DS1339_CONTROL_RS1 8
		+	#define DS1339_CONTROL_RS2 16
		+	#define DS1339_CONTROL_BBSQI 32
		+	#define DS1339_CONTROL_EOSC 128
		+
		+	// Trickle Charger register bitmasks:
		+	#define DS1339_TRICKLE_CHARGER_DEFAULT 0b10101010 // One diode, 2k resistor
		+	#define DS1339_TRICKLE_CHARGER_ROUT0 1
		+	#define DS1339_TRICKLE_CHARGER_ROUT1 2
		+	#define DS1339_TRICKLE_CHARGER_DS0 4
		+	#define DS1339_TRICKLE_CHARGER_DS1 8
		+	#define DS1339_TRICKLE_CHARGER_TCS0 16
		+	#define DS1339_TRICKLE_CHARGER_TCS1 32
		+	#define DS1339_TRICKLE_CHARGER_TCS2 64
		+	#define DS1339_TRICKLE_CHARGER_TCS3 128
		+
		+	enum RTCWEEKDAYS {
		+	R_MO = 1, R_TU, R_WE, R_TH, R_FR, R_SA, R_SU
		+	};
		+
		+	typedef struct {
		+	uint16_t        year; // Year
		+	uint8_t          month; // Month  [1..12]
		+	enum RTCWEEKDAYS weekday; // Weekday [1..7 = R_MO..R_SU]
		+	uint8_t          day; // Day    [1..31]
		+	} rtcdate_t;
		+	rtcdate_t rtc_date;
		+
		+	typedef struct {
		+	uint8_t dst; // Daylight-saving-time (time zone)
		+	uint8_t hour; // Hour    [0..23]
		+	uint8_t minute; // Minute  [0..59]
		+	uint8_t second; // Second  [0..59]
		+	} rtctime_t;
		+	rtctime_t rtc_time;
		+
		+	uint8_t BCD2DEC(uint8_t);
		+	uint8_t DEC2BCD(uint8_t);
		+	void DS1339_write_cfg(uint8_t);
		+	void DS1339_init(void);
		+	#define CALC_DST // Time zone will be calculated
		+	void DS1339_read(void);
		+	void DS1339_write(void);
		+
		+	/*****************************************************************************/
		+	// I2C Temperature Sensor (LM75B):
		+
		+	// Registers:
		+	#define LM75B_TEMP 0
		+	#define LM75B_CONF 1
		+	#define LM75B_THYST 2
		+	#define LM75B_TOS 3
		+
		+	// Conf register bitmasks:
		+	#define LM75B_CONF_DEFAULT 0 // Power on default
		+	#define LM75B_CONF_SHUTDOWN 1
		+	#define LM75B_CONF_OS_COMP_INT 2
		+	#define LM75B_CONF_OS_POL 4
		+	#define LM75B_CONF_OS_F_QUE0 8
		+	#define LM75B_CONF_OS_F_QUE1 16
		+
		+	extern double temperature;
		+
		+	void LM75B_write_cfg(uint8_t);
		+	#define LM75B_shutdown() {LM75B_write_cfg(LM75B_CONF_SHUTDOWN);}
		+	#define LM75B_init() {LM75B_write_cfg(LM75B_CONF_DEFAULT);}
		+	extern uint8_t temperature_low;
		+	extern uint8_t temperature_high;
		+	#define getTemperatureHigh() (temperature_high)
		+	#define getTemperatureLow() (temperature_low)
		+	void LM75B_read(void);
		+	double LM75B_calculate(void);
		+	double LM75B_measure(void);
		+
		+	/*****************************************************************************/
		+	// I2C Servo Controller (PCA9685):
		+
		+	// Registers:
		+	#define PCA9685_MODE1 0
		+	#define PCA9685_MODE2 1
		+	#define PCA9685_SUBADR1 2
		+	#define PCA9685_SUBADR2 3
		+	#define PCA9685_SUBADR3 4
		+	#define PCA9685_ALLCALLADR 5
		+	#define PCA9685_LED0_ON_L 6
		+	#define PCA9685_LED0_ON_H 7
		+	#define PCA9685_LED0_OFF_L 8
		+	#define PCA9685_LED0_OFF_H 9
		+	#define PCA9685_LED1_ON_L 10
		+	#define PCA9685_LED1_ON_H 11
		+	#define PCA9685_LED1_OFF_L 12
		+	#define PCA9685_LED1_OFF_H 13
		+	#define PCA9685_LED2_ON_L 14
		+	#define PCA9685_LED2_ON_H 15
		+	#define PCA9685_LED2_OFF_L 16
		+	#define PCA9685_LED2_OFF_H 17
		+	#define PCA9685_LED3_ON_L 18
		+	#define PCA9685_LED3_ON_H 19
		+	#define PCA9685_LED3_OFF_L 20
		+	#define PCA9685_LED3_OFF_H 21
		+	#define PCA9685_LED4_ON_L 22
		+	#define PCA9685_LED4_ON_H 23
		+	#define PCA9685_LED4_OFF_L 24
		+	#define PCA9685_LED4_OFF_H 25
		+	#define PCA9685_LED5_ON_L 26
		+	#define PCA9685_LED5_ON_H 27
		+	#define PCA9685_LED5_OFF_L 28
		+	#define PCA9685_LED5_OFF_H 29
		+	#define PCA9685_LED6_ON_L 30
		+	#define PCA9685_LED6_ON_H 31
		+	#define PCA9685_LED6_OFF_L 32
		+	#define PCA9685_LED6_OFF_H 33
		+	#define PCA9685_LED7_ON_L 34
		+	#define PCA9685_LED7_ON_H 35
		+	#define PCA9685_LED7_OFF_L 36
		+	#define PCA9685_LED7_OFF_H 37
		+	#define PCA9685_LED8_ON_L 38
		+	#define PCA9685_LED8_ON_H 39
		+	#define PCA9685_LED8_OFF_L 40
		+	#define PCA9685_LED8_OFF_H 41
		+	#define PCA9685_LED9_ON_L 42
		+	#define PCA9685_LED9_ON_H 43
		+	#define PCA9685_LED9_OFF_L 44
		+	#define PCA9685_LED9_OFF_H 45
		+	#define PCA9685_LED10_ON_L 46
		+	#define PCA9685_LED10_ON_H 47
		+	#define PCA9685_LED10_OFF_L 48
		+	#define PCA9685_LED10_OFF_H 49
		+	#define PCA9685_LED11_ON_L 50
		+	#define PCA9685_LED11_ON_H 51
		+	#define PCA9685_LED11_OFF_L 52
		+	#define PCA9685_LED11_OFF_H 53
		+	#define PCA9685_LED12_ON_L 54
		+	#define PCA9685_LED12_ON_H 55
		+	#define PCA9685_LED12_OFF_L 56
		+	#define PCA9685_LED12_OFF_H 57
		+	#define PCA9685_LED13_ON_L 58
		+	#define PCA9685_LED13_ON_H 59
		+	#define PCA9685_LED13_OFF_L 60
		+	#define PCA9685_LED13_OFF_H 61
		+	#define PCA9685_LED14_ON_L 62
		+	#define PCA9685_LED14_ON_H 63
		+	#define PCA9685_LED14_OFF_L 64
		+	#define PCA9685_LED14_OFF_H 65
		+	#define PCA9685_LED15_ON_L 66
		+	#define PCA9685_LED15_ON_H 67
		+	#define PCA9685_LED15_OFF_L 68
		+	#define PCA9685_LED15_OFF_H 69
		+	#define PCA9685_ALL_LED_ON_L 250
		+	#define PCA9685_ALL_LED_ON_H 251
		+	#define PCA9685_ALL_LED_OFF_L 252
		+	#define PCA9685_ALL_LED_OFF_H 253
		+	#define PCA9685_PRE_SCALE 254
		+	#define PCA9685_TESTMODE 255
		+
		+	// Mode1 register bitmasks:
		+	#define PCA9685_MODE1_ALLCALL 1
		+	#define PCA9685_MODE1_SUB3 2
		+	#define PCA9685_MODE1_SUB2 4
		+	#define PCA9685_MODE1_SUB1 8
		+	#define PCA9685_MODE1_SLEEP 16
		+	#define PCA9685_MODE1_AI 32
		+	#define PCA9685_MODE1_EXTCLK 64
		+	#define PCA9685_MODE1_RESTART 128
		+
		+	// Mode2 register bitmasks:
		+	#define PCA9685_MODE2_OUTNE01_DEFAULT 0
		+	#define PCA9685_MODE2_OUTDRV 4
		+	#define PCA9685_MODE2_OCH 8
		+	#define PCA9685_MODE2_INVRT 16
		+
		+	#define F_PCA9685 25000000.0 // Int. Clock: 25 MHz
		+
		+	void PCA9685_init(uint16_t);
		+	#define initServo(__FREQ__) {PCA9685_init(__FREQ__);}
		+	uint8_t servo(uint8_t);
		+	void PCA9685_set(uint8_t, uint16_t);
		+	#define setServo(__SERVO__,__POS__) {PCA9685_set(servo(__SERVO__),__POS__);}
		+	void PCA9685_shutdown(void);
		+	void PCA9685_restart(void);
		+	void setServoPower(uint8_t);
		+
		+	/*****************************************************************************/
		+	// I2C LED Driver (TCA6507):
		+
		+	// Registers:
		+	#define TCA6507_SELECT0 0
		+	#define TCA6507_SELECT1 1
		+	#define TCA6507_SELECT2 2
		+	#define TCA6507_FADEON_TIME 3
		+	#define TCA6507_FULLYON_TIME 4
		+	#define TCA6507_FADEOFF_TIME 5
		+	#define TCA6507_1ST_FULLYOFF_TIME 6
		+	#define TCA6507_2ND_FULLYOFF_TIME 7
		+	#define TCA6507_MAXIMUM_INTENSITY 8
		+	#define TCA6507_MASTER_INTENSITY 9
		+	#define TCA6507_INITIALIZATION 10
		+
		+	// Output states:
		+	#define STATE_OFF 0
		+	#define STATE_PWM0 2
		+	#define STATE_PWM1 3
		+	#define STATE_ON 4
		+	#define STATE_MINT 5
		+	#define STATE_BLINK0 6
		+	#define STATE_BLINK1 7
		+
		+	extern uint8_t select[3];
		+
		+	void TCA6507_write_cfg(uint8_t, uint8_t);
		+	void TCA6507_update(void);
		+	void TCA6507_set(uint8_t, uint8_t);
		+	#define setLED(__LED__,__STATE__) {TCA6507_set(__LED__,__STATE__);}
		+	void TCA6507_shutdown(void);
		+	void TCA6507_restart(void);
		+	void TCA6507_init(void);
		+
		+	/*****************************************************************************/
		+	// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):
		+
		+	// LSM303DLHC accelerometer internal registers:
		+	#define CTRL_REG1_A 0x20
		+	#define CTRL_REG2_A 0x21
		+	#define CTRL_REG3_A 0x22
		+	#define CTRL_REG4_A 0x23
		+	#define CTRL_REG5_A 0x24
		+	#define CTRL_REG6_A 0x25
		+	#define REFERENCE_A 0x26
		+	#define STATUS_REG_A 0x27
		+	#define OUT_X_L_A 0x28
		+	#define OUT_X_H_A 0x29
		+	#define OUT_Y_L_A 0x2a
		+	#define OUT_Y_H_A 0x2b
		+	#define OUT_Z_L_A 0x2c
		+	#define OUT_Z_H_A 0x2d
		+	#define FIFO_CTRL_REG_A 0x2e
		+	#define FIFO_SRC_REG_A 0x2f
		+	#define INT1_CFG_A 0x30
		+	#define INT1_SOURCE_A 0x31
		+	#define INT1_THS_A 0x32
		+	#define INT1_DURATION_A 0x33
		+	#define INT2_CFG_A 0x34
		+	#define INT2_SOURCE_A 0x35
		+	#define INT2_THS_A 0x36
		+	#define INT2_DURATION_A 0x37
		+	#define CLICK_CFG_A 0x38
		+	#define CLICK_SRC_A 0x39
		+	#define CLICK_THS_A 0x3a
		+	#define TIME_LIMIT_A 0x3b
		+	#define TIME_LATENCY_A 0x3c
		+	#define TIME_WINDOW_A 0x3d
		+
		+	// LSM303DLHC accelerometer register settings:
		+	#define CTRL_REG1_A_10HZ 0b00100111 // 10Hz & all axes enable
		+	#define CTRL_REG1_A_50HZ 0b01000111 // 50Hz & all axes enable
		+	#define CTRL_REG4_A_DEFAULT 0b00000000 // Default (Normal mode)
		+	#define CTRL_REG4_A_HR 0b00001000 // HR output mode
		+
		+	// LSM303DLHC magnetometer internal registers:
		+	#define CRA_REG_M 0x00
		+	#define CRB_REG_M 0x01
		+	#define MR_REG_M 0x02
		+	#define OUT_X_H_M 0x03
		+	#define OUT_X_L_M 0x04
		+	#define OUT_Z_H_M 0x05
		+	#define OUT_Z_L_M 0x06
		+	#define OUT_Y_H_M 0x07
		+	#define OUT_Y_L_M 0x08
		+	#define SR_REG_MG 0x09
		+	#define IRA_REG_M 0x0a
		+	#define IRB_REG_M 0x0b
		+	#define IRC_REG_M 0x0c
		+	#define TEMP_OUT_H_M 0x31
		+	#define TEMP_OUT_L_M 0x32
		+
		+	// LSM303DLHC magnetometer register settings:
		+	#define CRA_REG_M_15HZ 0b00010000 // Default (Data rate 15Hz)
		+	#define CRA_REG_M_15HZ_T_EN 0b10010000 // 15Hz & temperature enable
		+	#define CRB_REG_M_13GAUSS 0b00100000 // Gain +-1.3Gauss
		+	#define MR_REG_M_CCM 0b00000000 // Continuous-conversion mode
		+	#define MR_REG_M_SCM 0b00000001 // Single-conversion mode
		+	#define MR_REG_M_SLEEP 0b00000011 // Default (Sleep-mode)
		+
		+	extern int16_t x_axisa, y_axisa, z_axisa;
		+	extern double pitch, roll;
		+	extern double xa, ya, za;
		+	extern double the, phi;
		+
		+	extern int16_t x_axism, y_axism, z_axism;
		+	extern int16_t headingm;
		+	extern double xm, ym, zm;
		+	#ifdef GET_TEMP_M
		+	extern int16_t temperaturem;
		+	extern double temperature_mag;
		+	#endif
		+
		+	extern int16_t headingtc;
		+
		+	void LSM303DLHC_A_init(void);
		+	void readLSM303DLHC_A(void);
		+	void normalizeLSM303DLHC_A(void);
		+	void positionLSM303DLHC_A(void);
		+
		+	void LSM303DLHC_M_init(void);
		+	void readLSM303DLHC_M(void);
		+	void normalizeLSM303DLHC_M(void);
		+	int16_t headingLSM303DLHC_M(void);
		+
		+	int16_t headingLSM303DLHC_TC(void);
		+
		+	void LSM303DLHC_init(void);
		+
		+	/*****************************************************************************/
		+	// Analog Yaw-Rate Gyroscope (LY330ALH):
		+
		+	#define LY330ALH_MODE_NORMAL 0 // Power on default
		+	#define LY330ALH_MODE_SLEEP 1
		+	#define LY330ALH_MODE_SELFTEST 2
		+	#define LY330ALH_MODE_POWERDOWN 3 // Shutdown mode
		+
		+	extern int16_t z_axisg;
		+
		+	void LY330ALH_set_mode(uint8_t);
		+	uint16_t getRP6Base_ADC1(void);
		+	int16_t LY330ALH_measure(void);
		+
		+	/*****************************************************************************/
		+	// Hot Swappable 2-Wire Bus Buffer (TCA4311A):
		+
		+	//extern uint8_t readyBusbuff; // See "I2C I/O Port Expander" section
		+	extern uint8_t errorBusbuff;
		+
		+	#define getReadyBusbuff() (readyBusbuff)
		+	uint8_t readReadyBusbuff(void);
		+	void TCA4311A_shutdown(void);
		+	void TCA4311A_reconnect(void);
		+	void setRP6Base_PWR(uint8_t);
		+	void setI2CModulePower(uint8_t);
		+
		+	/*****************************************************************************/
		+	// Button:
		+
		+	//extern uint8_t pressedButton; // See "I2C I/O Port Expander" section
		+
		+	#define getPressedButton() (pressedButton)
		+	uint8_t readButton(void);
		+
		+	/*****************************************************************************/
		+	// Status LED:
		+
		+	void setStatusLED(uint8_t);
		+
		+	/*****************************************************************************/
		+	// RP6 Sensor Board initialisation and shutdown:
		+
		+	void sensor_board_init(void);
		+	void sensor_board_shutdown(void);
		+
		+	/*****************************************************************************/
		+
		+	#endif
		+
		+	/******************************************************************************
		+	* Additional info
		+	* ****************************************************************************
		+	* Changelog:
		+	*
		+	*  ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
		+	*
		+	* ****************************************************************************
		+	*/
		+
		+	/*****************************************************************************/
		+	// EOF
		+	</pre>
	======Library Source======		======Library Source======

---

Version vom 20. April 2014, 17:02 Uhr

Was bisher geschah

Siehe auch "Was bisher geschah" im RP6v2 und im RP6 Artikel!

Mit, 27. Nov 2013 - Ankündigung einer Sensorerweiterungsplatine, die an die RP6v2-M256-WIFI angeschlossen werden kann
Sam, 22. Feb 2014 - Auslieferungsbeginn des RP6 Sensor Boards, des I2C GPS Empfängers, des 3D Accelerometer Moduls und des Gyro Moduls
Mon, 17. Mär 2014 - Das RP6 Sensor Board ist "nicht lieferbar", neuer Liefertermin angekündigt für den 12.06.2014 (!?)
Die, 25. Mär 2014 - Das RP6 Sensor Board ist wieder "sofort lieferbar"

Allgemein

Dieser Artikel beschreibt die Grundlagen und Programmierung des RP6 Sensor Boards und der RP6 Xtra Module. Dieses Erweiterungs-Modul und die Xtra Module sind Teile des RP6-Systems, das in den RN-Wissen-Artikeln RP6, RP6v2 und RP6 - Programmierung ausführlich beschrieben wird. Ausgegliedert aus den "Haupt-Artikeln" habe ich diese Module, da sie m.E. zu einer neuen "Ära" des RP6-Systems gehören:

Sie wurden nicht mehr von SlyD (Dominik S. Herwald) in Zusammenarbeit mit AREXX entwickelt, sondern nun von JM3 Engeneering (Dipl.-Ing. Jürgen Maisel). Ab Februar 2014 waren dann das RP6 Sensor Board (1082384) und die drei RP6 Xtra Module I2C GPS Empfänger JM3-GPS (1082385), Gyro Modul JM3-GYRO (1082386) und 3D Accelerometer Modul JM3-3DA (1082387) bei CONRAD lieferbar. Einen ersten Eindruck vom Sensor Board kann man im RoboterNetz und AREXX Support Forum bekommen,- siehe unter Weblinks!

RP6 Sensor Board

Das RP6 Sensor Board gehört zu den "Erweiterungs-Modulen" des RP6-Systems. Andere "Erweiterungs-Module" werden im RP6v2 und im RP6 Artikel beschrieben!

Das RP6 Sensor Board ist das erste "Erweiterungs-Modul", das nicht über einen eigenen Microprozessor verfügt,- es ist also ein passives Modul, das von einem Microprozessor-System (z.B. RP6v2, M32, M128, M256 Wifi) angesteuert werden muss. Es ist ebenfalls das erste Modul, das nicht mehr über die rautenförmige Einbuchtung an der Vorderkante verfügt, sondern komplett rechteckig ist. Damit passt es (montiert auf den Standard-Abstandbolzen) nur auf den vorderen Platinenstapel 1 des RP6.

RP6 Xtra Module

Die RP6 Xtra Module gehören zu den "Zubehör- und Ersatzteilen" des RP6-Systems. Über die weiteren "Zubehör- und Ersatzteile" zum RP6 informiert der entsprechende Abschnitt im RP6v2 und im RP6 Artikel!

Die RP6 Xtra Module unterscheiden sich von den weiteren "Zubehör- und Ersatzteilen" dadurch, dass sie selbstständige Sensor-Platinen sind, die ideal zum RP6-System passen, aber auch mit anderen Microcontroller-Plattformen nutzbar sind.

RP6 Sensor Board

Das RP6 Sensor Board (1082384) wurde am 27.11.2013 von SlyD angekündigt (siehe Weblink!).

Beschreibung

(Laut Online-Katalog der Firma CONRAD!)

"Das RP6 Sensor Board erweitert die Möglichkeiten aller RP6 Roboter um wesentliche Sensorsignale für Robotik-Anwendungen wie zum Beispiel Dreharten (Yaw-Gyro), Kipp- und Neigewinkel, Kompassrichtung oder aber auch Zeit und Datum. Sie können einfach mit zusätzlichen Sharp Sensoren Abstände genau messen, z.B. für autonome Fahrrouten oder falls der Sensor nach unten gerichtet ist, kann er Treppenstufen oder ähnliches entdecken. Außerdem gibt es die Möglichkeit z.B. die Sharp Sensoren ein- bzw. auszuschalten um Strom zu sparen. Zusätzlich können bis zu 8 Servos angesteuert werden um z.B. einen Roboterarm zu bewegen, der auch mechanisch einfach zu montieren ist. Um die Möglichkeiten des RP6 Sensor Boards komplett ausnutzen zu können, wird ein RP6V2 und RP6Wifi Control Board empfohlen. Die ermöglicht den Roboter über Wifi fernzusteuern oder Sensordaten auf dem Kommandozentrum (PC) z.B Kompassdaten, Temperatur oder Kipp- und Neigewinkel anzeigen zu lassen."

Ausstattung

• Magnetometer- und Accelerometer für Kompassfunktion
• Gyro Sensor (Yaw)
• Sharp-Sensor Schnittstelle
• LED Scheinwerfer
• LED Treiber mit Smart Funktionen
• I²C Bus mit max. 400 kHz Übertragungsrate; Hot Swap I²C-Bus Buffer
• Taster mit LED
• Echtzeit-Uhr RTC
• Backupbatterie (Aufladung während des Betriebs)
• Expansion Port z.B. für GPS-I²C Bus Modul
• 8 Servo-Ausgänge mit eigenem Schaltregler
• 8 PWM Ausgänge
• 5 LED Ausgänge und 2 Digital-I/O Ausgänge.

Lieferumfang

• RP6 Sensor Board
• Montage-Kit
• Kabel
• Anleitung und CD mit Dokumentation.

Blockschaltbild

Technische Daten

Mikrocontroller:	kein
Speicher:	kein
Programmierung:	C++ (GCC, u.a.) über externes µC-System
Vorhandene Sensoren:	Temperatur, 3D Magnetfeld (Kompass), 3D Beschleunigung, Yaw-Gyro
Vorhandene Aktoren:	LED Controller (2 LEDs on board, 5 freie LED Ports), Status LED, Button, Servo (PWM) Controller für 8 Servos und 8 PWM-Kanäle, Real Time Clock (RTC), 2 freie I/O Ports
Abmessungen:	(L × B × H) 112 × 90 × 19 mm (B + 5 mm durch Scheinwerfer vorn)
Ausführung:	Fertig aufgebautes Erweiterungsmodul
Stromversorgung:	6 AA NiMH Akkus (über die RP6v2 Base) und optionaler Zusatzakku 7,2..10 V für Servo Power
Hersteller:	Arexx Niederlande

Umbau-Optionen

USRBUS- und XBUS-Wannenstecker

Der zweite Wannenstecker für den XBUS und USRBUS kann nachbestückt werden. Man braucht dazu:

• 2 Wannenstecker 2x5-polig gerade RM 2,54mm (CONRAD 741648)

Die Wannenstecker lötet man auf die eingezeichneten Positionen.

USRBUS-Belegung:

Stecker-Pin	USRBUS	Sensor Board-Funktion	Verwendung	I/O
1	Y1	"ADC0"	SHARP vorn	I
2	Y2	"ADC1"	Yaw-Gyro	I
3	Y3	PWR	Power für SHARPs, CAM_IF, I2C-MODULE	O
4	Y4		keine
5	Y5	"ADC4"	SHARP hinten	I
6..14	Y6..Y14		keine

XBUS-Belegung:

Stecker-Pin	XBUS	M256-Port	M256-Funktion	Sensor Board-Funktion	Jumper	Verwendung	I/O
1	GND		GND	GND
2	GND		GND	GND
3	VDD		VDD	VDD_1		VDD +5V
4	+UB		+UB	VSS_1		CAM_IF Power
5	VDD		VDD	VDD_1		VDD +5V
6	+UB		+UB	VSS_1		CAM_IF Power
7	INTU	PJ4	PCINT13	INTU		keine	I
8	INT1	PJ3	PCINT12	INT1	3	LSM303DLHC: INT1	I
9	INT3	PJ5	PCINT14	INT3	2 5	DS1339: SQW/INT_N LSM303DLHC: INT2	I I
10	SCL	PD0	SCL	SCL_M		I2C-Bus	O
11	INT2	PJ6	PCINT15	INT2	4	MAX7311: INT_N	I
12	SDA	PD1	SDA	SDA_M		I2C-Bus	I/O
13	MRESET		MRESET	MRESET		keine
14	GND		GND	GND

I/O-Patchfield_1

Dieses 10-polige "Patchfield_1", beschriftet mit "I/O" liegt neben dem Wannenstecker I/O und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker I/O zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_1" eine Stiftleiste löten. Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "I/O". Zuvor muss man u.U. Lötzinnreste aus den Lötpunkten mit Entlötlitze bzw. Entlötsaugpumpe entfernen.

Stecker I/O an M256 IO_PWM/T0/T1:

Stecker-Pin	M256-Port	M256-Funktion	Sensor Board-Funktion	Verwendung	I/O
1	PD4	ICP1		keine
2		GND		keine
3	PD6	T1		keine
4	PB7	OC0A		keine
5	PG5	OC0B		keine
6	PD7	T0		keine
7	PB5	OC1A		keine
8	PK5	ADC13		keine
9	PB6	OC1B	"PD5"	I2C Bus Buffer Enable	O
10		VDD		keine

ADC-Patchfield_2

Dieses 10-polige "Patchfield_2", beschriftet mit "ADC" liegt neben dem Wannenstecker ADC und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker ADC zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_2" eine Stiftleiste löten. Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "ADC".

Stecker ADC an M256 ADC_IO2/CMP:

Stecker-Pin	M256-Port	M256-Funktion	Sensor Board-Funktion	Jumper	Verwendung	I/O
1	PK0	ADC8			keine
2		GND			keine
3	PK1	ADC9	"ADC4"	6	SHARP hinten	I
4	PE3	OC3A/AIN1			keine
5	PK2	ADC10			keine
6		GND			keine
7	PK3	ADC11			keine
8	PE2	XCK0/AIN0			keine
9	PK4	ADC12			keine
10		VDD			keine

Anschluss-Optionen

I2C-MODULE

IO_EXT

PWM_OUT

TP1

LEDs

Rücklichter

Display-Hintergrundbeleuchtung

Freie Ausgänge

SHARP Sensoren

Vorn

Hinten

Servo Power

Roboterarm RA2-HOBBY

RP6 Xtra Module

Von den AREXX RP6 Xtra Modulen sind bei CONRAD für den RP6v2 erhältlich:

• Arexx I2C GPS Empfänger JM3-GPS (1082385)
• Arexx Gyro Modul JM3-GYRO (1082386)
• Arexx 3D Accelerometer Modul JM3-3DA (1082387)

I2C GPS Empfänger JM3-GPS

(Laut Online-Katalog der Firma CONRAD!)

"Universelles GPS-I²C-Bus Modul für RP6 Roboter. Der GPS Empfänger stellt die NMEA-0183 Datensätze GGA, RMC, GSA und VTG in den veschiedenen Registern zur Verfügung. Damit ist eine einfache Weiterverarbeitung möglich."

Beschreibung

Der I2C GPS Empfänger ist eine kleine (38 x 23 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf das RP6 Sensor Board aufgesteckt werden kann. Sie kann aber auch überall eingesetzt werden, wo eine stabilisierte 5V Versorgungsspannung und ein 5V-I2C-Bus bis 400 kHz zur Verfügung steht.

Die Platine ist bestückt mit dem "Fastrax GPS antenna module" UC530. Als Backup-Batterie dient eine wieder aufladbare Lithium-Batterie (MS621FE). Zur Kommunikation zwischen dem UC530 und dem I2C-Bus ist ein ATtiny1634 an Bord, der auch die Auswertung des NMEA Protokolls übernimmt. Die 3,3 V Versorgungsspannung wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt. Über den I2C-Bus können folgende Daten der NMEA-0183 Datensätze GGA, RMC, GSA und VTG auf einfache Weise gelesen werden:

• Geograf. Breite
• Geograf. Länge
• GPS Status
• HDOP
• Satellitenzahl
• Kurs
• Geschwindigkeit
• Höhe
• UTC-Zeit
• Datum
• PDOP
• VDOP
• Satellit 1..12

Der ATtiny1634 ist natürlich vorprogrammiert, kann aber über eine vorbereitete 6-polige ISP-Schnittstelle auf der Platine auch umprogrammiert werden.

Lieferumfang

• I2C GPS Empfänger
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• Versorgungsspannung: 5,0 V +/- 5%
• Stromverbrauch: <= 20 mA (typ.)
• I2C-Bus Geschwindigkeit: <= 400 kHz
• GPS Antenne: eingebaut
• Backup Batterie: Lithium, im Betrieb wieder aufladbar
• Temperatur Bereich: -10 bis 65 °C
• Form Faktor: 38 x 23 mm
• Gewicht: ca. 6 g
• Empfänger Spezifikationen (UC530):
  • Empfänger: GPS L1 C/A-Code, SPS
  • Kanäle: 66/22 (Suche/Track)
  • Tracking Empfindlichkeit: -165 dBm typ.
  • Navigations Empfindlichkeit: -148 dbm typ (Kaltstart)
  • Update Rate: 1 Sek.
  • Positions Genauigkeit:
    • 3,0m (67%) typ. horizontal
    • 5,0m (67%) typ. vertikal
    • 0,02 m/s (50%) typ. Geschwindigkeit
  • Zeit bis zum ersten Fix: typ. 31 Sek.
  • Protokoll: NMEA-0183 Rev. 3.01

Umbau-Optionen

Wenn man den ATtiny1634 selbst programmieren will, muss man Stiftleisten auf die Platine löten.

Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x3 Kontakte ab und lötet sie auf die Punkte "ISP6". Mit einem ISP-Programmer kann dann eine neue Firmware geflasht werden. Die aktuelle Firmware des ATtiny1634 (Version 1.2) wird als .HEX mitgeliefert, leider ohne Source-Code.

Vorsicht: Dabei kann der Prozessor im schlimmsten Fall nicht mehr ansprechbar sein oder der GPS Empfänger UC530 beschädigt werden. Man sollte diesen Umbau und die ISP-Programmierung nur machen, wenn man genau weiß, was man macht!

Gyro Modul JM3-GYRO

(Laut Online-Katalog der Firma CONRAD!)

"Das universelle Gyro-Sensor Modul für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 Wifi-Kit ist ideal zur Erkennung der Blockierung des Roboters in seiner Drehbewegung, sowie der Stabilisierung der Geradeausfahrt."

Beschreibung

Das Gyro Modul ist eine kleine (19 x 17 mm) Zusatzplatine (RP6 Xtra Modul), die im RP6-System genutzt werden kann. Es gibt zwar keinen direkt passenden "Steckplatz" auf einer der Microcontroller-Plattformen (RP6v2, M32, M128, M256 Wifi), jedoch ist der Anschluß einfach: Benötigt werden +5V und GND, sowie ein freier ADC-Kanal, der bis 5V messen kann. Damit kann das Gyro Modul sehr universell eingesetzt werden. Es misst die Drehrate um die Z-Achse (Hochachse oder Yaw).

Die Platine ist bestückt mit dem analogen Gyro-Sensor LY330ALH, dessen Ausgangsspannung durch zwei OP-Amps (LM321, LMP7731MF) in Ruhe bei 2,5 V liegt. Höhere Ausgangsspannungen (bis 5 V) bedeuten eine Drehgeschwindigkeit nach LINKS, niedrigere (bis 0 V) eine Drehgeschwindigkeit nach RECHTS. Die 3,3 V Versorgungsspannung des LY330ALH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.

Lieferumfang

• Gyro Modul
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• VDD: 5,0 V +/- 5%
• Vout(0): 2,5 V +/- 2% (ohne Bewegung)
• Vout: Vout(0) + 10mV / dps +/- 2%
• Messbereich: ca. 200 dps max.

Anschluss-Schema

Umbau-Optionen

Am Gyro Modul gibt es nichts umzubauen.

3D Accelerometer Modul JM3-3DA

(Laut Online-Katalog der Firma CONRAD!)

"Universeller 3-Achsen Beschleunigungssensor für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 WIFI-Kit. Ideal zur Winkelmessung (Neigung-, Kipp-Winkel) und vieles mehr."

Beschreibung

Das 3D Accelerometer Modul ist eine kleine (15 x 10 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf den EEPROM-Sockel (IC5) der Zusatzplatine RP6 CONTROL M32 gesteckt und über den SPI-Bus angesteuert werden kann. Das Accelerometer Modul kann aber auch mit allen Microcontroller-Plattformen betrieben werden, die eine stabilisierte 5V-Versorgungsspannung und einen SPI- oder I2C-Bus bieten.

Die Platine ist bestückt mit dem 3D Beschleunigungs-Sensor LIS302DLH. Die 3,3 V Versorgungsspannung des LIS302DLH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.

Lieferumfang

• 3D Accelerometer Modul
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• VDD: 5,0 V +/- 5%
• Messbereiche: +-2 g, +-4 g, +-8 g
• Datenausgabe: 16 Bit
• Schnittstellen: I2C, SPI

Umbau-Optionen

Will man das Modul auf der M32 einsetzen, muss man auf dem 3D Accelerometer Modul die Widerstände R2 und R3 auslöten (siehe Anleitung zum Modul, Seiten 4/5!) und einen IC-Sockel an Position IC5 und einen Kondensator an Position C19 auf der M32 einlöten, siehe hier!

Die Abbildung rechts zeigt das 3D Accelerometer Modul auf der M32.

Programmierung

RP6 Sensor Board

Dokumentation

Die Dokumentation zum RP6 Sensor Board befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".

Manuals

Das Handbuch kann auch hier eingesehen werden.

ACHTUNG: Das Manual enthält aktuell noch teils irreführende Beschreibungen!

Schematics

Datasheets

Demo-Programmme

Demo-Programme zum RP6 Sensor Board befinden sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".

Library

RP6 M256 WiFi

Configuration Header

Datei: RP6M256_Sensor_Board.h

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 Sensor Board Library
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Configuration header file for new RP6 Sensor Board library.
 * The RP6 Sensor Board (CONRAD 1082384) is connected to the RP6M256 with the
 * two plugs:
 *   Sensor Board <---> RP6M256
 *   --------------------------------
 *   I/O (K17)    <---> IO_PWM/T0/T1
 *   ADC (K18)    <---> ADC_IO2/CMP
 * The RP6 Sensor Board function "PD5" (IC6: EN) is now connected to RP6M256
 * OC1B (PB6) and the RP6 Sensor Board function "ADC4" (SHARP_R) to RP6M256
 * ADC9 (PK1).
 * On the RP6 Sensor Board the functions "ADC0", "ADC1", "PWR" and "ADC4" are
 * connected to the USRBUS. The USRBUS may be wired on the RP6 mainboard like
 * this:
 *   Function : Sensor Board - USRBUS: * <---> RP6 Mainboard
 *   --------------------------------------------------------
 *   SHARP_F  : "ADC0"       - Pin 1     <---> ADC0: Pin 3
 *   GYRO     : "ADC1"       - Pin 2     <---> ADC1: Pin 3
 *   PWRON ²  : "PWR"        - Pin 3     <---> PWR
 *   SHARP_R  : "ADC4"       - Pin 5     <---> Not connected!
 * At *) It is also possible not to use the USRBUs, but to connect the RP6
 *       Sensor Board directly to the RP6 mainboard with 3 single cables.
 * At ²) See RP6 manual "4.6.8. Stromsparfunktionen"! 
 *
 * ****************************************************************************
 */

#ifndef RP6M256_SENSOR_BOARD_H
#define RP6M256_SENSOR_BOARD_H


/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED

/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):
#define I2C_SENSOR_PORTEXP_ADR			0x40	// Default

// Interrupt I/O portpin definitions (RP6M256):
#define IO_SENSOR_INT1_IN				INT1_PI12 // PCINT12 PJ3  XBUS Pin 8
#define IO_SENSOR_INT1_DDR				DDRJ
#define IO_SENSOR_INT1_PIN				PINJ
#define IO_SENSOR_INT1_PORT				PORTJ
#define IO_SENSOR_INT2_IN				INT2_PI15 // PCINT15 PJ6  XBUS Pin 11
#define IO_SENSOR_INT2_DDR				DDRJ
#define IO_SENSOR_INT2_PIN				PINJ
#define IO_SENSOR_INT2_PORT				PORTJ
#define IO_SENSOR_INT3_IN				INT3_PI14 // PCINT14 PJ5  XBUS Pin 9
#define IO_SENSOR_INT3_DDR				DDRJ
#define IO_SENSOR_INT3_PIN				PINJ
#define IO_SENSOR_INT3_PORT				PORTJ
#define IO_SENSOR_INTU_IN				INTU_PI13 // PCINT13 PJ4  XBUS Pin 7
#define IO_SENSOR_INTU_DDR				DDRJ
#define IO_SENSOR_INTU_PIN				PINJ
#define IO_SENSOR_INTU_PORT				PORTJ

// I/O PORT pin numbers:
#define PIN7							7
#define PIN6							6
#define PIN5							5
#define PIN4							4
#define PIN3							3
#define PIN2							2
#define PIN1							1
#define PIN0							0

/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):
#define I2C_SENSOR_RTC_ADR				0xD0	// Default

/*****************************************************************************/
// I2C Temperature Sensor (LM75B):
#define I2C_SENSOR_TEMP_ADR				0x90	// Default

/*****************************************************************************/
// I2C Servo Controller (PCA9685):
#define I2C_SENSOR_SERVO_ADR			0x88	// Default

// Servo and PWM channels:
#define CH_SERVO_1						16		// At K6: 1  "SERVO_1"
#define CH_SERVO_2						15		// At K11: 1 "SERVO_2"
#define CH_SERVO_3						13		// At K29: 1 "SERVO_3"
#define CH_SERVO_4						14		// At K30: 1 "SERVO_4"
#define CH_SERVO_5						11		// At K31: 1 "SERVO_5"
#define CH_SERVO_6						12		// At K32: 1 "SERVO_6"
#define CH_SERVO_7						9		// At K33: 1 "SERVO_7"
#define CH_SERVO_8						10		// At K34: 1 "SERVO_8"

#define CH_PWM_OUT_1					1		// At K12: 1 "PWM_OUT"
#define CH_PWM_OUT_2					2		// At K12: 2
#define CH_PWM_OUT_3					3		// At K12: 3
#define CH_PWM_OUT_4					4		// At K12: 4
#define CH_PWM_OUT_5					5		// At K12: 5
#define CH_PWM_OUT_6					6		// At K12: 6
#define CH_PWM_OUT_7					7		// At K12: 7
#define CH_PWM_OUT_8					8		// At K12: 8

// Servo left touch (LT), right touch (RT), middle position (MP) constants:
// (Hints: - Servo impulse length [ms] = Servo position value / 204.8
//           (Formula only valid for a PWM of 50 Hz!)
//         - Min. servo impulse (0,7 ms) = Servo position 143
//         - Mid. servo impulse (1,5 ms) = Servo position 307
//         - Max. servo impulse (2,3 ms) = Servo position 471
//         - !!! You should NOT use servo position values < 143 or > 471 !!!)
#define SERVO1_LT						205		// Servo impulse ~1ms
#define SERVO1_RT						410		// Servo impulse ~2ms
#define SERVO1_MP						((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT)
#define SERVO2_LT						205
#define SERVO2_RT						410
#define SERVO2_MP						((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT)
#define SERVO3_LT						205
#define SERVO3_RT						410
#define SERVO3_MP						((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT)
#define SERVO4_LT						205
#define SERVO4_RT						410
#define SERVO4_MP						((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT)
#define SERVO5_LT						205
#define SERVO5_RT						410
#define SERVO5_MP						((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT)
#define SERVO6_LT						205
#define SERVO6_RT						410
#define SERVO6_MP						((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT)
#define SERVO7_LT						205
#define SERVO7_RT						410
#define SERVO7_MP						((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT)
#define SERVO8_LT						205
#define SERVO8_RT						410
#define SERVO8_MP						((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT)

/*****************************************************************************/
// I2C LED Driver (TCA6507):
#define I2C_SENSOR_LEDDRIVER_ADR		0x8A	// Default

// LED numbers:
#define LED0							0		// At K7
#define DISPLAY_BACKLIGHT				0		// Display background light
#define LED1							1		// At K4
#define LED2							2		// At K2
#define LED3							3		// At K10: 1
#define BACKLIGHT_R						3		// Right backlight
#define LED4							4		// At K10: 2
#define BACKLIGHT_L						4		// Left backlight
#define LED5							5		// White LED (D3)
#define HEADLIGHT_R						5		// Right headlight
#define LED6							6		// White LED (D1)
#define HEADLIGHT_L						6		// Left headlight

/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):

// LSM303DLHC accelerometer:
#define I2C_SENSOR_LSM303DLHC_A_ADR 	0x32	// Default

// LSM303DLHC accelerometer calibration data:
#define MAX_X_A						1037		// Max. X-axis value
#define MIN_X_A						-1030		// Min. X-axis value
#define MAX_Y_A						1030		// Max. Y-axis value
#define MIN_Y_A						-1019		// Min. Y-axis value
#define MAX_Z_A						1062		// Max. Z-axis value
#define MIN_Z_A						-1056		// Min. Z-axis value
#define OFFSET_PITCH_A				0.0			// Offset Pitch [°]
#define OFFSET_ROLL_A				0.0			// Offset Roll [°]

// LSM303DLHC magnetometer:
#define I2C_SENSOR_LSM303DLHC_M_ADR		0x3c	// Default

// LSM303DLHC magnetometer calibration data:
#define MAX_X_M						-111.0		// Max. X-axis value
#define MIN_X_M						-634.0		// Min. X-axis value
#define OFFSET_X_M					0			// Hard iron X-axis offset
#define MAX_Y_M						297.0		// Max. Y-axis value
#define MIN_Y_M						-215.0		// Min. Y-axis value
#define OFFSET_Y_M					0			// Hard iron Y-axis offset
#define MAX_Z_M						183.0		// Max. Z-axis value
#define MIN_Z_M						-214.0		// Min. Z-axis value
#define OFFSET_Z_M					0			// Hard iron Z-axis offset
#define DECLINATION_M				0.0			// E at local position [°]

// LSM303DLHC temperature sensor definitions:
#define GET_TEMP_M								// Enable temperature sensor
#define OFFSET_TEMP_M				0.0			// Temperature offset [°C]

/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):

// LY330ALH gyroscope:
#define ADC_SENSOR_LY330ALH				ADC_1	// At USRBUS: 2 (RP6BASE)

// LY330ALH gyroscope calibration data:
// (Higher values [505..1023] mean an angular velocity while rotating LEFT
//  and lower values [503..0] while rotating RIGHT!)
#define ADCVAL_MINRATE					0		// Min. rate value
#define ADCVAL_ZERORATE_LEVEL			504		// Zero rate level
#define ADCVAL_MAXRATE					1023	// Max. rate value

/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):

// Define USE_PD5, if you want to control the TCA4311A enable pin (EN) with
// the RP6 Sensor Board function "PD5". If USE_PD5 is NOT defined (default),
// the TCA4311A EN pin is controlled by the MAX7311 I/O 1.0 pin.
// ---------------
//#define USE_PD5
// ---------------

#define IO_SENSOR_TCA4311A_EN_IN		OC1B_PI6 // At I/O: 9 (RP6M256)
#define IO_SENSOR_TCA4311A_EN_DDR		DDRB
#define IO_SENSOR_TCA4311A_EN_PIN		PINB
#define IO_SENSOR_TCA4311A_EN_PORT		PORTB

/*****************************************************************************/
// Button:

// (Button is connected to I/O 1.7 of the MAX7311!)

/*****************************************************************************/
// Status LED:

// (Status LED is connected to I/O 1.6 of the MAX7311!)

/*****************************************************************************/
// Other ADC channel definitions:

// (Depending on connections on the RP6 Sensor Board!)
#define ADC_SENSOR_SHARP_F				ADC_0	// At USRBUS: 1 (RP6BASE)
#define ADC_SENSOR_SHARP_R				ADC_9	// At ADC: 3    (RP6M256)

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Header

Datei: RP6M256_Sensor_BoardLib.h

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 Sensor Board Library
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Header file for new RP6 Sensor Board library.
 *
 * ****************************************************************************
 */

#ifndef RP6M256_SENSOR_BOARDLIB_H
#define RP6M256_SENSOR_BOARDLIB_H


/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6M256_Sensor_Board.h"
#include <math.h>

/*****************************************************************************/
// RP6M256_I2CMasterLib definitions:

#ifndef I2C_RP6_BASE_ADR
	#define I2C_RP6_BASE_ADR			10
#endif
#ifndef I2C_REG_ADC_ADC0_L
	#define I2C_REG_ADC_ADC0_L			23
#endif
#ifndef I2C_REG_ADC_ADC1_L
	#define I2C_REG_ADC_ADC1_L			25
#endif
#ifndef CMD_POWER_OFF
	#define CMD_POWER_OFF				0
#endif
#ifndef CMD_POWER_ON
	#define CMD_POWER_ON				1
#endif

/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):

// Registers:
#define MAX7311_INPUT_PORT1				0
#define MAX7311_INPUT_PORT2				1
#define MAX7311_OUTPUT_PORT1			2
#define MAX7311_OUTPUT_PORT2			3
#define MAX7311_PORT1_POL_INV			4
#define MAX7311_PORT2_POL_INV			5
#define MAX7311_PORT1_CONFIG			6
#define MAX7311_PORT2_CONFIG			7
#define MAX7311_TIMEOUT					8

// Port 1 configuration register bitmasks:
#define ST_GYRO							(1 << PIN7)	// I/O 0.7 (Output)
#define PD_GYRO							(1 << PIN6)	// I/O 0.6 (Output)
#define PWR_CAM_IF						(1 << PIN5)	// I/O 0.5 (Output)
#define EN_LEDDRIV						(1 << PIN4)	// I/O 0.4 (Output)
#define RDY_BUSBUFF						(1 << PIN3)	// I/O 0.3 (Input)
#define PWR_SHARP_R						(1 << PIN2)	// I/O 0.2 (Output)
#define PWR_SHARP_F						(1 << PIN1)	// I/O 0.1 (Output)
#define PWR_I2C							(1 << PIN0)	// I/O 0.0 (Output)

// Port 2 configuration register bitmasks:
#define IO2_I2C							(1 << PIN7)	// I/O 1.7 (Input)
#define IO1_I2C							(1 << PIN6)	// I/O 1.6 (Output)
#define DRDY_MAGACC						(1 << PIN5)	// I/O 1.5 (Input)
#define PWR_SERVO						(1 << PIN4)	// I/O 1.4 (Output)
#define OEN_SERVO						(1 << PIN3)	// I/O 1.3 (Output)
#define IO_K19_1						(1 << PIN2)	// I/O 1.2 (Input)
#define IO_K19_3						(1 << PIN1)	// I/O 1.1 (Input)
#define EN_BUSBUFF						(1 << PIN0)	// I/O 1.0 (Output)

// Port 1 & 2 register 16-bit bitmasks:
// (CONFIG: 1 = Input, 0 = Output)
#define MAX7311_CONFIG_DEFAULT			0b1010011000001000
#define MAX7311_CONFIG_USE_PD5			0b1010011100001000
// (OUTPUT: 1 = High/True, 0 = Low/False)
#define MAX7311_OUTPUT_DEFAULT			0b0100000100010000
#define MAX7311_OUTPUT_FULLPOWER		0b0001000100110111
#define MAX7311_OUTPUT_SHUTDOWN			0b0100100011000000

// Safe configuration:
// (Ensures if activated, that I/Os 0.3, 1.5 and 1.7 cannot be configured
//  as output with the function MAX7311_write_cfg(). This may protect the
//  RP6 Sensor Board from hardware damage.)
// -------------------
#define SAFE_CONFIG
// -------------------

typedef union {
 	uint8_t byte;
	struct {
		unsigned int1   :1;				// INT1  XBUS Pin 8
		unsigned int2   :1;				// INT2  XBUS Pin 11
		unsigned int3   :1;				// INT3  XBUS Pin 9
		unsigned intu   :1;				// INTU  XBUS Pin 7
		unsigned unused :4;
	};
	struct {
		unsigned magacc1     :1;		// INT1: LSM303DLHC INT1
		unsigned portexp     :1;		// INT2: MAX7311 INT_N
		unsigned magacc2_rtc :1;		// INT3: LSM303DLHC INT2
		unsigned noname      :5;		//       OR DS1339 SQW/INT_N
	};
} interrupt_RP6SBstatus_t;
extern interrupt_RP6SBstatus_t interrupt_RP6SBstatus;

typedef union {
	uint16_t word;
	struct {
		unsigned pwr_i2c     :1;		// I/O 0.0 (Output)
		unsigned pwr_sharp_f :1;		// I/O 0.1 (Output)
		unsigned pwr_sharp_r :1;		// I/O 0.2 (Output)
		unsigned rdy_busbuff :1;		// I/O 0.3 (Input)
		unsigned en_leddriv  :1;		// I/O 0.4 (Output)
		unsigned pwr_cam_if  :1;		// I/O 0.5 (Output)
		unsigned pd_gyro     :1;		// I/O 0.6 (Output)
		unsigned st_gyro     :1;		// I/O 0.7 (Output)
		unsigned en_busbuff  :1;		// I/O 1.0 (Output)
		unsigned io_k19_3    :1;		// I/O 1.1 (Input)
		unsigned io_k19_1    :1;		// I/O 1.2 (Input)
		unsigned oen_servo   :1;		// I/O 1.3 (Output)
		unsigned pwr_servo   :1;		// I/O 1.4 (Output)
		unsigned drdy_magacc :1;		// I/O 1.5 (Input)
		unsigned io1_i2c     :1;		// I/O 1.6 (Output)
		unsigned io2_i2c     :1;		// I/O 1.7 (Input)
	};
} max7311IOs_t;
extern max7311IOs_t inport;
extern max7311IOs_t outport;
extern uint8_t readyBusbuff;
extern uint8_t pressedButton;

void MAX7311_write_cfg(uint16_t);
void task_checkINTs(void);
void task_RP6SensorBoard(void);
void MAX7311_update(void);
void MAX7311_write(uint16_t);
void MAX7311_init(void);

/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):

// Registers:
#define DS1339_SECONDS					0
#define DS1339_MINUTES					1
#define DS1339_HOURS					2
#define DS1339_DAY						3
#define DS1339_DATE						4
#define DS1339_MONTH_CENTURY			5
#define DS1339_YEAR						6
#define DS1339_ALARM1_SECONDS			7
#define DS1339_ALARM1_MINUTES			8
#define DS1339_ALARM1_HOURS				9
#define DS1339_ALARM1_DAY_DATE			10
#define DS1339_ALARM2_MINUTES			11
#define DS1339_ALARM2_HOURS				12
#define DS1339_ALARM2_DAY_DATE			13
#define DS1339_CONTROL					14
#define DS1339_STATUS					15
#define DS1339_TRICKLE_CHARGER			16

// Control register bitmasks:
#define DS1339_CONTROL_DEFAULT			0		// Power on default
#define DS1339_CONTROL_A1IE				1
#define DS1339_CONTROL_A2IE				2
#define DS1339_CONTROL_INTCN			4
#define DS1339_CONTROL_RS1				8
#define DS1339_CONTROL_RS2				16
#define DS1339_CONTROL_BBSQI			32
#define DS1339_CONTROL_EOSC				128

// Trickle Charger register bitmasks:
#define DS1339_TRICKLE_CHARGER_DEFAULT	0b10101010 // One diode, 2k resistor
#define DS1339_TRICKLE_CHARGER_ROUT0	1
#define DS1339_TRICKLE_CHARGER_ROUT1	2
#define DS1339_TRICKLE_CHARGER_DS0		4
#define DS1339_TRICKLE_CHARGER_DS1		8
#define DS1339_TRICKLE_CHARGER_TCS0		16
#define DS1339_TRICKLE_CHARGER_TCS1		32
#define DS1339_TRICKLE_CHARGER_TCS2		64
#define DS1339_TRICKLE_CHARGER_TCS3		128

enum RTCWEEKDAYS {
	R_MO = 1, R_TU, R_WE, R_TH, R_FR, R_SA, R_SU
};

typedef struct {
	uint16_t         year;				// Year
	uint8_t          month;				// Month   [1..12]
	enum RTCWEEKDAYS weekday;			// Weekday [1..7 = R_MO..R_SU]
	uint8_t          day;				// Day     [1..31]
} rtcdate_t;
rtcdate_t rtc_date;

typedef struct {
	uint8_t dst;						// Daylight-saving-time (time zone)
	uint8_t hour;						// Hour    [0..23]
	uint8_t minute;						// Minute  [0..59]
	uint8_t second;						// Second  [0..59]
} rtctime_t;
rtctime_t rtc_time;

uint8_t BCD2DEC(uint8_t);
uint8_t DEC2BCD(uint8_t);
void DS1339_write_cfg(uint8_t);
void DS1339_init(void);
#define CALC_DST						// Time zone will be calculated
void DS1339_read(void);
void DS1339_write(void);

/*****************************************************************************/
// I2C Temperature Sensor (LM75B):

// Registers:
#define LM75B_TEMP						0
#define LM75B_CONF						1
#define LM75B_THYST						2
#define LM75B_TOS						3

// Conf register bitmasks:
#define LM75B_CONF_DEFAULT				0		// Power on default
#define LM75B_CONF_SHUTDOWN				1
#define LM75B_CONF_OS_COMP_INT			2
#define LM75B_CONF_OS_POL				4
#define LM75B_CONF_OS_F_QUE0			8
#define LM75B_CONF_OS_F_QUE1			16

extern double temperature;

void LM75B_write_cfg(uint8_t);
#define LM75B_shutdown() {LM75B_write_cfg(LM75B_CONF_SHUTDOWN);}
#define LM75B_init() {LM75B_write_cfg(LM75B_CONF_DEFAULT);}
extern uint8_t temperature_low;
extern uint8_t temperature_high;
#define getTemperatureHigh() (temperature_high)
#define getTemperatureLow() (temperature_low)
void LM75B_read(void);
double LM75B_calculate(void);
double LM75B_measure(void);

/*****************************************************************************/
// I2C Servo Controller (PCA9685):

// Registers:
#define PCA9685_MODE1					0
#define PCA9685_MODE2					1
#define PCA9685_SUBADR1					2
#define PCA9685_SUBADR2					3
#define PCA9685_SUBADR3					4
#define PCA9685_ALLCALLADR				5
#define PCA9685_LED0_ON_L				6
#define PCA9685_LED0_ON_H				7
#define PCA9685_LED0_OFF_L				8
#define PCA9685_LED0_OFF_H				9
#define PCA9685_LED1_ON_L				10
#define PCA9685_LED1_ON_H				11
#define PCA9685_LED1_OFF_L				12
#define PCA9685_LED1_OFF_H				13
#define PCA9685_LED2_ON_L				14
#define PCA9685_LED2_ON_H				15
#define PCA9685_LED2_OFF_L				16
#define PCA9685_LED2_OFF_H				17
#define PCA9685_LED3_ON_L				18
#define PCA9685_LED3_ON_H				19
#define PCA9685_LED3_OFF_L				20
#define PCA9685_LED3_OFF_H				21
#define PCA9685_LED4_ON_L				22
#define PCA9685_LED4_ON_H				23
#define PCA9685_LED4_OFF_L				24
#define PCA9685_LED4_OFF_H				25
#define PCA9685_LED5_ON_L				26
#define PCA9685_LED5_ON_H				27
#define PCA9685_LED5_OFF_L				28
#define PCA9685_LED5_OFF_H				29
#define PCA9685_LED6_ON_L				30
#define PCA9685_LED6_ON_H				31
#define PCA9685_LED6_OFF_L				32
#define PCA9685_LED6_OFF_H				33
#define PCA9685_LED7_ON_L				34
#define PCA9685_LED7_ON_H				35
#define PCA9685_LED7_OFF_L				36
#define PCA9685_LED7_OFF_H				37
#define PCA9685_LED8_ON_L				38
#define PCA9685_LED8_ON_H				39
#define PCA9685_LED8_OFF_L				40
#define PCA9685_LED8_OFF_H				41
#define PCA9685_LED9_ON_L				42
#define PCA9685_LED9_ON_H				43
#define PCA9685_LED9_OFF_L				44
#define PCA9685_LED9_OFF_H				45
#define PCA9685_LED10_ON_L				46
#define PCA9685_LED10_ON_H				47
#define PCA9685_LED10_OFF_L				48
#define PCA9685_LED10_OFF_H				49
#define PCA9685_LED11_ON_L				50
#define PCA9685_LED11_ON_H				51
#define PCA9685_LED11_OFF_L				52
#define PCA9685_LED11_OFF_H				53
#define PCA9685_LED12_ON_L				54
#define PCA9685_LED12_ON_H				55
#define PCA9685_LED12_OFF_L				56
#define PCA9685_LED12_OFF_H				57
#define PCA9685_LED13_ON_L				58
#define PCA9685_LED13_ON_H				59
#define PCA9685_LED13_OFF_L				60
#define PCA9685_LED13_OFF_H				61
#define PCA9685_LED14_ON_L				62
#define PCA9685_LED14_ON_H				63
#define PCA9685_LED14_OFF_L				64
#define PCA9685_LED14_OFF_H				65
#define PCA9685_LED15_ON_L				66
#define PCA9685_LED15_ON_H				67
#define PCA9685_LED15_OFF_L				68
#define PCA9685_LED15_OFF_H				69
#define PCA9685_ALL_LED_ON_L			250
#define PCA9685_ALL_LED_ON_H			251
#define PCA9685_ALL_LED_OFF_L			252
#define PCA9685_ALL_LED_OFF_H			253
#define PCA9685_PRE_SCALE				254
#define PCA9685_TESTMODE				255

// Mode1 register bitmasks:
#define PCA9685_MODE1_ALLCALL			1
#define PCA9685_MODE1_SUB3				2
#define PCA9685_MODE1_SUB2				4
#define PCA9685_MODE1_SUB1				8
#define PCA9685_MODE1_SLEEP				16
#define PCA9685_MODE1_AI				32
#define PCA9685_MODE1_EXTCLK			64
#define PCA9685_MODE1_RESTART			128

// Mode2 register bitmasks:
#define PCA9685_MODE2_OUTNE01_DEFAULT	0
#define PCA9685_MODE2_OUTDRV			4
#define PCA9685_MODE2_OCH				8
#define PCA9685_MODE2_INVRT				16

#define F_PCA9685						25000000.0	// Int. Clock: 25 MHz

void PCA9685_init(uint16_t);
#define initServo(__FREQ__) {PCA9685_init(__FREQ__);}
uint8_t servo(uint8_t);
void PCA9685_set(uint8_t, uint16_t);
#define setServo(__SERVO__,__POS__) {PCA9685_set(servo(__SERVO__),__POS__);}
void PCA9685_shutdown(void);
void PCA9685_restart(void);
void setServoPower(uint8_t);

/*****************************************************************************/
// I2C LED Driver (TCA6507):

// Registers:
#define TCA6507_SELECT0				0
#define TCA6507_SELECT1				1
#define TCA6507_SELECT2				2
#define TCA6507_FADEON_TIME			3
#define TCA6507_FULLYON_TIME		4
#define TCA6507_FADEOFF_TIME		5
#define TCA6507_1ST_FULLYOFF_TIME	6
#define TCA6507_2ND_FULLYOFF_TIME	7
#define TCA6507_MAXIMUM_INTENSITY	8
#define TCA6507_MASTER_INTENSITY	9
#define TCA6507_INITIALIZATION		10

// Output states:
#define STATE_OFF					0
#define STATE_PWM0					2
#define STATE_PWM1					3
#define STATE_ON					4
#define STATE_MINT					5
#define STATE_BLINK0				6
#define STATE_BLINK1				7

extern uint8_t select[3];

void TCA6507_write_cfg(uint8_t, uint8_t);
void TCA6507_update(void);
void TCA6507_set(uint8_t, uint8_t);
#define setLED(__LED__,__STATE__) {TCA6507_set(__LED__,__STATE__);}
void TCA6507_shutdown(void);
void TCA6507_restart(void);
void TCA6507_init(void);

/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):

// LSM303DLHC accelerometer internal registers:
#define CTRL_REG1_A					0x20
#define CTRL_REG2_A					0x21
#define CTRL_REG3_A					0x22
#define CTRL_REG4_A					0x23
#define CTRL_REG5_A					0x24
#define CTRL_REG6_A					0x25
#define REFERENCE_A					0x26
#define STATUS_REG_A				0x27
#define OUT_X_L_A					0x28
#define OUT_X_H_A					0x29
#define OUT_Y_L_A					0x2a
#define OUT_Y_H_A					0x2b
#define OUT_Z_L_A					0x2c
#define OUT_Z_H_A					0x2d
#define FIFO_CTRL_REG_A				0x2e
#define FIFO_SRC_REG_A				0x2f
#define INT1_CFG_A					0x30
#define INT1_SOURCE_A				0x31
#define INT1_THS_A					0x32
#define INT1_DURATION_A				0x33
#define INT2_CFG_A					0x34
#define INT2_SOURCE_A				0x35
#define INT2_THS_A					0x36
#define INT2_DURATION_A				0x37
#define CLICK_CFG_A					0x38
#define CLICK_SRC_A					0x39
#define CLICK_THS_A					0x3a
#define TIME_LIMIT_A				0x3b
#define TIME_LATENCY_A				0x3c
#define TIME_WINDOW_A				0x3d

// LSM303DLHC accelerometer register settings:
#define CTRL_REG1_A_10HZ			0b00100111	// 10Hz & all axes enable
#define CTRL_REG1_A_50HZ			0b01000111	// 50Hz & all axes enable
#define CTRL_REG4_A_DEFAULT			0b00000000	// Default (Normal mode)
#define CTRL_REG4_A_HR				0b00001000	// HR output mode

// LSM303DLHC magnetometer internal registers:
#define CRA_REG_M					0x00
#define CRB_REG_M					0x01
#define MR_REG_M					0x02
#define OUT_X_H_M					0x03
#define OUT_X_L_M					0x04
#define OUT_Z_H_M					0x05
#define OUT_Z_L_M					0x06
#define OUT_Y_H_M					0x07
#define OUT_Y_L_M					0x08
#define SR_REG_MG					0x09
#define IRA_REG_M					0x0a
#define IRB_REG_M					0x0b
#define IRC_REG_M					0x0c
#define TEMP_OUT_H_M				0x31
#define TEMP_OUT_L_M				0x32

// LSM303DLHC magnetometer register settings:
#define CRA_REG_M_15HZ				0b00010000	// Default (Data rate 15Hz)
#define CRA_REG_M_15HZ_T_EN			0b10010000	// 15Hz & temperature enable
#define CRB_REG_M_13GAUSS			0b00100000	// Gain +-1.3Gauss
#define MR_REG_M_CCM				0b00000000	// Continuous-conversion mode
#define MR_REG_M_SCM				0b00000001	// Single-conversion mode
#define MR_REG_M_SLEEP				0b00000011	// Default (Sleep-mode)

extern int16_t x_axisa, y_axisa, z_axisa;
extern double pitch, roll;
extern double xa, ya, za;
extern double the, phi;

extern int16_t x_axism, y_axism, z_axism;
extern int16_t headingm;
extern double xm, ym, zm;
#ifdef GET_TEMP_M
extern int16_t temperaturem;
extern double temperature_mag;
#endif

extern int16_t headingtc;

void LSM303DLHC_A_init(void);
void readLSM303DLHC_A(void);
void normalizeLSM303DLHC_A(void);
void positionLSM303DLHC_A(void);

void LSM303DLHC_M_init(void);
void readLSM303DLHC_M(void);
void normalizeLSM303DLHC_M(void);
int16_t headingLSM303DLHC_M(void);

int16_t headingLSM303DLHC_TC(void);

void LSM303DLHC_init(void);

/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):

#define LY330ALH_MODE_NORMAL		0			// Power on default
#define LY330ALH_MODE_SLEEP			1
#define LY330ALH_MODE_SELFTEST		2
#define LY330ALH_MODE_POWERDOWN		3			// Shutdown mode

extern int16_t z_axisg;

void LY330ALH_set_mode(uint8_t);
uint16_t getRP6Base_ADC1(void);
int16_t LY330ALH_measure(void);

/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):

//extern uint8_t readyBusbuff;			// See "I2C I/O Port Expander" section
extern uint8_t errorBusbuff;

#define getReadyBusbuff() (readyBusbuff)
uint8_t readReadyBusbuff(void);
void TCA4311A_shutdown(void);
void TCA4311A_reconnect(void);
void setRP6Base_PWR(uint8_t);
void setI2CModulePower(uint8_t);

/*****************************************************************************/
// Button:

//extern uint8_t pressedButton;			// See "I2C I/O Port Expander" section

#define getPressedButton() (pressedButton)
uint8_t readButton(void);

/*****************************************************************************/
// Status LED:

void setStatusLED(uint8_t);

/*****************************************************************************/
// RP6 Sensor Board initialisation and shutdown:

void sensor_board_init(void);
void sensor_board_shutdown(void);

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Source

Demo

RP6 CCPRO M128

RP6 CONTROL M32

Projekte

RP6 Xtra Module

Die Dokumentation zu den RP6 Xtra Modulen befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM". Auf der CD-ROM sind auch Software-Beispiele zu finden.

I2C GPS Empfänger JM3-GPS

Gyro Modul JM3-GYRO

Analoge Auswertung

Am Pin OUT des Gyro Moduls (siehe Anschluss-Schema hier!) liegt die Ausgangsspannung des Sensors (0..5V) an. Sie kann mit einem ADC-Eingang eines Microcontrollers gemessen werden. Syntax mit den Libraries der RP6Base, M32 oder M256 WiFi (ADC Kanal 0):

uint16_t result = readADC(0);

In der C-Control-Pro Umgebung der M128 sieht das z.B. in CompactC so aus (ADC Kanal 0):

word result;
ADC_Set(ADC_VREF_VCC, 0);
...
result = ADC_Read();

Das Ergebnis result kann noch zur Weiterverarbeitung angepasst werden:

#define ADCVAL_ZERORATE_LEVEL			511		// Zero rate Level
...
int16_t z_axisg = result - ADCVAL_ZERORATE_LEVEL;

Dadurch zeigt z_axisg Null an, wenn keine Rotation anliegt und wird positiv, wenn nach LINKS gedreht wird und negativ bei RECHTS-Drehung. Die Definition ADCVAL_ZERORATE_LEVEL muss natürlich an den eigenen Sensor-Nullwert angepasst werden.

Optionale Ansteuerung

Im Anschluss-Schema hier ist erkennbar, dass es noch 2 Steuerleitungen SLEEP/PD und ST gibt. Mit diesen Anschlüssen kann die Betriebsart des Gyro Moduls festgelegt werden, siehe nachfolgende Tabelle:

Betriebsart	ST	SLEEP/PD	Stromverbrauch
Normal	0	0	4,2mA
Sleep	0	1	2,2mA
Self-test	1	0	4,2mA
Power-down	1	1	5µA

Will man zwischen den Betriebsarten per Software umschalten, muss man die beiden Steuerleitungen mit I/O Portpins des Microcontrollers verbinden, die als Ausgänge konfiguriert sind. Braucht man diese Funktion nicht, reicht es, ST und SLEEP/PD mit GND zu verbinden (Betriebsart "Normal").

3D Accelerometer Modul JM3-3DA

Das 3D Accelerometer Modul kann über den I2C- oder den SPI-Bus angesteuert werden. Wird das Modul auf der RP6 CONTROL M32 Platine betrieben (es sitzt also auf dem Sockel IC5 für das 2. SPI-EEPROM), dann erfolgt die Ansteuerung über den SPI-Bus. Die SPI-Grundfunktionen befinden sich in der RP6ControlLib, die daher eingebunden werden muss.

RP6 CONTROL M32: ACCSENS Library

Die nachfolgende RP6ControlACCSENSLib (GCC) dient zur Ansteuerung des 3D Accelerometer Moduls durch die RP6 CONTROL M32.

Configuration Header

Vor dem Kompilieren der Library muss eine Anpassung des Configuration Headers an die eigenen Hardware-Voraussetzungen erfolgen. Dazu gibt es im Configuration Header eine Definition:

• EXP_STACK_1 -> Die RP6 CONTROL M32 sitzt auf dem vorderen Platinenstapel 1 des RP6

Im nachfolgenden Listing ist die Definition EXP_STACK_1 NICHT aktiv (d.h. die M32 sitzt auf dem hinteren Platinenstapel 2!). Dies muss an die eigene Hardware angepaßt werden. Wenn die RP6 CONTROL M32 auf dem vorderen Platinenstapel 1 des RP6 sitzt, muss die Zeile mit "EXP_STACK_1" "aktiviert" werden. Dazu entfernt man "//" am Zeilenanfang:

#define EXP_STACK_1

Bevor man den Beschleunigungssensor nutzen kann, muss man ihn kalibrieren:

Accelerometer Kalibrierung:

• Die X-Achse wird kalibriert, indem man den RP6 mit der rechten Seite nach unten hält und den Maximalwert in MAX_X_A einträgt. Dann neigt man ihn auf seine linke Seite und trägt den negativen Maximalwert in MIN_X_A ein.
• Die Y-Achse wird kalibriert, indem man den RP6 mit der Vorderseite nach oben hält und den Maximalwert in MAX_Y_A einträgt. Dann kippt man die Vorderseite nach unten und trägt den negativen Maximalwert in MIN_Y_A ein.
• Die Z-Achse wird kalibriert, indem man den RP6 normal waagerecht hält und den Maximalwert in MAX_Z_A einträgt. Dann dreht man den RP6 "auf den Kopf" und trägt den negativen Maximalwert in MIN_Z_A ein.
• Sollten die angezeigten Pitch- und Roll-Werte nach der Kalibrierung der Achsen und genau waagerecht stehendem Sensor bzw. RP6 vom Nullwert deutlich abweichen, kann man sie mithilfe der Definitionen OFFSET_PITCH_A und OFFSET_ROLL_A "nullen". Die Einheit dieser Definitionen ist °.

Nach dem Eintragen der Kalibrierungswerte muss das Programm neu kompiliert werden!

Datei: RP6ControlACCSENS.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENS.h
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Configuration header file for new ACCSENS library.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

#ifndef RP6CONTROLACCSENS_H
#define RP6CONTROLACCSENS_H


/*****************************************************************************/
// Sensor position on the RP6 robot:
// (If EXP_STACK_1 is defined, the RP6 CONTROL M32 is located on the front
//  stack for addon boards on the RP6 robot. If EXP_STACK_1 is NOT defined
//  (default), the RP6 CONTROL M32 is mounted on the rear stack.)
// -------------------
//#define EXP_STACK_1
// -------------------

/*****************************************************************************/
// The following sensor is used:
// - 3D Accelerometer Modul (CONRAD 1082387-62) with LIS302DLH 3-axis
//   accelerometer

/*****************************************************************************/
// LIS302DLH accelerometer calibration data:
#define MAX_X_A						32767		// Max. X-axis value
#define MIN_X_A						-32768		// Min. X-axis value
#define MAX_Y_A						32767		// Max. Y-axis value
#define MIN_Y_A						-32768		// Min. Y-axis value
#define MAX_Z_A						32767		// Max. Z-axis value
#define MIN_Z_A						-32768		// Min. Z-axis value
#define OFFSET_PITCH_A				0.0			// Offset Pitch [°]
#define OFFSET_ROLL_A				0.0			// Offset Roll [°]

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6ControlACCSENSLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Header

Datei: RP6ControlACCSENSLib.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENSLib.h
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is the RP6ControlACCSENSLib header file.
 * You have to include this file, if you want to use the library
 * RP6ControlACCSENSLib.c in your own projects.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

#ifndef RP6CONTROLACCSENSLIB_H
#define RP6CONTROLACCSENSLIB_H


/*****************************************************************************/
// Includes:

#include "RP6ControlLib.h" 		// The RP6 Control Library (v. 1.1 or higher). 
#include "RP6ControlACCSENS.h"
#include <math.h>

/*****************************************************************************/

#define ACC_CS							MEM_CS2
#define DEVICE_IDENTIFIER				0x32

// Registers:
#define WHO_AM_I						0x0f
#define CTRL_REG1						0x20
#define CTRL_REG2						0x21
#define CTRL_REG3						0x22
#define CTRL_REG4						0x23
#define CTRL_REG5						0x24
#define HP_FILTER_RESET					0x25
#define REFERENCE						0x26
#define STATUS_REG						0x27
#define OUT_X_L							0x28
#define OUT_X_H							0x29
#define OUT_Y_L							0x2a
#define OUT_Y_H							0x2b
#define OUT_Z_L							0x2c
#define OUT_Z_H							0x2d
#define INT1_CFG						0x30
#define INT1_SOURCE						0x31
#define INT1_THS						0x32
#define INT1_DURATION					0x33
#define INT2_CFG						0x34
#define INT2_SOURCE						0x35
#define INT2_THS						0x36
#define INT2_DURATION					0x37

// Control Register bitmasks:
#define CTRL_REG1_POWERDOWN				0b00000000
#define CTRL_REG1_3D_DEFAULT			0b00000111
#define CTRL_REG1_3D_LOPWR_10HZ			0b11000111
#define CTRL_REG2_DEFAULT				0b00000000
#define CTRL_REG3_DEFAULT				0b00000000
#define CTRL_REG4_DEFAULT				0b00000000
#define CTRL_REG4_BDU_2G				0b10000000
#define CTRL_REG4_BDU_2G_STSIGN			0b10001000
#define CTRL_REG4_BDU_BLE_2G			0b11000000
#define CTRL_REG4_BDU_BLE_2G_STSIGN		0b11001000
#define CTRL_REG5_DEFAULT				0b00000000

// Read/write address bitmasks:
#define READ_MASK						0b10000000
#define READ_AUTOINC_MASK				0b11000000
#define WRITE_MASK						0b01111111

extern int16_t x_axisa, y_axisa, z_axisa;
extern double pitch, roll;
extern double xa, ya, za;
extern double the, phi;

uint8_t SPI_ACCSENS_readByte(uint8_t regAddr);
void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length);
void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data);
uint8_t SPI_ACCSENS_getStatus(void);

void SPI_ACCSENS_init(void);
void SPI_ACCSENS_shutdown(void);
void SPI_ACCSENS_read(void);
void SPI_ACCSENS_normalize(void);
void SPI_ACCSENS_position(void);

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6ControlACCSENSLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Source

Datei: RP6ControlACCSENSLib.c

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENSLib.c
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 *         with optional LC-Display 16x2 chars (CONRAD 190911-62)
 *         and a 3D Accelerometer Modul (CONRAD 1082387-62) (IC5)
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is my RP6 Control ACCSENS Library. This library may be used to access
 * the basic functions of the 3D Accelerometer Modul. This RP6 Xtra module may
 * be placed on the IC5 socket on the RP6 CONTROL M32 instead of a second
 * EEPROM.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */
 
/*****************************************************************************/
// Includes:

#include "RP6ControlACCSENSLib.h"

/*****************************************************************************/
// SPI functions:

/**
 * Reads a single register from the 3D Accelerometer
 * Modul (ACCSENS).
 *
 */
uint8_t SPI_ACCSENS_readByte(uint8_t regAddr)
{
	uint8_t data;
	regAddr |= READ_MASK;						// Read byte
	PORTB &= ~ACC_CS;
	writeSPI(regAddr);
	data = readSPI();
	PORTB |= ACC_CS;
	return data;
}

/**
 * Reads "length" bytes into the buffer "buffer"
 * from startAddr on. 
 *
 */
void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length)
{
	startAddr |= READ_AUTOINC_MASK;				// Read byte, auto increment
	PORTB &= ~ACC_CS;
	writeSPI(startAddr);
	readBufferSPI(&buffer[0], length);
	PORTB |= ACC_CS;
}

/**
 * Write a single data byte to the specified sensor
 * address.
 *
 */
void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data)
{
	PORTB &= ~ACC_CS;
	writeSPI(regAddr);
	writeSPI(data);
	PORTB |= ACC_CS;
}

/**
 * Returns sensor status register - for checking if
 * sensor is busy. 
 *
 */
uint8_t SPI_ACCSENS_getStatus(void)
{
	uint8_t status;
	PORTB &= ~ACC_CS;
	writeSPI(STATUS_REG);
	status = readSPI();
	PORTB |= ACC_CS;
	return status;
}

/*****************************************************************************/
// 3D accelerometer functions:

int16_t x_axisa, y_axisa, z_axisa;
double pitch, roll;
double xa, ya, za;
double the, phi;

/**
 * Initializes the sensor. You must call this
 * function at the beginning of a program, that
 * uses the 3D Accelerometer Modul.
 *
 */
void SPI_ACCSENS_init(void)
{
	// All axes enable, low power, 10Hz output data rate:
	SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_3D_LOPWR_10HZ);
	// No update while reading, +- 2g, self-test minus:
	SPI_ACCSENS_writeByte(CTRL_REG4, CTRL_REG4_BDU_2G_STSIGN);
}

/**
 * Sets the LIS302DLH power-down mode. If you
 * do not need the sensor, you can save energy
 * by using the sensor power-down mode.
 *
 */
void SPI_ACCSENS_shutdown(void)
{
	// LIS302DLH in power-down mode:
	SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_POWERDOWN);
}

/**
 * This function reads the X-axis, Y-axis and Z-axis
 * values from the LIS302DLH accelerometer and stores
 * them in the global variables x_axisa, y_axisa and
 * z_axisa.
 *
 */
void SPI_ACCSENS_read(void)
{
#ifndef EXP_STACK_1
	// Default sensor position (M32 located on the RP6 rear stack):
	// xb = -y:
	x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
	x_axisa *= -1;
	// yb = -x:
	y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
	y_axisa *= -1;
	// zb = z:
	z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#else
	// Other sensor position (M32 located on the RP6 front stack):
	// xb = y:
	x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
	// yb = x:
	y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
	// zb = z:
	z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#endif
}

/**
 * This function limits and normalizes the x-axisa,
 * y-axisa and z-axisa values read from the LIS302DLH
 * accelerometer with the function SPI_ACCSENS_read() and
 * stores them in the global double variables xa, ya and
 * za.
 * It also calculates the position in space in form of the
 * tilt angles (the = pitch, phi = roll) and stores them
 * in the global double variables the and phi.
 *
 */
void SPI_ACCSENS_normalize(void)
{
	// Limit raw values:
	if (x_axisa < MIN_X_A) x_axisa = MIN_X_A;
	if (x_axisa > MAX_X_A) x_axisa = MAX_X_A;
	if (y_axisa < MIN_Y_A) y_axisa = MIN_Y_A;
	if (y_axisa > MAX_Y_A) y_axisa = MAX_Y_A;
	if (z_axisa < MIN_Z_A) z_axisa = MIN_Z_A;
	if (z_axisa > MAX_Z_A) z_axisa = MAX_Z_A;
	xa = (double) x_axisa / 10000.0;
	ya = (double) y_axisa / 10000.0;
	za = (double) z_axisa / 10000.0;
	// Calculate Pitch and Roll:
	phi = asin(xa);								// Calculate the and phi
	the = asin(ya / cos(the));					//  from the raw values
}

/**
 * This function calculates the position in space by
 * using the the and phi values (see function
 * SPI_ACCSENS_normalize()!). It stores the position
 * in the global double variables pitch and roll. 
 *
 */
void SPI_ACCSENS_position(void)
{
	pitch = the * 180 / M_PI + OFFSET_PITCH_A;
	roll = phi * 180 / M_PI + OFFSET_ROLL_A;
}

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 04.04.2014 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Erklärung

Demo

makefile:

...
TARGET = RP6Control_01_ACCSENS
...
SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlLib.c
SRC += $(RP6_LIB_PATH)/RP6common/RP6uart.c
SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlACCSENSLib.c
...

Datei: RP6Control_01_ACCSENS.c

// Uncommented Version of RP6Control_01_ACCSENS.c
// written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6ControlACCSENSLib.h"


void writeDouble(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeString(&buffer[0]);
}

void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeStringLCD(&buffer[0]);
}


int main(void)
{
	initRP6Control();

	writeString_P("\n\nRP6Control SPI ACCSENS Test Program!\n"); 
	setLEDs(0b1111);
	mSleep(50);
	initLCD(); 
	showScreenLCD("################", "################");

	showScreenLCD("  SPI ACCSENS", "  Test Program");
	mSleep(1000);
	setLEDs(0b0000);

	SPI_ACCSENS_init();

	uint8_t whoami = SPI_ACCSENS_readByte(WHO_AM_I);
	writeString_P("\nWHO AM I: ");
	writeInteger(whoami, HEX);
	writeChar('\n');
	mSleep(2000);
	
	startStopwatch1();

	while(true) 
	{
		if(getStopwatch1() > 1000)
		{
			SPI_ACCSENS_read();
			writeString_P("\n3D Accelerometer SENSOR ->\n");
			writeString_P("X-axis: ");
			writeInteger(x_axisa, DEC);
			writeChar('\n');
			writeString_P("Y-axis: ");
			writeInteger(y_axisa, DEC);
			writeChar('\n');
			writeString_P("Z-axis: ");
			writeInteger(z_axisa, DEC);
			writeChar('\n');
			SPI_ACCSENS_normalize();
			SPI_ACCSENS_position();
			writeString_P("POSITION: \n");
			writeString_P("  Pitch [°]: ");
			writeDouble(pitch, 6, 1);
			writeChar('\n');
			writeString_P("  Roll [°]:  ");
			writeDouble(roll, 6, 1);
			writeChar('\n');
			setCursorPosLCD(0, 0);
			writeStringLCD_P("P");
			writeDoubleLCD(pitch, 6, 1);
			writeStringLCD_P(" ");
			setCursorPosLCD(0, 8);
			writeStringLCD_P("R");
			writeDoubleLCD(roll, 6, 1);
			writeStringLCD_P(" ");
			setCursorPosLCD(1, 0);
			writeStringLCD_P("X");
			writeIntegerLCD(x_axisa, DEC);
			writeStringLCD_P("   ");
			setCursorPosLCD(1, 5);
			writeStringLCD_P("Y");
			writeIntegerLCD(y_axisa, DEC);
			writeStringLCD_P("   ");
			setCursorPosLCD(1, 10);
			writeStringLCD_P("Z");
			writeIntegerLCD(z_axisa, DEC);
			writeStringLCD_P("    ");
			mSleep(2000);
		}
	}
	return 0;
}

Erklärung

Erfahrungsberichte

...in Arbeit...(kann aber gerne ergänzt werden)

Siehe auch

• RP6v2
• RP6
• RP6 - Programmierung
• RP6 Kamera - Mitmach-Projekt
• RP6v2 I2C-Portexpander
• RP6v2 USB-RS232-Adapter
• RP6v2 Orientierung
• RP6 Multi IO Projekt
• RP6 Multi IO Projekt - Software
• CCRP5
• Yeti
• Asuro
• C't-Bot

Weblinks

Siehe auch die Weblinks im RP6v2 und im RP6 Artikel!

• RP6 Sensor Board bei CONRAD
• JM3 Engeneering: RP6 Roboter Erweiterung
• Ankündigung einer Sensorerweiterungsplatine für die RP6v2 M256 WiFi im Roboternetz RP6 Forum
• Erfahrungsbericht zur Sensor Board im AREXX Support Forum und im Roboternetz RP6 Forum
• Erfahrungsbericht zum I2C GPS Empfänger im AREXX Support Forum

Autoren

--Dirk 19:23, 13. Apr 2014 (CET)