کد:
// Test the ADNS3080 Optical Flow Sensor
#include "SPI.h"
#include "ADNS3080.h"
#define AP_SPI_DATAIN 12 //MISO
#define AP_SPI_DATAOUT 11 //MOSI
#define AP_SPI_CLOCK 13 //SCK
#define ADNS3080_CHIP_SELECT 10 //SS
#define ADNS3080_RESET 9 //RESET
byte orig_spi_settings_spcr;
byte orig_spi_settings_spsr;
int _cs_pin=ADNS3080_CHIP_SELECT;
int _reset_pin=1; // set to 1 if you have reset connected
unsigned int last_update;
boolean _overflow=false;
boolean _motion=false;
int raw_dx;
int raw_dy;
unsigned int surface_quality;
void setup()
{
Serial.begin(115200);
Serial.println("www.bot-thoughts.com\nOptical Flow test program V1.0\n");
delay(1000);
// flowSensor initialization
if( initOF() == false )
Serial.println("Failed to initialise ADNS3080");
delay(1000);
}
void loop()
{
int value;
display_menu();
// wait for user to enter something
while( !Serial.available() ) {
delay(20);
}
// get character from user
value = Serial.read();
switch( value ) {
case 'c' :
//display_config();
break;
case 'f' :
//set_frame_rate();
break;
case 'i' :
// display image
display_image();
break;
case 'I' :
display_image_continuously();
break;
case 'm' :
display_motion();
break;
case 'r' :
// set resolution
//set_resolution();
break;
case 's' :
//set_shutter_speed();
break;
case 'z' :
//flowSensor.clear_motion();
break;
case '\r' : // ignore return type characters
case '\n' :
break;
default:
Serial.println("unrecognised command");
Serial.println();
break;
}
}
// DISPLAY FUNCTIONS
// Prints out a list of functions.
void display_menu()
{
Serial.println();
Serial.println("please choose from the following options:");
// Serial.println(" c - display all config");
// Serial.println(" f - set frame rate");
Serial.println(" i - display image");
Serial.println(" I - display image continuously");
Serial.println(" m - display motion");
// Serial.println(" r - set resolution");
// Serial.println(" s - set shutter speed");
// Serial.println(" z - clear all motion");
// Serial.println(" a - frame rate auto/manual");
Serial.println();
}
// Captures and displays image from flowSensor
void display_image()
{
Serial.println("image data --------------");
print_pixel_data(&Serial);
Serial.println("-------------------------");
}
// display_image - captures and displays image from flowSensor flowSensor
void display_image_continuously()
{
int i;
Serial.println("press any key to return to menu");
Serial.flush();
while( !Serial.available() ) {
display_image();
i=0;
while( i<20 && !Serial.available() ) {
delay(100); // give the viewer a bit of time to catchup
i++;
}
}
Serial.flush();
}
// show x,y and squal values constantly until user presses a key
//
void display_motion()
{
boolean first_time = true;
Serial.flush();
// display instructions on how to exit
Serial.println("press x to return to menu..");
delay(1000);
while( !Serial.available() ) {
updateOF();
// check for errors
if( _overflow )
Serial.println("overflow!!");
// x,y,squal
Serial.print("dx: ");
Serial.print(raw_dx,DEC);
Serial.print("\tdy: ");
Serial.print(raw_dy,DEC);
Serial.print("\tsqual:");
Serial.print(surface_quality,DEC);
Serial.println();
first_time = false;
// short delay
delay(100);
}
// flush the serial
Serial.flush();
}
// ADNS3080 SPECIFIC FUNCTIONS
// reset sensor by holding a pin high (or is it low?) for 10us.
void reset()
{
// return immediately if the reset pin is not defined
if( _reset_pin == 0)
return;
digitalWrite(_reset_pin,HIGH); // reset sensor
delayMicroseconds(10);
digitalWrite(_reset_pin,LOW); // return sensor to normal
}
// Read a register from the sensor
byte read_register(byte address)
{
byte result = 0, junk = 0;
backup_spi_settings();
// take the chip select low to select the device
digitalWrite(_cs_pin, LOW);
// send the device the register you want to read:
junk = SPI.transfer(address);
// small delay
delayMicroseconds(50);
// send a value of 0 to read the first byte returned:
result = SPI.transfer(0x00);
// take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH);
restore_spi_settings();
return result;
}
// init - initialise sensor
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
boolean initOF()
{
int retry = 0;
pinMode(AP_SPI_DATAOUT,OUTPUT);
pinMode(AP_SPI_DATAIN,INPUT);
pinMode(AP_SPI_CLOCK,OUTPUT);
pinMode(_cs_pin,OUTPUT);
if( _reset_pin != 0)
pinMode(ADNS3080_RESET,OUTPUT);
digitalWrite(_cs_pin,HIGH); // disable device (Chip select is active low)
// reset the device
reset();
// start the SPI library:
SPI.begin();
// check the sensor is functioning
if( retry < 3 ) {
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
return true;
retry++;
}
return false;
}
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
//
byte backup_spi_settings()
{
// store current spi values
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
orig_spi_settings_spsr = SPSR & SPI2X;
// set the values that we need
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
return orig_spi_settings_spcr;
}
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
byte restore_spi_settings()
{
byte temp;
// restore SPSR
temp = SPSR;
temp &= ~SPI2X;
temp |= orig_spi_settings_spsr;
SPSR = temp;
// restore SPCR
temp = SPCR;
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
temp |= orig_spi_settings_spcr; // restore important bits
SPCR = temp;
return temp;
}
// write a value to one of the sensor's registers
void write_register(byte address, byte value)
{
byte junk = 0;
backup_spi_settings();
// take the chip select low to select the device
digitalWrite(_cs_pin, LOW);
// send register address
junk = SPI.transfer(address | 0x80 );
// small delay
delayMicroseconds(50);
// send data
junk = SPI.transfer(value);
// take the chip select high to de-select:
digitalWrite(_cs_pin, HIGH);
restore_spi_settings();
}
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
void print_pixel_data(Stream *serPort)
{
int i,j;
boolean isFirstPixel = true;
byte regValue;
byte pixelValue;
// write to frame capture register to force capture of frame
write_register(ADNS3080_FRAME_CAPTURE,0x83);
// wait 3 frame periods + 10 nanoseconds for frame to be captured
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
// display the pixel data
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
regValue = read_register(ADNS3080_FRAME_CAPTURE);
if( isFirstPixel && (regValue & 0x40) == 0 ) {
serPort->println("failed to find first pixel");
}
isFirstPixel = false;
pixelValue = ( regValue << 2);
serPort->print(pixelValue,DEC);
if( j!= ADNS3080_PIXELS_X-1 )
serPort->print(",");
delayMicroseconds(50);
}
serPort->println();
}
// hardware reset to restore sensor to normal operation
reset();
}
bool updateOF()
{
byte motion_reg;
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
delayMicroseconds(50); // small delay
// check for movement, update x,y values
motion_reg = read_register(ADNS3080_MOTION);
_overflow = ((motion_reg & 0x10) != 0); // check if we've had an overflow
if( (motion_reg & 0x80) != 0 ) {
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
delayMicroseconds(50); // small delay
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
_motion = true;
}else{
raw_dx = 0;
raw_dy = 0;
}
last_update = millis();
return true;
}
LinkBack URL
About LinkBacks
ماژول سنسور جریان نوری - Optical Fllow ویژه APM
پاسخ با نقل قول
