Kiko Ishimoto / MyLib4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   robocon2017mbed_contoroler_L2

Fork of MyLib by Kiko Ishimoto

Nunchuck/Nunchuck.cpp@11:08d8bd103eda, 2017-10-13 (annotated)

Committer:
kikoaac
Date:
Fri Oct 13 09:35:30 2017 +0000
Revision:
11:08d8bd103eda
Parent:
10:d0b1160ee5c2
Child:
12:479a1f294781
testes

Who changed what in which revision?

UserRevisionLine numberNew 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 }
kikoaac 11:08d8bd103eda 38 }else{
kikoaac 11:08d8bd103eda 39 flag = false;
gaku_sigu 0:a919993ff50f 40 }
gaku_sigu 0:a919993ff50f 41 }
gaku_sigu 0:a919993ff50f 42 else
gaku_sigu 0:a919993ff50f 43 flag = init();
gaku_sigu 0:a919993ff50f 44
gaku_sigu 0:a919993ff50f 45 timer.reset();
gaku_sigu 0:a919993ff50f 46 }
gaku_sigu 0:a919993ff50f 47
gaku_sigu 0:a919993ff50f 48
gaku_sigu 0:a919993ff50f 49 int8_t Nunchuck::analogx()
gaku_sigu 0:a919993ff50f 50 {
gaku_sigu 0:a919993ff50f 51 getdata();
gaku_sigu 0:a919993ff50f 52 int8_t temp;
gaku_sigu 0:a919993ff50f 53 temp = data[0] - 128;
gaku_sigu 3:e7a900958f54 54 #if NUNCHUCK_ANALOGDATA
gaku_sigu 3:e7a900958f54 55 if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE)
gaku_sigu 0:a919993ff50f 56 temp = 0;
gaku_sigu 0:a919993ff50f 57 #else
gaku_sigu 0:a919993ff50f 58 if(-50 < temp && temp < 50)
gaku_sigu 0:a919993ff50f 59 temp = 0;
gaku_sigu 0:a919993ff50f 60 else if(temp <= -50)
gaku_sigu 0:a919993ff50f 61 temp = -1;
gaku_sigu 0:a919993ff50f 62 else if(temp >= 50)
gaku_sigu 0:a919993ff50f 63 temp = 1;
gaku_sigu 0:a919993ff50f 64 #endif
gaku_sigu 0:a919993ff50f 65 return temp;
gaku_sigu 0:a919993ff50f 66 }
gaku_sigu 0:a919993ff50f 67
gaku_sigu 0:a919993ff50f 68
gaku_sigu 0:a919993ff50f 69 int8_t Nunchuck::analogy()
gaku_sigu 0:a919993ff50f 70 {
gaku_sigu 0:a919993ff50f 71 getdata();
gaku_sigu 0:a919993ff50f 72 int8_t temp;
gaku_sigu 0:a919993ff50f 73 temp = data[1] - 128;
gaku_sigu 3:e7a900958f54 74 #if NUNCHUCK_ANALOGDATA
gaku_sigu 3:e7a900958f54 75 if(-1*(NUNCHUCK_DEADZONE) < temp && temp < NUNCHUCK_DEADZONE)
gaku_sigu 0:a919993ff50f 76 temp = 0;
gaku_sigu 0:a919993ff50f 77 #else
gaku_sigu 0:a919993ff50f 78 if(-50 < temp && temp < 50)
gaku_sigu 0:a919993ff50f 79 temp = 0;
gaku_sigu 0:a919993ff50f 80 else if(temp <= -50)
gaku_sigu 0:a919993ff50f 81 temp = -1;
gaku_sigu 0:a919993ff50f 82 else if(temp >= 50)
gaku_sigu 0:a919993ff50f 83 temp = 1;
gaku_sigu 0:a919993ff50f 84 #endif
gaku_sigu 0:a919993ff50f 85 return temp;
gaku_sigu 0:a919993ff50f 86 }
gaku_sigu 0:a919993ff50f 87
gaku_sigu 0:a919993ff50f 88
gaku_sigu 3:e7a900958f54 89 double Nunchuck::analograd()
gaku_sigu 3:e7a900958f54 90 {
gaku_sigu 3:e7a900958f54 91 double x = analogx();
gaku_sigu 3:e7a900958f54 92 double y = analogy();
gaku_sigu 3:e7a900958f54 93
gaku_sigu 9:1c0640c61fce 94 return atan2(y, x);
gaku_sigu 3:e7a900958f54 95 }
gaku_sigu 3:e7a900958f54 96
gaku_sigu 5:69e9c81e9490 97
gaku_sigu 5:69e9c81e9490 98 double Nunchuck::analogdeg()
gaku_sigu 5:69e9c81e9490 99 {
gaku_sigu 5:69e9c81e9490 100 return analograd() * 180.0 / PI;
gaku_sigu 5:69e9c81e9490 101 }
gaku_sigu 5:69e9c81e9490 102
gaku_sigu 5:69e9c81e9490 103
gaku_sigu 7:15e3890a6782 104 double Nunchuck::analogrange()
gaku_sigu 7:15e3890a6782 105 {
gaku_sigu 7:15e3890a6782 106 double x = analogx();
gaku_sigu 7:15e3890a6782 107 double y = analogy();
gaku_sigu 7:15e3890a6782 108 return sqrt((x*x + y*y));
gaku_sigu 7:15e3890a6782 109 }
gaku_sigu 7:15e3890a6782 110
gaku_sigu 7:15e3890a6782 111
gaku_sigu 0:a919993ff50f 112 int Nunchuck::accx()
gaku_sigu 0:a919993ff50f 113 {
gaku_sigu 0:a919993ff50f 114 getdata();
gaku_sigu 0:a919993ff50f 115 int temp = data[2] << 2;
gaku_sigu 0:a919993ff50f 116 if ((data[5] >> 2) & 1) temp += 2;
gaku_sigu 0:a919993ff50f 117 if ((data[5] >> 3) & 1) temp += 1;
gaku_sigu 10:d0b1160ee5c2 118 return temp;
gaku_sigu 0:a919993ff50f 119 }
gaku_sigu 0:a919993ff50f 120
gaku_sigu 0:a919993ff50f 121
gaku_sigu 0:a919993ff50f 122 int Nunchuck::accy()
gaku_sigu 0:a919993ff50f 123 {
gaku_sigu 0:a919993ff50f 124 getdata();
gaku_sigu 0:a919993ff50f 125 int temp = data[3] << 2;
gaku_sigu 0:a919993ff50f 126 if ((data[5] >> 4) & 1) temp += 2;
gaku_sigu 0:a919993ff50f 127 if ((data[5] >> 5) & 1) temp += 1;
gaku_sigu 10:d0b1160ee5c2 128 return temp;
gaku_sigu 0:a919993ff50f 129 }
gaku_sigu 0:a919993ff50f 130
gaku_sigu 0:a919993ff50f 131
gaku_sigu 0:a919993ff50f 132 int Nunchuck::accz()
gaku_sigu 0:a919993ff50f 133 {
gaku_sigu 0:a919993ff50f 134 getdata();
gaku_sigu 0:a919993ff50f 135 int temp = data[4] << 2;
gaku_sigu 0:a919993ff50f 136 if ((data[5] >> 6) & 1) temp += 2;
gaku_sigu 0:a919993ff50f 137 if ((data[5] >> 7) & 1) temp += 1;
gaku_sigu 10:d0b1160ee5c2 138 return temp;
gaku_sigu 0:a919993ff50f 139 }
gaku_sigu 0:a919993ff50f 140
gaku_sigu 0:a919993ff50f 141
gaku_sigu 0:a919993ff50f 142 bool Nunchuck::buttonz()
gaku_sigu 0:a919993ff50f 143 {
gaku_sigu 0:a919993ff50f 144 getdata();
gaku_sigu 0:a919993ff50f 145 return !(data[5] & 0x01);
gaku_sigu 0:a919993ff50f 146 }
gaku_sigu 0:a919993ff50f 147
gaku_sigu 0:a919993ff50f 148
gaku_sigu 0:a919993ff50f 149 bool Nunchuck::buttonc()
gaku_sigu 0:a919993ff50f 150 {
gaku_sigu 0:a919993ff50f 151 getdata();
gaku_sigu 0:a919993ff50f 152 return !(data[5] & 0x02);
gaku_sigu 0:a919993ff50f 153 }