Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_OrientationLib.h"
//#include "RP6ControlServoLib.h"
#define I2C_RP6_BASE_ADR 10
/************************variables*****************************************/
uint8_t ch;
char item[12];
char dir[3];
/*********************I2C-fehlermeldungen******************/
void I2C_transmissionError(uint8_t errorState) //gibt I2C fehlermeldungen über LCD aus
{
clearLCD();
writeStringLCD_P("I2C ERROR -->");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("TWI STATE: 0x");
writeIntegerLCD(errorState, HEX);
}
/**
* Returns a 2 character string for the eighth
* parts of the direction calculated from the
* heading value.
*
* Input: heading -> Heading value [0..359]
*
*/
void calculateDir(char *dir, uint16_t heading)
{
dir[1] = ' ';
dir[2] = '\0';
if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';}
if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';}
if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';}
if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';}
}
/************************Write a floating point number to the LCD.******/
/*
* Example:
*
* // Write a floating point number to the LCD (no exponent):
* writeDoubleLCD(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
void writeDouble(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString(&buffer[0]);
}
/*****************gyroscope***************/
void gyroscope(void) // L3GD20 gyroscope
{
orientation_init();
task_I2CTWI();
readL3GD20(); // Get sensor values
// normalizeL3GD20();
task_I2CTWI();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 8); // line 1 pos 9
writeStringLCD_P("Y");
writeIntegerLCD(y_axisg, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("Z");
writeIntegerLCD(z_axisg, DEC);
writeStringLCD_P(" ");
#ifdef GET_TEMP
temperatureg = calcTempL3GD20(temperatureg) + 5;
temperatureg += OFFSET_TEMP;
setCursorPosLCD(1, 8); // line 2 pos 9
writeStringLCD_P("T");
writeIntegerLCD(temperatureg, DEC);
writeStringLCD_P(" ");
#endif
task_I2CTWI();
mSleep(3000);//4,5 sec
clearLCD();
}
/************accelerometer****************/
void accelerometer(void) // LSM303DLHC accelerometer
{
orientation_init();
clearLCD();
task_I2CTWI();
readLSM303DLHC_A(); // Get sensor values
task_I2CTWI();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 5); // line 1 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(y_axisa, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 10); // line 1 pos 11
writeStringLCD_P("Z");
writeIntegerLCD(z_axisa, DEC);
writeStringLCD_P(" ");
normalizeLSM303DLHC_A(); // Normalize data
positionLSM303DLHC_A(); // Calculate position
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("P");
writeDoubleLCD(pitch, 6, 1);
writeStringLCD_P(" ");
setCursorPosLCD(1, 8); // line 2 pos 9
writeStringLCD_P("R");
writeDoubleLCD(roll, 6, 1);
writeStringLCD_P(" ");
task_I2CTWI();
mSleep(3000);
}
/*****************magnetometer************/
void magnetometer(void) // LSM303DLHC magnetometer
{
orientation_init();
clearLCD();
task_I2CTWI();
readLSM303DLHC_M(); // Get sensor values
task_I2CTWI();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axism, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 5); // line 1 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(y_axism, DEC);
writeStringLCD_P(" ");
setCursorPosLCD(0, 10); // line 1 pos 11
#ifndef GET_TEMP_M
writeStringLCD_P("Z");
writeIntegerLCD(z_axism, DEC);
writeStringLCD_P(" ");
#else
temperature_imu = (double) temperaturem / 8.0 + OFFSET_TEMP_M;
writeStringLCD_P("T");
writeDoubleLCD(temperature_imu, 5, 1);
#endif
// normalizeLSM303DLHC_M(); // Normalize data
headingm = headingLSM303DLHC_M(); // Calculate heading
calculateDir(dir, headingm);
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("H");
writeIntegerLengthLCD(headingm, DEC, 3);
writeStringLCD_P(" ");
writeStringLCD(dir);
headingtc = headingLSM303DLHC_TC(); // Calculate TILT COMPENSATED
calculateDir(dir, headingtc); // heading
writeStringLCD_P(" C");
writeIntegerLengthLCD(headingtc, DEC, 3);
writeStringLCD_P(" ");
writeStringLCD(dir);
writeStringLCD_P(" ");
mSleep(3000);//4,5 sec
clearLCD();
task_I2CTWI();
}
/*************** hauptprogramm ***********/
int main(void)
{
initRP6Control();
multiio_init();
initLCD();
//orientation_init();
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
I2CTWI_initMaster(100);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); //aktiviert I2C fehlermeldungen
// Voltage & current sensor test:
LTC2990_measure();
setCursorPosLCD(0, 0);
//writeStringLCD("BAT Current: ");
//writeDoubleLCD(cbat, 6, 1);
writeStringLCD(" accu: ");
writeDoubleLCD(vbat, 4, 1);
writeStringLCD( " V");
mSleep(1500);
showScreenLCD(" RP6Control M32", " gyro_test_3");
mSleep(2500);
clearLCD();
while(true)
{
/*****************anzeige gedrückter buttons****************/
clearLCD();
pressedMultiIOButtonNumber = getMultiIOPressedButtonNumber();
setCursorPosLCD(0, 0);
writeStringLCD("Button: ");
writeIntegerLCD(pressedMultiIOButtonNumber, DEC);
mSleep(500);
uint8_t key = getMultiIOPressedButtonNumber();
/********************funktion der buttons*********************/
if(key)
{
switch(key)
{
case 1://
setLEDs(0b0001);
gyroscope();
break;
case 2://
setLEDs(0b0010);
accelerometer();
break;
case 3://
setLEDs(0b0100);
magnetometer();
break;
case 4://
setLEDs(0b1000);
gyroscope();
accelerometer();
magnetometer();
break;
}
}
}
return 0;
}
folgendes fiel mir auf wofür ich noch nach einer erklärung suche:
Zitieren
Lesezeichen