mylibrary
Nunchuck/Nunchuck.cpp@4:ae9dc8d5c37e, 2017-05-11 (annotated)
- Committer:
- gaku_sigu
- Date:
- Thu May 11 07:34:44 2017 +0000
- Revision:
- 4:ae9dc8d5c37e
- Parent:
- 3:e7a900958f54
- Child:
- 5:69e9c81e9490
remove?error
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 | 3:e7a900958f54 | 92 | return atan2(y,x); |
gaku_sigu | 3:e7a900958f54 | 93 | } |
gaku_sigu | 3:e7a900958f54 | 94 | |
gaku_sigu | 0:a919993ff50f | 95 | int Nunchuck::accx() |
gaku_sigu | 0:a919993ff50f | 96 | { |
gaku_sigu | 0:a919993ff50f | 97 | getdata(); |
gaku_sigu | 0:a919993ff50f | 98 | int temp = data[2] << 2; |
gaku_sigu | 0:a919993ff50f | 99 | if ((data[5] >> 2) & 1) temp += 2; |
gaku_sigu | 0:a919993ff50f | 100 | if ((data[5] >> 3) & 1) temp += 1; |
gaku_sigu | 0:a919993ff50f | 101 | return data[2]; |
gaku_sigu | 0:a919993ff50f | 102 | } |
gaku_sigu | 0:a919993ff50f | 103 | |
gaku_sigu | 0:a919993ff50f | 104 | |
gaku_sigu | 0:a919993ff50f | 105 | int Nunchuck::accy() |
gaku_sigu | 0:a919993ff50f | 106 | { |
gaku_sigu | 0:a919993ff50f | 107 | getdata(); |
gaku_sigu | 0:a919993ff50f | 108 | int temp = data[3] << 2; |
gaku_sigu | 0:a919993ff50f | 109 | if ((data[5] >> 4) & 1) temp += 2; |
gaku_sigu | 0:a919993ff50f | 110 | if ((data[5] >> 5) & 1) temp += 1; |
gaku_sigu | 0:a919993ff50f | 111 | return data[3]; |
gaku_sigu | 0:a919993ff50f | 112 | } |
gaku_sigu | 0:a919993ff50f | 113 | |
gaku_sigu | 0:a919993ff50f | 114 | |
gaku_sigu | 0:a919993ff50f | 115 | int Nunchuck::accz() |
gaku_sigu | 0:a919993ff50f | 116 | { |
gaku_sigu | 0:a919993ff50f | 117 | getdata(); |
gaku_sigu | 0:a919993ff50f | 118 | int temp = data[4] << 2; |
gaku_sigu | 0:a919993ff50f | 119 | if ((data[5] >> 6) & 1) temp += 2; |
gaku_sigu | 0:a919993ff50f | 120 | if ((data[5] >> 7) & 1) temp += 1; |
gaku_sigu | 0:a919993ff50f | 121 | return data[4]; |
gaku_sigu | 0:a919993ff50f | 122 | } |
gaku_sigu | 0:a919993ff50f | 123 | |
gaku_sigu | 0:a919993ff50f | 124 | |
gaku_sigu | 0:a919993ff50f | 125 | bool Nunchuck::buttonz() |
gaku_sigu | 0:a919993ff50f | 126 | { |
gaku_sigu | 0:a919993ff50f | 127 | getdata(); |
gaku_sigu | 0:a919993ff50f | 128 | return !(data[5] & 0x01); |
gaku_sigu | 0:a919993ff50f | 129 | } |
gaku_sigu | 0:a919993ff50f | 130 | |
gaku_sigu | 0:a919993ff50f | 131 | |
gaku_sigu | 0:a919993ff50f | 132 | bool Nunchuck::buttonc() |
gaku_sigu | 0:a919993ff50f | 133 | { |
gaku_sigu | 0:a919993ff50f | 134 | getdata(); |
gaku_sigu | 0:a919993ff50f | 135 | return !(data[5] & 0x02); |
gaku_sigu | 0:a919993ff50f | 136 | } |