![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code for the left controller (Niimote)
Dependencies: PinDetect_hw3 USBDevice mbed
Diff: acc_main.cpp
- Revision:
- 1:8252a6052ebe
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/acc_main.cpp Mon Sep 21 15:19:03 2015 +0000 @@ -0,0 +1,253 @@ +#include "mbed.h" +#include "USBKeyboard.h" +#include "PinDetect.h" + +/* Testing example code from Simon Scott on using BNO055. */ + +USBKeyboard keyboard; + +Serial pc(USBTX,USBRX); +I2C i2c(A4, A5); + +DigitalOut led(LED1); +DigitalOut led2(LED2); + +const int bno055_addr = 0x28 << 1; + +const int BNO055_ID_ADDR = 0x00; +const int BNO055_EULER_H_LSB_ADDR = 0x1A; +const int BNO055_TEMP_ADDR = 0x34; +const int BNO055_OPR_MODE_ADDR = 0x3D; +const int BNO055_CALIB_STAT_ADDR = 0x35; +const int BNO055_SYS_STAT_ADDR = 0x39; +const int BNO055_SYS_ERR_ADDR = 0x3A; +const int BNO055_AXIS_MAP_CONFIG_ADDR = 0x41; +const int BNO055_SYS_TRIGGER_ADDR = 0x3F; + +typedef struct CalibStatus_t +{ + int mag; + int acc; + int gyr; + int sys; +} CalibStatus; + +typedef struct Euler_t +{ + float heading; + float pitch; + float roll; +} Euler; + +Euler calibrationVals; +/** + * Function to write to a single 8-bit register + */ +void writeReg(int regAddr, char value) +{ + char wbuf[2]; + wbuf[0] = regAddr; + wbuf[1] = value; + i2c.write(bno055_addr, wbuf, 2, false); +} + +/** + * Function to read from a single 8-bit register + */ +char readReg(int regAddr) +{ + char rwbuf = regAddr; + i2c.write(bno055_addr, &rwbuf, 1, false); + i2c.read(bno055_addr, &rwbuf, 1, false); + return rwbuf; +} + +/** + * Returns the calibration status of each component + */ +CalibStatus readCalibrationStatus() +{ + CalibStatus status; + int regVal = readReg(BNO055_CALIB_STAT_ADDR); + + status.mag = regVal & 0x03; + status.acc = (regVal >> 2) & 0x03; + status.gyr = (regVal >> 4) & 0x03; + status.sys = (regVal >> 6) & 0x03; + + return status; +} + + +/** + * Checks that there are no errors on the accelerometer + */ +bool bno055Healthy() +{ + int sys_error = readReg(BNO055_SYS_ERR_ADDR); + wait(0.001); + int sys_stat = readReg(BNO055_SYS_STAT_ADDR); + wait(0.001); + + if(sys_error == 0 && sys_stat == 5) + return true; + else + return false; +} + +/** + * Reads the Euler angles, zeroed out + */ +Euler getEulerAngles() +{ + char buf[16]; + Euler e; + + // Read in the Euler angles + buf[0] = BNO055_EULER_H_LSB_ADDR; + i2c.write(bno055_addr, buf, 1, false); + i2c.read(bno055_addr, buf, 6, false); + + short int euler_head = buf[0] + (buf[1] << 8); + short int euler_roll = buf[2] + (buf[3] << 8); + short int euler_pitch = buf[4] + (buf[5] << 8); + + e.heading = ((float)euler_head) / 16.0; + e.roll = ((float)euler_roll) / 16.0; + e.pitch = ((float)euler_pitch) / 16.0; + + return e; +} + + + +/** + * Configure and initialize the BNO055 + */ +bool initBNO055() +{ + unsigned char regVal; + i2c.frequency(400000); + bool startupPass = true; + + // Do some basic power-up tests + regVal = readReg(BNO055_ID_ADDR); + if(regVal == 0xA0) + pc.printf("BNO055 successfully detected!\r\n"); + else { + pc.printf("ERROR: no BNO055 detected\r\n"); + startupPass = false; + } + + regVal = readReg(BNO055_TEMP_ADDR); + pc.printf("Chip temperature is: %d C\r\n", regVal); + + if(regVal == 0) + startupPass = false; + + // Change mode to CONFIG + writeReg(BNO055_OPR_MODE_ADDR, 0x00); + wait(0.2); + + regVal = readReg(BNO055_OPR_MODE_ADDR); + pc.printf("Change to mode: %d\r\n", regVal); + wait(0.1); + + // Remap axes + writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06); // b00_00_01_10 + wait(0.1); + + // Set to external crystal + writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80); + wait(0.2); + + // Change mode to NDOF + writeReg(BNO055_OPR_MODE_ADDR, 0x0C); + wait(0.2); + + regVal = readReg(BNO055_OPR_MODE_ADDR); + pc.printf("Change to mode: %d\r\n", regVal); + wait(0.1); + + calibrationVals = getEulerAngles(); + return startupPass; +} + +float reduceAngle(float target, float source) { + float a = target - source; + if (a > 180) { + a -= 360; + } else if (a < -180) { + a += 360; + } + return a; +} +int main() { + +// button1.attach_asserted(&button1_pressed); +// button1.attach_deasserted(&button1_released); +// button1.setAssertValue(0); +// button1.setSampleFrequency(); +// +// button2.attach_asserted(&button2_pressed); +// button2.attach_deasserted(&button2_released); +// button2.setAssertValue(0); +// button2.setSampleFrequency(); + + bool startupPassed; + Euler e; + + //RED_LED = 0; + led = 0; + led2 = 1; + // Initialize + pc.baud(115200); + pc.printf("starting up\r\n"); + wait(0.5); + startupPassed = initBNO055(); // Note: set LED to RED if this fails + + float rollAvg = 0; + + float pitchAvg = 0; + int readCount = 0; + + bool sentChar; + // Read orientation values + while(true) + { + sentChar = false; + // Read in the Euler angles + e = getEulerAngles(); + + wait(0.001); + + // Read in the calibration status + CalibStatus calStat = readCalibrationStatus(); + + // correct the heading, roll, pitch vals + float corHead = reduceAngle(e.heading, calibrationVals.heading); + float corRoll = reduceAngle(e.roll, calibrationVals.roll); + float corPitch = reduceAngle(e.pitch, calibrationVals.pitch); +// printf("Corrected: %7.2f \tRoll: %7.2f \tPitch: %7.2f \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", corHead, corRoll, corPitch, calStat.mag, calStat.acc, calStat.gyr, calStat.sys); + + + rollAvg = corRoll; //.8*rollAvg + .2 * reduceAngle(corRoll, rollNums[readCount]); + //rollNums[readCount] = corRoll; + + + pitchAvg = corPitch;//.8*pitchAvg + .2 * reduceAngle(corPitch, pitchNums[readCount]); + //pitchNums[readCount] = corPitch; + + if (readCount >=5) { readCount = 0; } + readCount++; + + if (rollAvg < -10){ + keyboard.keyCode('z'); + keyboard.keyCode('z'); + keyboard.keyCode('z'); + keyboard.keyCode('z'); + } + + wait(0.05); + } +}