1/* 2 * Author: Jon Trulson <jtrulson@ics.com> 3 * Copyright (c) 2015 Intel Corporation. 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining 6 * a copy of this software and associated documentation files (the 7 * "Software"), to deal in the Software without restriction, including 8 * without limitation the rights to use, copy, modify, merge, publish, 9 * distribute, sublicense, and/or sell copies of the Software, and to 10 * permit persons to whom the Software is furnished to do so, subject to 11 * the following conditions: 12 * 13 * The above copyright notice and this permission notice shall be 14 * included in all copies or substantial portions of the Software. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 */ 24 25#include <iostream> 26#include <string> 27#include <stdexcept> 28#include <unistd.h> 29 30#include "nunchuck.h" 31 32using namespace upm; 33using namespace std; 34 35 36NUNCHUCK::NUNCHUCK(int bus, uint8_t addr) 37{ 38 stickX = 0; 39 stickY = 0; 40 accelX = 0; 41 accelY = 0; 42 accelZ = 0; 43 buttonC = false; 44 buttonZ = false; 45 46 // setup our i2c link 47 if ( !(m_i2c = mraa_i2c_init(bus)) ) 48 { 49 throw std::invalid_argument(std::string(__FUNCTION__) + 50 ": mraa_i2c_init() failed"); 51 return; 52 } 53 54 mraa_result_t rv; 55 56 if ( (rv = mraa_i2c_address(m_i2c, addr)) != MRAA_SUCCESS ) 57 { 58 throw std::invalid_argument(std::string(__FUNCTION__) + 59 ": mraa_i2c_address() failed"); 60 } 61} 62 63NUNCHUCK::~NUNCHUCK() 64{ 65 mraa_i2c_stop(m_i2c); 66} 67 68bool NUNCHUCK::writeByte(uint8_t reg, uint8_t byte) 69{ 70 mraa_result_t rv; 71 72 if ( (rv = mraa_i2c_write_byte_data(m_i2c, byte, reg)) != MRAA_SUCCESS ) 73 { 74 throw std::runtime_error(std::string(__FUNCTION__) + 75 ": mraa_i2c_write_byte_data() failed"); 76 return false; 77 } 78 79 return true; 80} 81 82int NUNCHUCK::readBytes(uint8_t reg, uint8_t *buffer, int len) 83{ 84 if (!len || !buffer) 85 return 0; 86 87 mraa_i2c_address(m_i2c, NUNCHUCK_I2C_ADDR); 88 mraa_i2c_write_byte(m_i2c, reg); 89 90 return mraa_i2c_read(m_i2c, buffer, len); 91} 92 93bool NUNCHUCK::init() 94{ 95 usleep(1000000); 96 97 // disable encryption 98 if (!writeByte(0xf0, 0x55)) 99 return false; 100 101 if (!writeByte(0xfb, 0x00)) 102 return false; 103 104 return true; 105} 106 107void NUNCHUCK::update() 108{ 109 const int bufsize = 6; 110 uint8_t buf[bufsize]; 111 int rv; 112 113 rv = readBytes(0x00, buf, bufsize); 114 115 if (rv != bufsize) 116 { 117 throw std::runtime_error(std::string(__FUNCTION__) + 118 ": readBytes() failed"); 119 return; 120 } 121 122 // analog stick X 123 stickX = buf[0]; 124 125 // analog stick Y 126 stickY = buf[1]; 127 128 // accelerometer X 129 accelX = ( (buf[2] << 2) | ((buf[5] & 0x0c) >> 2) ); 130 131 // accelerometer Y 132 accelY = ( (buf[3] << 2) | ((buf[5] & 0x30) >> 4) ); 133 134 // accelerometer Z 135 accelZ = ( (buf[4] << 2) | ((buf[5] & 0xc0) >> 6) ); 136 137 // buttonC 138 if (buf[5] & 0x02) 139 buttonC = false; 140 else 141 buttonC = true; 142 143 // buttonZ 144 if (buf[5] & 0x01) 145 buttonZ = false; 146 else 147 buttonZ = true; 148} 149 150