Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_OrientationLib.h"
#include "RP6ControlServoLib.h"
#include "RP6Control_LFSBumperLib.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);
}
/*********************calculateDir*********************************************/
/**
* 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]);
}
/*************** 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
showScreenLCD(" RP6Control M32", " min_IMU");
mSleep(2500);
clearLCD();
while(true)
{
setLEDs(0b0001);
startStopwatch1();
// IMPORTANT:
orientation_init(); // Orientation init!
while(true)
{
if(getStopwatch1() > 1000) // 1,5s
{
#ifdef IMU_9D
// 9D-IMU test:
// L3GD20 gyroscope:
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();
// LSM303DLHC accelerometer:
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);
// LSM303DLHC magnetometer:
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();
#endif
setStopwatch1(0);
}
task_I2CTWI();
}
}
return 0;
}
ich weiss jetzt natürlich nicht was die werte, die da angezeigt werden, nun bedeuten, ob sie richtig sind...
Zitieren
Lesezeichen