forked from mstrens/grbl_controller_esp32
-
Notifications
You must be signed in to change notification settings - Fork 0
/
nunchuk.cpp
224 lines (202 loc) · 8.01 KB
/
nunchuk.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
/**
* @license Nunchuk Arduino library v0.0.1 16/12/2016
* http://www.xarg.org/2016/12/arduino-nunchuk-library/
*
* Copyright (c) 2016, Robert Eisele ([email protected])
* Dual licensed under the MIT or GPL Version 2 licenses.
**/
#include <Wire.h>
#include "config.h"
#include "language.h"
#include "com.h"
#include "nunchuk.h"
#include "draw.h"
// Calibration joystick values
#define NUNCHUK_JOYSTICK_X_ZERO 127
#define NUNCHUK_JOYSTICK_Y_ZERO 128
// The Nunchuk I2C address
#define NUNCHUK_ADDRESS 0x52
#if ARDUINO >= 100
#define I2C_READ() Wire.read()
#define I2C_WRITE(x) Wire.write(x)
#else
#define I2C_READ() Wire.receive()
#define I2C_WRITE(x) Wire.send(x)
#endif
#define I2C_START(x) Wire.beginTransmission(x)
#define I2C_STOP() Wire.endTransmission(true)
uint8_t nunchuk_data[6];
int8_t prevMoveX = 0;
int8_t prevMoveY = 0;
int8_t prevMoveZ = 0;
int8_t prevMoveA = 0;
int8_t jogDistX = 0;
int8_t jogDistY = 0;
int8_t jogDistZ = 0;
int8_t jogDistA = 0;
float moveMultiplier ;
uint32_t cntSameMove = 0 ;
uint32_t startMoveMillis = 0;
uint8_t jog_status = JOG_NO ;
boolean jogCancelFlag = false ;
boolean jogCmdFlag = false ;
boolean nunchukOK ; // keep flag to detect a nunchuk at startup
extern char machineStatus[9];
//extern char lastMsg[80] ;
/**
* Initializes the Nunchuk communication by sending a sequence of bytes
*/
void nunchuk_init() {
Wire.begin();
// Change TWI speed for nunchuk, which uses Fast-TWI (400kHz)
// Normally this will be set in twi_init(), but this hack works without modifying the original source
Wire.setClock(400000);
delay(500); // delay ajouté pour donner le temps à la nunchuk de s'initialiser
Wire.beginTransmission(NUNCHUK_ADDRESS);
if ( Wire.endTransmission() ) { // return 0 if the I2C device replies without error
nunchukOK = false ;
fillMsg( __NO_NUNCHUK ) ;
} else {
nunchukOK = true ;
disableEncription() ;
nunchuk_read() ; // read once to (perhaps) avoid error message at start up
delay(20);
}
//disableEncription() ;
}
void disableEncription() {
I2C_START(NUNCHUK_ADDRESS); // disable encription with next 2 commands
I2C_WRITE(0xF0);
I2C_WRITE(0x55);
I2C_STOP();
I2C_START(NUNCHUK_ADDRESS);
I2C_WRITE(0xFB);
I2C_WRITE(0x00);
I2C_STOP();
}
/**
* Central function to read a full chunk of data from Nunchuk
*
* @return A boolean if the data transfer was successful
*/
uint8_t nunchuk_read() {
uint8_t i;
uint8_t error = 0 ;
Wire.requestFrom(NUNCHUK_ADDRESS, 6);
//delayMicroseconds(10);
for (i = 0; i < 6 && Wire.available(); i++) {
nunchuk_data[i] = I2C_READ();
if ( nunchuk_data[i] == 0 || nunchuk_data[i] == 0xFF) {
error |= (1 << i) ;
}
// Serial.print( (uint8_t) nunchuk_data[i] , HEX) ;
}
//Serial.println(" ");
error = ( error == 0b00111111 ); // error when all received bytes are 00 or FF.
if ( error ) {
disableEncription() ; // try to reactivate nunchunck in case of error.
}
I2C_START(NUNCHUK_ADDRESS);
I2C_WRITE(0x00);
I2C_STOP();
return !error ; // return true when there is no error
}
uint8_t nunchuk_buttonZ() { // Checks the current state of button Z
return (~nunchuk_data[5] >> 0) & 1;
}
uint8_t nunchuk_buttonC() { // Checks the current state of button C
return (~nunchuk_data[5] >> 1) & 1;
}
#define NUNCHUK_READ_DELAY 100 // read nunchuk every 100 msec (about)
//#define NUNCHUK_READ_DELAY_FIRST_CMD 5000
// (if statusPrinting = PRINTING_STOPPED and) if machineStatus= Idle or Jog, then if delay since previous read exceed 100msec, then read nunchuk data
// and if buttonZ or buttonC is released while it was pressed bebore, then send a command to cancel JOG (= char 0x85 )
// If button buttonZ or buttonC is pressed and if joysttick is moved, then send a jog command accordingly
// sending grbl jogging commands is done in com.cpp because it has to wait for "ok" after sending a command.
// There are 2 type of grbl commands : one for canceling and some for jogging
void handleNunchuk (void) {
uint32_t nunchukMillis = millis() ;
static uint32_t lastNunchukMillis = 0;
// static uint8_t previousButtonC = 0 ;
// static uint8_t previousButtonZ = 0 ;
// int8_t moveDx ;
// int8_t moveDy ;
int8_t moveX = 0 ; //static int8 prevMoveX = 0;
int8_t moveY = 0 ; //static int8 prevMoveY = 0;
int8_t moveZ = 0 ; //static int8 prevMoveZ = 0;
int8_t moveA = 0 ; //static int8 prevMoveZ = 0;
//float moveMultiplier ;
//Serial.print("handle=");Serial.println( machineStatus[0] , HEX);
if ( ( nunchukMillis - lastNunchukMillis ) > NUNCHUK_READ_DELAY ) { // we can not read to fast
lastNunchukMillis = nunchukMillis ;
if (nunchuk_read() ) {
//Serial.print(nunchuk_data[0],HEX); Serial.print(",");Serial.print(nunchuk_data[1],HEX); Serial.print(",");Serial.println(nunchuk_data[5] & 0x03 , HEX);
if ( nunchuk_buttonC() && nunchuk_buttonZ() == 0 ) { // si le bouton C est enfoncé mais pas le bouton Z
//Serial.print( ( int )nunchuk_data[0] ) ; Serial.print(" ") ; Serial.println( ( int )nunchuk_data[1] ) ;
if (nunchuk_data[0] < 80 ) {
moveX = - 1 ;
} else if (nunchuk_data[0] > 170 ) {
moveX = 1 ;
}
if (nunchuk_data[1] < 80 ) {
moveY = - 1 ;
} else if (nunchuk_data[1] > 170 ) {
moveY = 1 ;
}
} else if ( nunchuk_buttonZ() && nunchuk_buttonC() == 0 ) { // si le bouton Z est enfoncé mais pas le bouton C
#ifdef AA_AXIS
if (nunchuk_data[0] < 80 ) {
moveA = - 1 ;
} else if (nunchuk_data[0] > 170 ) {
moveA = 1 ;
}
#endif
if (nunchuk_data[1] < 80 ) {
moveZ = - 1 ;
} else if (nunchuk_data[1] > 170 ) {
moveZ = 1 ;
}
}
//if ( (machineStatus[0] == 'J' ) && ( ( prevMoveX != moveX) || ( prevMoveY != moveY) || ( prevMoveZ != moveZ) ) ) { // cancel Jog if jogging and t least one direction change
if ( (machineStatus[0] == 'J' || machineStatus[0] == 'I' ) && ( prevMoveX != moveX || prevMoveY != moveY || prevMoveZ != moveZ || prevMoveA != moveA) ) { // cancel Jog if jogging and at least one direction change
jogCancelFlag = true ;
cntSameMove = 0 ; // reset the counter
//Serial.println("cancel jog") ;
} else {
//jogCancelFlag = false ;
}
if ( moveX || moveY || moveZ || moveA ) { // if at least one move is asked
if (cntSameMove == 0 ) {
// moveMultiplier = 0.01 ;
startMoveMillis = millis() ;
}
//else if (cntSameMove < 5 ) { // avoid to send to fast a new move
// moveMultiplier = 0.0 ;
//} else if (cntSameMove < 10 ) {
// moveMultiplier = 0.01 ;
//} else if (cntSameMove < 15 ) {
// moveMultiplier = 0.1 ;
//} else if (cntSameMove < 20 ) {
// moveMultiplier = 1 ;
//} else {
// moveMultiplier = 4;
//}
cntSameMove++ ;
jogCmdFlag = true ;
jogDistX = moveX;
jogDistY = moveY;
jogDistZ = moveZ;
jogDistA = moveA;
} else { // no move asked ( moveX || moveY || moveZ || moveA)
cntSameMove = 0 ;
// moveMultiplier = 0 ; // put the value on 0 to avoid an old move to be execute ; let the flag to be reset by the com.cpp file after a OK being received
//jogCmdFlag = false ;
} // end if ( moveX || moveY || moveZ)
prevMoveX = moveX ;
prevMoveY = moveY ;
prevMoveZ = moveZ ;
prevMoveA = moveA ;
//Serial.print("cnt= ") ; Serial.println( cntSameMove ) ;
} // end of nunchukRead is true
} //end of test on delay
} // end handleNunchuk