Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6ControlServoLib.h"
#include "RP6Control_OrientationLib.h"
#define I2C_RP6_BASE_ADR 10
/*****************************************************************************/
// Variables:
uint8_t ch;
char item[12];
char dir[3];
/*****************************************************************************/
/**
* 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 and terminal***********************/
/**
*
*
* 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]);
}
/*********************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);
}
/*****************gyroscope***************/
void gyroscope(void) // L3GD20 gyroscope
{
mSleep(1000);
task_I2CTWI();
readL3GD20(); // Get sensor values
// normalizeL3GD20();
task_I2CTWI();
clearLCD();
setCursorPosLCD(0, 0); // line 1
writeStringLCD_P("X");
writeIntegerLCD(x_axisg, DEC);
setCursorPosLCD(0, 6); // line 1 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(y_axisg, DEC);
setCursorPosLCD(0, 12); // line1 pos 12
writeStringLCD_P("Z");
writeIntegerLCD(z_axisg, DEC);
// setCursorPosLCD(1, 0); // line 2
// writeStringLCD_P("X");
writeString_P("x: ");
writeDouble(xg, 1, 1);
writeString_P("\n");
/* setCursorPosLCD(1, 6); // line 2 pos 6
writeStringLCD_P("Y");
writeIntegerLCD(yg, DEC);
setCursorPosLCD(1, 12); // line 2 pos 12
writeStringLCD_P("Z");
writeIntegerLCD(zg, DEC);*/
/*********/
/* setServoPower(1);
initSERVO(SERVO1);
setServo(1, SERVO1_LT);
mSleep(50);
setServo(1, SERVO1_RT);
mSleep(100);
setServoPower(0);
task_SERVO();
*/
/*********/
#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
mSleep(2000);//3 sec
clearLCD();
task_I2CTWI();
}
/************accelerometer****************/
void accelerometer(void) // LSM303DLHC accelerometer
{
mSleep(1000);
task_I2CTWI();
readLSM303DLHC_A(); // Get sensor values
// normalizeLSM303DLHC_A();
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(" ");
mSleep(2000);
clearLCD();
task_I2CTWI();
}
/*****************magnetometer************/
void magnetometer(void) // LSM303DLHC magnetometer
{
mSleep(1000);
task_I2CTWI();
readLSM303DLHC_M(); // Get sensor values
// normalizeLSM303DLHC_M();
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(1000);
// 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
showScreenLCD(" RP6Control M32", " calibrate_imu");
mSleep(2500);
clearLCD();
orientation_init();
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);
while(true)
{
setLEDs(0b0001);
gyroscope();
mSleep(500);
clearLCD();
/**************************/
uint8_t key_1 = getMultiIOPressedButtonNumber();
key_1 = getMultiIOPressedButtonNumber();
if(key_1 != 0)
{
break;
}
/**************************/
}
break;
case 2://
setLEDs(0b0010);
while(true)
{
setLEDs(0b0010);
accelerometer();
mSleep(500);
clearLCD();
/**************************/
uint8_t key_1 = getMultiIOPressedButtonNumber();
key_1 = getMultiIOPressedButtonNumber();
if(key_1 != 0)
{
break;
}
/**************************/
}
break;
case 3://
setLEDs(0b0100);
while(true)
{
magnetometer();
mSleep(50);
clearLCD();
/**************************/
uint8_t key_1 = getMultiIOPressedButtonNumber();
key_1 = getMultiIOPressedButtonNumber();
if(key_1 != 0)
{
break;
}
/**************************/
}
break;
case 4://
setLEDs(0b1000);
while(true)
{
setLEDs(0b1000);
gyroscope();
mSleep(50);
clearLCD();
accelerometer();
mSleep(50);
clearLCD();
magnetometer();
mSleep(50);
clearLCD();
/**************************/
uint8_t key_1 = getMultiIOPressedButtonNumber();
key_1 = getMultiIOPressedButtonNumber();
if(key_1 != 0)
{
break;
}
/**************************/
}
break;
}
}
}
return 0;
}
damit kann ich jetzt z.b. den 3D-gyro (und seine einzelnen komponenten) testen und werte überprüfen ohne unnütz warten zu müssen bis nun die erwünschte einzelfunktion wiederkommt...
Zitat von Wsk8
Lesezeichen