gaku takasawa
mylibrary
Nunchuck/Nunchuck.cpp@9:1c0640c61fce, 2017-05-11 (annotated)
- Committer:
- gaku_sigu
- Date:
- Thu May 11 11:04:30 2017 +0000
- Revision:
- 9:1c0640c61fce
- Parent:
- 7:15e3890a6782
- Child:
- 10:d0b1160ee5c2
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| gaku_sigu | 0:a919993ff50f | 1 | #include "Nunchuck.h" |
| gaku_sigu | 0:a919993ff50f | 2 | |
| gaku_sigu | 0:a919993ff50f | 3 | |
| gaku_sigu | 0:a919993ff50f | 4 | Nunchuck::Nunchuck(PinName SDA, PinName SCL) : I2C(SDA, SCL) |
| gaku_sigu | 0:a919993ff50f | 5 | { |
| gaku_sigu | 0:a919993ff50f | 6 | flag = 0; |
| gaku_sigu | 0:a919993ff50f | 7 | for(int i = 0; i < 6; i++) |
| gaku_sigu | 0:a919993ff50f | 8 | data[i] = 0; |
| gaku_sigu | 0:a919993ff50f | 9 | init(); |
| gaku_sigu | 0:a919993ff50f | 10 | timer.start(); |
| gaku_sigu | 0:a919993ff50f | 11 | } |
| gaku_sigu | 0:a919993ff50f | 12 | |
| gaku_sigu | 0:a919993ff50f | 13 | bool Nunchuck::init() |
| gaku_sigu | 0:a919993ff50f | 14 | { |
| gaku_sigu | 0:a919993ff50f | 15 | unsigned char cmd[] = {0x40, 0x00}; |
| gaku_sigu | 3:e7a900958f54 | 16 | if (I2C::write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == 0) |
| gaku_sigu | 0:a919993ff50f | 17 | return 1; |
| gaku_sigu | 0:a919993ff50f | 18 | else |
| gaku_sigu | 0:a919993ff50f | 19 | return 0; |
| gaku_sigu | 0:a919993ff50f | 20 | } |
| gaku_sigu | 0:a919993ff50f | 21 | |
| gaku_sigu | 0:a919993ff50f | 22 | void Nunchuck::getdata() |
| gaku_sigu | 0:a919993ff50f | 23 | { |
| gaku_sigu | 0:a919993ff50f | 24 | if(timer.read_ms() < 50) |
| gaku_sigu | 0:a919993ff50f | 25 | return; |
| gaku_sigu | 0:a919993ff50f | 26 | |
| gaku_sigu | 0:a919993ff50f | 27 | |
| gaku_sigu | 0:a919993ff50f | 28 | if(flag) { |
| gaku_sigu | 0:a919993ff50f | 29 | const unsigned char cmd[] = {0x00}; |
| gaku_sigu | 4:ae9dc8d5c37e | 30 | if (I2C::write(NUNCHUCK_ADDR, (const char*)cmd, sizeof(cmd)) == 0) |
| gaku_sigu | 0:a919993ff50f | 31 | { |
| gaku_sigu | 0:a919993ff50f | 32 | wait(0.01); |
| gaku_sigu | 4:ae9dc8d5c37e | 33 | if (I2C::read(NUNCHUCK_ADDR, data, sizeof(data)) == 0) |
| gaku_sigu | 0:a919993ff50f | 34 | { |
| gaku_sigu | 2:7b7821addb45 | 35 | for(int i = 0; i < 6; ++i) |
| gaku_sigu | 0:a919993ff50f | 36 | data[i] = (data[i] ^ 0x17) + 0x17; |
| gaku_sigu | 0:a919993ff50f | 37 | } |
| gaku_sigu | 0:a919993ff50f | 38 | } |
| gaku_sigu | 0:a919993ff50f | 39 | } |
| gaku_sigu | 0:a919993ff50f | 40 | else |
| gaku_sigu | 0:a919993ff50f | 41 | flag = init(); |
| gaku_sigu | 0:a919993ff50f | 42 | |
| gaku_sigu | 0:a919993ff50f | 43 | timer.reset(); |
| gaku_sigu | 0:a919993ff50f | 44 | } |
| gaku_sigu | 0:a919993ff50f | 45 | |
| gaku_sigu | 0:a919993ff50f | 46 | |
| gaku_sigu | 0:a919993ff50f | 47 | int8_t Nunchuck::analogx() |
| gaku_sigu | 0:a919993ff50f | 48 | { |
| gaku_sigu | 0:a919993ff50f | 49 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 50 | int8_t temp; |
| gaku_sigu | 0:a919993ff50f | 51 | temp = data[0] - 128; |
| gaku_sigu | 3:e7a900958f54 | 52 | #if NUNCHUCK_ANALOGDATA |
| gaku_sigu | 3:e7a900958f54 | 53 | if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE) |
| gaku_sigu | 0:a919993ff50f | 54 | temp = 0; |
| gaku_sigu | 0:a919993ff50f | 55 | #else |
| gaku_sigu | 0:a919993ff50f | 56 | if(-50 < temp && temp < 50) |
| gaku_sigu | 0:a919993ff50f | 57 | temp = 0; |
| gaku_sigu | 0:a919993ff50f | 58 | else if(temp <= -50) |
| gaku_sigu | 0:a919993ff50f | 59 | temp = -1; |
| gaku_sigu | 0:a919993ff50f | 60 | else if(temp >= 50) |
| gaku_sigu | 0:a919993ff50f | 61 | temp = 1; |
| gaku_sigu | 0:a919993ff50f | 62 | #endif |
| gaku_sigu | 0:a919993ff50f | 63 | return temp; |
| gaku_sigu | 0:a919993ff50f | 64 | } |
| gaku_sigu | 0:a919993ff50f | 65 | |
| gaku_sigu | 0:a919993ff50f | 66 | |
| gaku_sigu | 0:a919993ff50f | 67 | int8_t Nunchuck::analogy() |
| gaku_sigu | 0:a919993ff50f | 68 | { |
| gaku_sigu | 0:a919993ff50f | 69 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 70 | int8_t temp; |
| gaku_sigu | 0:a919993ff50f | 71 | temp = data[1] - 128; |
| gaku_sigu | 3:e7a900958f54 | 72 | #if NUNCHUCK_ANALOGDATA |
| gaku_sigu | 3:e7a900958f54 | 73 | if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE) |
| gaku_sigu | 0:a919993ff50f | 74 | temp = 0; |
| gaku_sigu | 0:a919993ff50f | 75 | #else |
| gaku_sigu | 0:a919993ff50f | 76 | if(-50 < temp && temp < 50) |
| gaku_sigu | 0:a919993ff50f | 77 | temp = 0; |
| gaku_sigu | 0:a919993ff50f | 78 | else if(temp <= -50) |
| gaku_sigu | 0:a919993ff50f | 79 | temp = -1; |
| gaku_sigu | 0:a919993ff50f | 80 | else if(temp >= 50) |
| gaku_sigu | 0:a919993ff50f | 81 | temp = 1; |
| gaku_sigu | 0:a919993ff50f | 82 | #endif |
| gaku_sigu | 0:a919993ff50f | 83 | return temp; |
| gaku_sigu | 0:a919993ff50f | 84 | } |
| gaku_sigu | 0:a919993ff50f | 85 | |
| gaku_sigu | 0:a919993ff50f | 86 | |
| gaku_sigu | 3:e7a900958f54 | 87 | double Nunchuck::analograd() |
| gaku_sigu | 3:e7a900958f54 | 88 | { |
| gaku_sigu | 3:e7a900958f54 | 89 | double x = analogx(); |
| gaku_sigu | 3:e7a900958f54 | 90 | double y = analogy(); |
| gaku_sigu | 3:e7a900958f54 | 91 | |
| gaku_sigu | 9:1c0640c61fce | 92 | return atan2(y, x); |
| gaku_sigu | 3:e7a900958f54 | 93 | } |
| gaku_sigu | 3:e7a900958f54 | 94 | |
| gaku_sigu | 5:69e9c81e9490 | 95 | |
| gaku_sigu | 5:69e9c81e9490 | 96 | double Nunchuck::analogdeg() |
| gaku_sigu | 5:69e9c81e9490 | 97 | { |
| gaku_sigu | 5:69e9c81e9490 | 98 | return analograd() * 180.0 / PI; |
| gaku_sigu | 5:69e9c81e9490 | 99 | } |
| gaku_sigu | 5:69e9c81e9490 | 100 | |
| gaku_sigu | 5:69e9c81e9490 | 101 | |
| gaku_sigu | 7:15e3890a6782 | 102 | double Nunchuck::analogrange() |
| gaku_sigu | 7:15e3890a6782 | 103 | { |
| gaku_sigu | 7:15e3890a6782 | 104 | double x = analogx(); |
| gaku_sigu | 7:15e3890a6782 | 105 | double y = analogy(); |
| gaku_sigu | 7:15e3890a6782 | 106 | return sqrt((x*x + y*y)); |
| gaku_sigu | 7:15e3890a6782 | 107 | } |
| gaku_sigu | 7:15e3890a6782 | 108 | |
| gaku_sigu | 7:15e3890a6782 | 109 | |
| gaku_sigu | 0:a919993ff50f | 110 | int Nunchuck::accx() |
| gaku_sigu | 0:a919993ff50f | 111 | { |
| gaku_sigu | 0:a919993ff50f | 112 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 113 | int temp = data[2] << 2; |
| gaku_sigu | 0:a919993ff50f | 114 | if ((data[5] >> 2) & 1) temp += 2; |
| gaku_sigu | 0:a919993ff50f | 115 | if ((data[5] >> 3) & 1) temp += 1; |
| gaku_sigu | 0:a919993ff50f | 116 | return data[2]; |
| gaku_sigu | 0:a919993ff50f | 117 | } |
| gaku_sigu | 0:a919993ff50f | 118 | |
| gaku_sigu | 0:a919993ff50f | 119 | |
| gaku_sigu | 0:a919993ff50f | 120 | int Nunchuck::accy() |
| gaku_sigu | 0:a919993ff50f | 121 | { |
| gaku_sigu | 0:a919993ff50f | 122 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 123 | int temp = data[3] << 2; |
| gaku_sigu | 0:a919993ff50f | 124 | if ((data[5] >> 4) & 1) temp += 2; |
| gaku_sigu | 0:a919993ff50f | 125 | if ((data[5] >> 5) & 1) temp += 1; |
| gaku_sigu | 0:a919993ff50f | 126 | return data[3]; |
| gaku_sigu | 0:a919993ff50f | 127 | } |
| gaku_sigu | 0:a919993ff50f | 128 | |
| gaku_sigu | 0:a919993ff50f | 129 | |
| gaku_sigu | 0:a919993ff50f | 130 | int Nunchuck::accz() |
| gaku_sigu | 0:a919993ff50f | 131 | { |
| gaku_sigu | 0:a919993ff50f | 132 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 133 | int temp = data[4] << 2; |
| gaku_sigu | 0:a919993ff50f | 134 | if ((data[5] >> 6) & 1) temp += 2; |
| gaku_sigu | 0:a919993ff50f | 135 | if ((data[5] >> 7) & 1) temp += 1; |
| gaku_sigu | 0:a919993ff50f | 136 | return data[4]; |
| gaku_sigu | 0:a919993ff50f | 137 | } |
| gaku_sigu | 0:a919993ff50f | 138 | |
| gaku_sigu | 0:a919993ff50f | 139 | |
| gaku_sigu | 0:a919993ff50f | 140 | bool Nunchuck::buttonz() |
| gaku_sigu | 0:a919993ff50f | 141 | { |
| gaku_sigu | 0:a919993ff50f | 142 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 143 | return !(data[5] & 0x01); |
| gaku_sigu | 0:a919993ff50f | 144 | } |
| gaku_sigu | 0:a919993ff50f | 145 | |
| gaku_sigu | 0:a919993ff50f | 146 | |
| gaku_sigu | 0:a919993ff50f | 147 | bool Nunchuck::buttonc() |
| gaku_sigu | 0:a919993ff50f | 148 | { |
| gaku_sigu | 0:a919993ff50f | 149 | getdata(); |
| gaku_sigu | 0:a919993ff50f | 150 | return !(data[5] & 0x02); |
| gaku_sigu | 0:a919993ff50f | 151 | } |