-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathmousecam.ino
239 lines (204 loc) · 5.75 KB
/
mousecam.ino
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
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
#include "SPI.h"
// these pins may be different on different boards
// this is for the uno
#define PIN_SS 10
#define PIN_MISO 12
#define PIN_MOSI 11
#define PIN_SCK 13
#define PIN_MOUSECAM_RESET 3
#define PIN_MOUSECAM_CS 2
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_PRODUCT_ID 0x00
#define ADNS3080_REVISION_ID 0x01
#define ADNS3080_MOTION 0x02
#define ADNS3080_DELTA_X 0x03
#define ADNS3080_DELTA_Y 0x04
#define ADNS3080_SQUAL 0x05
#define ADNS3080_PIXEL_SUM 0x06
#define ADNS3080_MAXIMUM_PIXEL 0x07
#define ADNS3080_CONFIGURATION_BITS 0x0a
#define ADNS3080_EXTENDED_CONFIG 0x0b
#define ADNS3080_DATA_OUT_LOWER 0x0c
#define ADNS3080_DATA_OUT_UPPER 0x0d
#define ADNS3080_SHUTTER_LOWER 0x0e
#define ADNS3080_SHUTTER_UPPER 0x0f
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
#define ADNS3080_MOTION_CLEAR 0x12
#define ADNS3080_FRAME_CAPTURE 0x13
#define ADNS3080_SROM_ENABLE 0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
#define ADNS3080_SROM_ID 0x1f
#define ADNS3080_OBSERVATION 0x3d
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
#define ADNS3080_PIXEL_BURST 0x40
#define ADNS3080_MOTION_BURST 0x50
#define ADNS3080_SROM_LOAD 0x60
#define ADNS3080_PRODUCT_ID_VAL 0x17
void mousecam_reset()
{
digitalWrite(PIN_MOUSECAM_RESET,HIGH);
delay(1); // reset pulse >10us
digitalWrite(PIN_MOUSECAM_RESET,LOW);
delay(35); // 35ms from reset to functional
}
int mousecam_init()
{
pinMode(PIN_MOUSECAM_RESET,OUTPUT);
pinMode(PIN_MOUSECAM_CS,OUTPUT);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
mousecam_reset();
int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
if(pid != ADNS3080_PRODUCT_ID_VAL)
return -1;
// turn on sensitive mode
mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);
return 0;
}
void mousecam_write_reg(int reg, int val)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg | 0x80);
SPI.transfer(val);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(50);
}
int mousecam_read_reg(int reg)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg);
delayMicroseconds(75);
int ret = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(1);
return ret;
}
struct MD
{
byte motion;
char dx, dy;
byte squal;
word shutter;
byte max_pix;
};
void mousecam_read_motion(struct MD *p)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_MOTION_BURST);
delayMicroseconds(75);
p->motion = SPI.transfer(0xff);
p->dx = SPI.transfer(0xff);
p->dy = SPI.transfer(0xff);
p->squal = SPI.transfer(0xff);
p->shutter = SPI.transfer(0xff)<<8;
p->shutter |= SPI.transfer(0xff);
p->max_pix = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(5);
}
// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
// you must call mousecam_reset() after this if you want to go back to normal operation
int mousecam_frame_capture(byte *pdata)
{
mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83);
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_PIXEL_BURST);
delayMicroseconds(50);
int pix;
byte started = 0;
int count;
int timeout = 0;
int ret = 0;
for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; )
{
pix = SPI.transfer(0xff);
delayMicroseconds(10);
if(started==0)
{
if(pix&0x40)
started = 1;
else
{
timeout++;
if(timeout==100)
{
ret = -1;
break;
}
}
}
if(started==1)
{
pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range
}
}
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(14);
return ret;
}
void setup()
{
pinMode(PIN_SS,OUTPUT);
pinMode(PIN_MISO,INPUT);
pinMode(PIN_MOSI,OUTPUT);
pinMode(PIN_SCK,OUTPUT);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32);
SPI.setDataMode(SPI_MODE3);
SPI.setBitOrder(MSBFIRST);
Serial.begin(38400);
if(mousecam_init()==-1)
{
Serial.println("Mouse cam failed to init");
while(1);
}
}
char asciiart(int k)
{
static char foo[] = "WX86*3I>!;~:,`. ";
return foo[k>>4];
}
byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];
void loop()
{
#if 0
// if enabled this section grabs frames and outputs them as ascii art
if(mousecam_frame_capture(frame)==0)
{
int i,j,k;
for(i=0, k=0; i<ADNS3080_PIXELS_Y; i++)
{
for(j=0; j<ADNS3080_PIXELS_X; j++, k++)
{
Serial.print(asciiart(frame[k]));
Serial.print(' ');
}
Serial.println();
}
}
Serial.println();
delay(250);
#else
// if enabled this section produces a bar graph of the surface quality that can be used to focus the camera
// also drawn is the average pixel value 0-63 and the shutter speed and the motion dx,dy.
int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
MD md;
mousecam_read_motion(&md);
for(int i=0; i<md.squal/4; i++)
Serial.print('*');
Serial.print(' ');
Serial.print((val*100)/351);
Serial.print(' ');
Serial.print(md.shutter); Serial.print(" (");
Serial.print((int)md.dx); Serial.print(',');
Serial.print((int)md.dy); Serial.println(')');
// Serial.println(md.max_pix);
delay(100);
#endif
}