#include <SPI.h>
// TIMING
#define MAXSAMPLE 1000 // Number of data samples:
#define CLAW_FS 100 // Control System Sample Rate (long) (Hz)
volatile boolean PING = false;
IntervalTimer clawtimer; // Timer Object for CLAW
void beacon()
{
PING = true;
}
boolean wow()
{
return PING;
}
// SPI Pins
#define READ 0x80
#define WRITE 0x00
#define MOSIX 11
#define MISOX 12
#define SCKX 13
#define SSX 10
SPISettings SETA(10e6, MSBFIRST, SPI_MODE0);
// IMU REGISTERS
uint8_t address_accelx = 0X28;
uint8_t address_accely = 0X2A;
uint8_t address_accelz = 0X2C;
uint8_t address_gyrox = 0X22;
uint8_t address_gyroy = 0X24;
uint8_t address_gyroz = 0X26;
// IMU COUNTS
int16_t caccx, caccy, caccz, cgyrox, cgyroy, cgyroz;
// STREAM VARIABLES
#define MAXLINE 136
char nmeac[MAXLINE];
void spi_initialize()
{
pinMode(SSX, OUTPUT);
digitalWrite(SSX,HIGH);
SPI.setMOSI(MOSIX);
SPI.setMISO(MISOX);
SPI.setSCK(SCKX);
SPI.begin();
}
uint8_t spi_read( SPISettings SETX, uint8_t target)
{
uint8_t out1;
uint8_t out2;
SPI.beginTransaction(SETX);
digitalWrite(SSX,LOW);
delayMicroseconds(1);
out1 = SPI.transfer(target | READ);
out2 = SPI.transfer(0x00);
digitalWrite(SSX,HIGH);
SPI.endTransaction();
return out2;
}
int16_t spi_read_16bit( SPISettings SETX, uint8_t target)
{
uint8_t out1, out2, out3;
int16_t out4;
SPI.beginTransaction(SETX);
digitalWrite(SSX,LOW);
delayMicroseconds(1);
out1 = SPI.transfer(target | READ);
out2 = SPI.transfer(0x00);
SPI.endTransaction();
SPI.beginTransaction(SETX);
delayMicroseconds(1);
out3 = SPI.transfer(0x00);
digitalWrite(SSX,HIGH);
SPI.endTransaction();
out4 = (int16_t)(out2 | (out3 << 8));
return out4;
}
void spi_write( SPISettings SETX, uint8_t target, uint8_t data2write)
{
SPI.beginTransaction(SETX);
digitalWrite(SSX, LOW); // Select chip
delayMicroseconds(1);
SPI.transfer(target | WRITE);
SPI.transfer(data2write);
digitalWrite(SSX, HIGH); // De-select chip
SPI.endTransaction();
}
void init_imu()
{
uint8_t address, target, writeout;
uint8_t result;
Serial.println("Who Am I: 101");
target = 0x0F; // WHO-AM-I BYTE
Serial.println(spi_read( SETA, target ));
Serial.println("Continuous or not");
target = 0X12;
//writeout = 0X00; // DO NOT USE; PREVENTS TEENSY FROM READING 2ND BYTE OF ACCEL OR GYRO 16-BIT READING
writeout = 0X04; // 00000100
spi_write( SETA, target, writeout);
Serial.println("Decimation:");
target = 0X08;
writeout = 0X09;
spi_write( SETA, target, writeout);
Serial.println("Accel Settings:");
target = 0X10;
writeout = 0X4E; // 0100 1110, odr: 104Hz, +/-8g, filter: 100hz
spi_write( SETA, target, writeout);
Serial.println("Gyro Settings:");
target = 0X11;
writeout = 0X48; // 0100 1000, odr: 104hz, 1000 dps
spi_write( SETA, target, writeout);
Serial.println("CTRL4_C (13h) :");
target = 0X13;
writeout = 0X80; // bandwidth determined by setting BW_XL[1:0]
spi_write( SETA, target, writeout);
}
void setup()
{
double microsec1;
int ii;
// INITIALIZE
Serial.begin(115200);
Serial.println("Hello:");
spi_initialize();
init_imu();
// TIMER STUFF
microsec1 = 1.0e6/CLAW_FS;
clawtimer.begin( beacon, microsec1 );
delayMicroseconds(1000000);
PING = false;
ii = 0;
while (ii<MAXSAMPLE)
{
// FORCE FIXED TIME STEP
while ( wow()!=true);
PING = false;
// GET IMU Measurements. Units: counts
//------------------------------------------
caccx = spi_read_16bit( SETA, address_accelx);
caccy = spi_read_16bit( SETA, address_accely);
caccz = spi_read_16bit( SETA, address_accelz);
cgyrox = spi_read_16bit( SETA, address_gyrox );
cgyroy = spi_read_16bit( SETA, address_gyroy );
cgyroz = spi_read_16bit( SETA, address_gyroz );
sprintf(nmeac, " %d %d %d %d %d %d \r\n", caccx, caccy, caccz, cgyrox, cgyroy, cgyroz );
Serial.print(nmeac);
ii++;
}
clawtimer.end();
}
void loop()
{
}