Project MODI

A simple obstacle avoidance robot using an IR distance sensor with an IMU.

Mar 3, 2021

•

5009 views

•

1 respects

Components and supplies

Arduino MotorShield Rev3

Female/Female Jumper Wires

Box-Mini Robot Chassis Kit

MODI Shield IR Distance Sensor

Machine Screw, M3

Arduino UNO

Tools and machines

Wire Stripper & Cutter, 18-10 AWG / 0.75-4mm² Capacity Wires

Project description

Code

modi shield_boxmini code

arduino

The code is a tweaked version of the example MODI Shield code downloaded from Dweebots.com. I simply added a routine to drive the DC motors and make decisions based on the data from the IMU.

1///////////////////////////////////////////////////////////////////////////////////////////////////////
2//   Include
3///////////////////////////////////////////////////////////////////////////////////////////////////////
4
5#include   <Wire.h>
6
7///////////////////////////////////////////////////////////////////////////////////////////////////////
8//   Define
9///////////////////////////////////////////////////////////////////////////////////////////////////////
10
11#define   MODI_I2C_ADDR               (0xA8 >> 1)         // MODIShield I2C 8-bit Slave Address
12
13/*   *** GPIO Map ***
14MODIShield Pin --------- Arduino Uno Pin
15INT                      2
16SDA                       A4
17SCL                      A5
18 */
19#define MODI_INT_PIN                 2                   // MODIShield Interrupt pin
20#define LED_PIN                      13                  // Arduino Uno Onboard LED pin
21
22//   Weight for fractional bits of accelerometer conversion
23#define FRAC_2d1                  5000   // 2^9
24#define FRAC_2d2                  2500  // 2^8
25#define FRAC_2d3                  1250   // 2^7
26#define FRAC_2d4                  625   // 2^6
27#define FRAC_2d5                  313    // 2^5
28#define FRAC_2d6                  156   // 2^4
29#define FRAC_2d7                   78    // 2^3
30#define FRAC_2d8                  39    // 2^2
31#define   FRAC_2d9                  20    // 2^1
32#define FRAC_2d10                 10    //   2^0
33
34// MODI Register Map Addr
35#define WHO_AM_I_REG                0
36#define   DEV_INFO_REG                1
37#define ADDR_PTR_REG                2
38#define   CMD_REG                     3
39#define STAT_REG                    4
40#define   DIST_LSB_REG                5
41#define DIST_MSB_REG                6
42#define   AX_LSB_REG                  7
43#define AX_MSB_REG                  8
44#define   AY_LSB_REG                  9
45#define AY_MSB_REG                  10
46#define   AZ_LSB_REG                  11
47#define AZ_MSB_REG                  12
48#define   SAMP_PER_REG                13
49#define ACCEL_FS_SEL_REG            14
50
51//   MODI Register Bit Map
52
53// Command Register
54#define CMD_SLA_REL                 0x01
55#define   CMD_SW_RST                  0x02
56#define CMD_ST_DAT                  0x03
57#define   CMD_SP_DAT                  0x04
58
59// Accelerometer Full-Scale Select Register
60#define   ACCEL_FS_2G                 1
61#define ACCEL_FS_4G                 2
62#define   ACCEL_FS_8G                 3
63
64#define TRUE                        1
65#define   FALSE                       0
66
67#define I2C_STOP                    TRUE
68#define   I2C_RPT_START               FALSE
69
70// Wire.endTransmission() return val
71#define   TX_SUCCESS                  0
72#define DATA_BUFF_OVFLW             1
73#define   SLA_ADDR_NACK               2
74#define DATA_NACK                   3
75#define   OTHER_ERR                   4
76
77// Running Average Array Size
78#define SAMP_ARR_SZ                  5
79
80//Motor Definitions
81#define MTR_DIR_A                    12
82#define   MTR_DIR_B                    13
83#define MTR_PWM_A                    3
84#define   MTR_PWM_B                    11
85#define MTR_BRAKE_A                  9
86#define   MTR_BRAKE_B                  8
87#define FORWARD                     LOW
88#define   REVERSE                      HIGH
89#define MTR_A                       MTR_PWM_A
90#define   MTR_B                       MTR_PWM_B
91
92///////////////////////////////////////////////////////////////////////////////////////////////////////
93//   Typedefs
94///////////////////////////////////////////////////////////////////////////////////////////////////////
95
96typedef   unsigned char       uint8;
97typedef unsigned short      uint16;
98typedef signed   short        int16;
99
100///////////////////////////////////////////////////////////////////////////////////////////////////////
101//   Global Variables
102///////////////////////////////////////////////////////////////////////////////////////////////////////
103
104volatile   bool              modiInt = FALSE;
105uint8                      modiDataBytes[8];
106float                       accelX_f = 0, accelY_f = 0, accelZ_f = 0;
107float                      distCm_f   = 0;
108float                      sampleArr[SAMP_ARR_SZ];
109float                      distCm_favg   = 0;
110uint8                      accelFSRange = 0;
111
112///////////////////////////////////////////////////////////////////////////////////////////////////////
113//   Prototypes
114///////////////////////////////////////////////////////////////////////////////////////////////////////
115
116void   configPins( void );
117void configInterrupts( void );
118
119void modiInit( uint8   accelFs );
120uint8 setAccelFullscale( uint8 fs );
121uint8 modiWriteReg( uint8   regAddr, uint8 regData );
122uint8 modiWriteRegBlock( uint8 regAddr, uint8 numBytes,   uint8 *pBuff );
123uint8 modiReadReg( uint8 regAddr );
124bool modiReadRegBlock(   uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff );
125uint8 modiSendCommand( uint8   cmd );
126
127bool getModiData( void );
128float modiBytes2Distance( uint8 *pBytes   );
129float modiBytes2Accel( uint8 fsMode, uint8 msb , uint8 lsb );
130float runningAvg(   float *pSampArr, uint8 sampArrSz, float samp );
131void printModiData( void );
132void   ModiShieldISR( void );
133
134void drive(uint8 motor_speed, bool motor_direction,uint8   motor_channel);
135
136
137///////////////////////////////////////////////////////////////////////////////////////////////////////
138//   Main
139///////////////////////////////////////////////////////////////////////////////////////////////////////
140
141void   setup() 
142{
143  configPins();         // Configure GPIOs
144  Wire.begin();         //   Join I2C bus as master
145  
146  pinMode(SDA, INPUT);  // Disable internal Pull-up   resistors for I2C SDA and SCL lines
147  pinMode(SCL, INPUT);
148  
149  Serial.begin(9600);               // Start serial terminal
150  modiInit(ACCEL_FS_2G);           //   Init MODIShield
151  configInterrupts();              // Configure Interrupts
152   //drive(125,FORWARD,MTR_A);
153  //drive(125,FORWARD,MTR_B);
154}
155
156void   loop() 
157{
158  if( modiInt )                           // Check if MODI has generated   an interrupt
159  {
160    if( getModiData() )                   // Read all eight   data registers, convert to floats
161    {                    
162      distCm_favg   = runningAvg( sampleArr, SAMP_ARR_SZ, distCm_f ); // Perform running average on   distance measurement
163      printModiData();                    // Print data   to COM port
164      if (accelZ_f>= .6 || accelZ_f <= -.6 || accelY_f <= -.6 ||   accelY_f >= .6)
165      {
166         drive(0,FORWARD,MTR_A);  //stop motors
167          drive(0,FORWARD,MTR_B);
168         delay(3000);
169      }
170      else
171       {
172        if (distCm_f < 20 )
173        {
174          drive(150,REVERSE,MTR_A);
175           drive(150,FORWARD,MTR_B);
176          //delay(500);
177        }
178         /*else if (distCm_f < 20)
179        {
180          drive(126,REVERSE,MTR_A);
181           drive(126,REVERSE,MTR_B);
182        }
183        */
184        else if   (distCm_f > 20)
185        {
186          drive(160,FORWARD,MTR_A);
187          drive(160,FORWARD,MTR_B);
188         }       
189      }
190    }
191    modiSendCommand( CMD_SLA_REL );       //   TX slave release command to MODI via I2C
192    modiInt = FALSE;                      //   Clear app-lvl interrupt flag
193  }
194  //delay(250);                             //   Delay (for readability)
195}
196
197///////////////////////////////////////////////////////////////////////////////////////////////////////
198//   Functions
199///////////////////////////////////////////////////////////////////////////////////////////////////////
200void   drive(uint8 motor_speed, bool motor_dir,uint8 motor_channel)
201{
202  if (motor_channel   == MTR_A)
203  {
204    digitalWrite(MTR_DIR_A, motor_dir);
205    analogWrite(MTR_PWM_A,   motor_speed);
206  }
207  else if (motor_channel == MTR_B)
208  {
209    digitalWrite(MTR_DIR_B,   motor_dir);
210    analogWrite(MTR_PWM_B, motor_speed);
211  }
212}
213
214
215
216float   runningAvg( float *pSampArr, uint8 sampArrSz, float samp )
217{
218    uint8 i =   0;
219    float avg = 0;
220
221    // Sum last "arrSize" number of samples
222     for ( i = 0; i < sampArrSz; i++ )
223    {
224       avg += pSampArr[i];
225     }
226
227    // Add current sample to sum
228    avg += samp;
229    // Divide   by array size plus 1 (to accomodate addition of current sample)
230    avg = avg   / (sampArrSz + 1);
231
232
233    // Shift out oldest sample
234    for (i = (sampArrSz-1);   i > 0 ; i-- )
235    {
236        pSampArr[i] = pSampArr[i - 1];
237    }
238
239     // Shift in newest sample
240    pSampArr[0] = samp;
241            
242    return   avg;
243  
244} // runningAvg
245
246uint8 setAccelFullscale( uint8 fs )
247{
248   uint8 retVal =  modiWriteReg( ACCEL_FS_SEL_REG, fs );
249
250  if( retVal == TX_SUCCESS   )
251  {
252    accelFSRange = fs;
253  }
254    
255  return retVal;
256  
257}   // setAccelFullscale
258
259bool getModiData( void )
260{
261  bool retVal = modiReadRegBlock(   DIST_LSB_REG, 8, modiDataBytes );           // Read data bytes for distance, accel   XYZ
262  if( retVal )
263  {                                                                           //   Convert rx'd bytes to floats
264      distCm_f = modiBytes2Distance( modiDataBytes   + 0 );                     // cm  
265      accelX_f = modiBytes2Accel( accelFSRange,   modiDataBytes[3], modiDataBytes[2] );    // G
266      accelY_f = modiBytes2Accel(   accelFSRange, modiDataBytes[5], modiDataBytes[4] );    // G
267      accelZ_f =   modiBytes2Accel( accelFSRange, modiDataBytes[7], modiDataBytes[6] );    // G
268   }
269
270  return retVal;
271  
272} // getModiData
273
274void printModiData(   void )
275{
276    Serial.print("D(cm): ");        // Print Distance
277    Serial.print(distCm_favg,2);
278     
279    Serial.print("  AX(g): ");      // Print Accel X
280    Serial.print(accelX_f,4);
281
282     Serial.print("  AY(g): ");      // Print Accel Y
283    Serial.print(accelY_f,4);
284
285     Serial.print("  AZ(g): ");      // Print Accel Z
286    Serial.println(accelZ_f,4);
287     Serial.println("***********************************************");
288    
289}   // printModiData
290
291float modiBytes2Distance( uint8 *pBytes )
292{
293  uint16   distCode = ( ( (uint16)pBytes[1] ) << 8 ) + pBytes[0];   // concatenate bytes
294   float dist = ( (float)distCode )*0.00050813*100.0;              // compute distance
295   return dist;                                                    // return result   in cm
296  
297} // modiBytes2Distance
298
299void modiInit( uint8 accelFs )
300{
301   modiSendCommand( CMD_SW_RST );
302  Serial.println("Initializing MODIShield...");    
303  delay(1000);
304  
305  uint8 rxBuff = 0;
306  
307  rxBuff = modiReadReg(   WHO_AM_I_REG );     // Read Who Am I Register
308  Serial.print("Who am I: 0x");              // Print Who Am I ID to COM port
309  Serial.println(rxBuff, HEX);    
310  rxBuff = modiReadReg( DEV_INFO_REG );     // Read Device Info Register
311   Serial.print("Device Info: 0x");          // Print Device Info to COM port
312   Serial.println(rxBuff, HEX);
313
314  if( modiReadReg( STAT_REG ) == 0x01 )     //   Read MODIShield Status Register
315  {
316    Serial.println("MODIShield Initialization:   SUCCESS");
317  }
318  else
319  {
320    Serial.println("MODIShield Initialization:   FAILURE");
321    modiSendCommand( CMD_SW_RST );          // If device Init falied,   perform SW reset on MODIShield
322  }
323  
324  Serial.print("Config Accel Full-scale   range...");   // Configure Accelerometer full-scale range
325  
326  if( setAccelFullscale(   accelFs ) == TX_SUCCESS )
327  {
328    Serial.println("SUCCESS");
329  }
330   else
331  {
332    Serial.println("FAILURE");
333  }
334
335  while( modiSendCommand(   CMD_ST_DAT ) != TX_SUCCESS )    //  Send start data collection command
336  {
337     delay(100);
338  }
339  
340} // modiInit
341
342uint8 modiSendCommand( uint8   cmd )
343{
344   return modiWriteReg( CMD_REG, cmd );
345   
346} // modiSendCommand
347
348uint8   modiWriteReg( uint8 regAddr, uint8 regData )
349{
350  uint8 txBuff[2] = {regAddr,   regData};
351  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to   MODIShield
352  Wire.write( txBuff, 2 );                            // Pass outgoing   bytes to Wire lib API
353  return Wire.endTransmission(I2C_STOP);              //   TX bytes, send STOP condition (release I2C bus) and return comm status
354} // modiWriteReg
355
356uint8   modiWriteRegBlock( uint8 regAddr, uint8 numBytes, uint8 *pBuff )
357{
358  Wire.beginTransmission(MODI_I2C_ADDR);               // Transmit to MODIShield
359  Wire.write( pBuff, numBytes );                      //   Pass outgoing bytes to Wire lib API
360  return Wire.endTransmission(I2C_STOP);               // TX bytes, send STOP condition (release I2C bus) and return comm   status
361} // modiWriteRegBlock
362
363uint8 modiReadReg( uint8 regAddr )
364{
365   uint8 regData = 0;
366  if ( modiWriteReg( ADDR_PTR_REG, regAddr ) == TX_SUCCESS   )
367  {
368      Wire.requestFrom( MODI_I2C_ADDR, 1 );    // Request 1 byte
369       
370      while( Wire.available() )              // slave may send less than   requested
371      {   
372          regData = Wire.read();             // RX byte
373       }
374  }
375  return regData;
376  
377} // modiReadReg
378
379bool modiReadRegBlock(   uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff )
380{
381  bool retVal = FALSE;
382   
383  if ( modiWriteReg( ADDR_PTR_REG, stRegAddr ) == TX_SUCCESS )
384  {
385       Wire.requestFrom(MODI_I2C_ADDR, (int)numBytes );    // Request 'numBytes'   number of bytes
386      uint8 i = 0; 
387      while( Wire.available() )                     //   slave may send less than requested
388      {   
389          pRxBuff[i] = Wire.read();                  // RX byte
390          i++;                                      //   Inc RX buff index
391          if( i == numBytes )                       // Break   from loop if buffer size exceeded
392            break;
393      }
394
395      retVal   = TRUE;
396  }
397
398  return retVal;
399  
400} // modiReadRegBlock
401
402void   configPins( void )
403{
404  pinMode( MODI_INT_PIN, INPUT );           // Configure   MODIShield Interrupt pin as input ( with internal pull-up resistor disabled )
405   pinMode(LED_PIN, OUTPUT);                 // Set LED pin as OUTPUT          
406   digitalWrite(LED_PIN, LOW);               // Init LED OFF
407  pinMode(MTR_DIR_A,OUTPUT);
408   pinMode(MTR_DIR_B,OUTPUT);
409  pinMode(MTR_PWM_A, OUTPUT);
410  pinMode(MTR_PWM_B,   OUTPUT);
411  pinMode(MTR_BRAKE_A, OUTPUT);
412  pinMode(MTR_BRAKE_B, OUTPUT);
413   digitalWrite(MTR_BRAKE_A,LOW);
414  digitalWrite(MTR_BRAKE_B,LOW);
415  
416  
417   
418  
419} // configPins
420
421void configInterrupts( void )
422{
423  attachInterrupt(digitalPinToInterrupt(MODI_INT_PIN),   ModiShieldISR, FALLING);
424  
425} // configInterrupts
426
427float modiBytes2Accel(   uint8 fsMode, uint8 msb , uint8 lsb )
428{
429  uint16 integerAccum = 0;
430  uint16   fracAccum = 0;             // Fractional Accumulator
431  uint16 accelWord_frac   = 0;        // Fractional portion of accel word
432  uint8 accelWord_int = 0;
433   
434  float fOut = 0.0;
435  bool neg = FALSE;
436  
437  uint16 accelWord = (   (uint16)msb ) << 8;      // Concatenate MSByte and LSByte into single word
438  accelWord   |= lsb;
439  accelWord &= 0xFFF0;                          // Mask out lower nibble   of LSByte register ( will be all zeros anyway since conversion result is left-justified   )
440
441  if( msb > 0x7F )                              // If negative, perform   2's complement
442  {
443    accelWord = ( ~accelWord ) + 1;
444    neg = TRUE;
445   }
446
447  if( fsMode == ACCEL_FS_2G )           // 2G
448  {
449     accelWord_int   = ( accelWord >> 14 ) & 0x01;
450     accelWord_frac = accelWord << 2;   // Shift   out sign and integer bits (remaining frac bits are now left-justified)
451  }
452   else if( fsMode == ACCEL_FS_4G )      // 4G
453  {
454    accelWord_int = ( accelWord   >> 13 ) & 0x03;
455    accelWord_frac = accelWord << 3;    // Shift out sign and   integer bits (remaining frac bits are now left-justified)
456  }
457  else                                  //   8G
458  {
459    accelWord_int = ( accelWord >> 12 ) & 0x07;
460    accelWord_frac   = accelWord << 4;    // Shift out sign and integer bits (remaining frac bits are   now left-justified)
461  }
462
463  // Determine fractional portion
464  if( accelWord_frac   & 0x8000 )
465     fracAccum += FRAC_2d1;
466  if( accelWord_frac & 0x4000 )
467      fracAccum += FRAC_2d2;  
468  if( accelWord_frac & 0x2000 )
469     fracAccum   += FRAC_2d3;
470  if( accelWord_frac & 0x1000 )
471     fracAccum += FRAC_2d4;
472   if( accelWord_frac & 0x0800 )
473     fracAccum += FRAC_2d5;  
474  if( accelWord_frac   & 0x0400 )
475     fracAccum += FRAC_2d6;
476  if( accelWord_frac & 0x0200 )
477      fracAccum += FRAC_2d7;    
478  if( accelWord_frac & 0x0100 )
479     fracAccum   += FRAC_2d8;
480
481  if( ( fsMode != ACCEL_FS_8G ) && ( accelWord_frac & 0x0080   )  )
482  {
483     fracAccum += FRAC_2d9;
484  }
485
486  if( ( fsMode == ACCEL_FS_2G   ) && ( accelWord_frac & 0x0040 )  )
487  {
488     fracAccum += FRAC_2d10;
489  }
490
491   fOut = ( (float)fracAccum ) / 10000.0;        // Convert to g's (1g = 9.81 m/s^2)
492   fOut += ( (float)accelWord_int );
493   
494  if( neg )                                    //   If number was negative, convert float to a negative number as well
495  {
496    fOut   = fOut*-1.0;
497  }
498  
499  return fOut;
500
501} // modiBytes2Accel
502
503///////////////////////////////////////////////////////////////////////////////////////////////////////
504//   Interrupt Service Routines
505///////////////////////////////////////////////////////////////////////////////////////////////////////
506
507void   ModiShieldISR( void ) 
508{
509  modiInt = TRUE;       // Set app-level interrupt   flag
510}
511

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Include
///////////////////////////////////////////////////////////////////////////////////////////////////////

#include   <Wire.h>

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Define
///////////////////////////////////////////////////////////////////////////////////////////////////////

#define   MODI_I2C_ADDR               (0xA8 >> 1)         // MODIShield I2C 8-bit Slave Address

/*   *** GPIO Map ***
MODIShield Pin --------- Arduino Uno Pin
INT                      2
SDA                       A4
SCL                      A5
 */
#define MODI_INT_PIN                 2                   // MODIShield Interrupt pin
#define LED_PIN                      13                  // Arduino Uno Onboard LED pin

//   Weight for fractional bits of accelerometer conversion
#define FRAC_2d1                  5000   // 2^9
#define FRAC_2d2                  2500  // 2^8
#define FRAC_2d3                  1250   // 2^7
#define FRAC_2d4                  625   // 2^6
#define FRAC_2d5                  313    // 2^5
#define FRAC_2d6                  156   // 2^4
#define FRAC_2d7                   78    // 2^3
#define FRAC_2d8                  39    // 2^2
#define   FRAC_2d9                  20    // 2^1
#define FRAC_2d10                 10    //   2^0

// MODI Register Map Addr
#define WHO_AM_I_REG                0
#define   DEV_INFO_REG                1
#define ADDR_PTR_REG                2
#define   CMD_REG                     3
#define STAT_REG                    4
#define   DIST_LSB_REG                5
#define DIST_MSB_REG                6
#define   AX_LSB_REG                  7
#define AX_MSB_REG                  8
#define   AY_LSB_REG                  9
#define AY_MSB_REG                  10
#define   AZ_LSB_REG                  11
#define AZ_MSB_REG                  12
#define   SAMP_PER_REG                13
#define ACCEL_FS_SEL_REG            14

//   MODI Register Bit Map

// Command Register
#define CMD_SLA_REL                 0x01
#define   CMD_SW_RST                  0x02
#define CMD_ST_DAT                  0x03
#define   CMD_SP_DAT                  0x04

// Accelerometer Full-Scale Select Register
#define   ACCEL_FS_2G                 1
#define ACCEL_FS_4G                 2
#define   ACCEL_FS_8G                 3

#define TRUE                        1
#define   FALSE                       0

#define I2C_STOP                    TRUE
#define   I2C_RPT_START               FALSE

// Wire.endTransmission() return val
#define   TX_SUCCESS                  0
#define DATA_BUFF_OVFLW             1
#define   SLA_ADDR_NACK               2
#define DATA_NACK                   3
#define   OTHER_ERR                   4

// Running Average Array Size
#define SAMP_ARR_SZ                  5

//Motor Definitions
#define MTR_DIR_A                    12
#define   MTR_DIR_B                    13
#define MTR_PWM_A                    3
#define   MTR_PWM_B                    11
#define MTR_BRAKE_A                  9
#define   MTR_BRAKE_B                  8
#define FORWARD                     LOW
#define   REVERSE                      HIGH
#define MTR_A                       MTR_PWM_A
#define   MTR_B                       MTR_PWM_B

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Typedefs
///////////////////////////////////////////////////////////////////////////////////////////////////////

typedef   unsigned char       uint8;
typedef unsigned short      uint16;
typedef signed   short        int16;

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Global Variables
///////////////////////////////////////////////////////////////////////////////////////////////////////

volatile   bool              modiInt = FALSE;
uint8                      modiDataBytes[8];
float                       accelX_f = 0, accelY_f = 0, accelZ_f = 0;
float                      distCm_f   = 0;
float                      sampleArr[SAMP_ARR_SZ];
float                      distCm_favg   = 0;
uint8                      accelFSRange = 0;

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Prototypes
///////////////////////////////////////////////////////////////////////////////////////////////////////

void   configPins( void );
void configInterrupts( void );

void modiInit( uint8   accelFs );
uint8 setAccelFullscale( uint8 fs );
uint8 modiWriteReg( uint8   regAddr, uint8 regData );
uint8 modiWriteRegBlock( uint8 regAddr, uint8 numBytes,   uint8 *pBuff );
uint8 modiReadReg( uint8 regAddr );
bool modiReadRegBlock(   uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff );
uint8 modiSendCommand( uint8   cmd );

bool getModiData( void );
float modiBytes2Distance( uint8 *pBytes   );
float modiBytes2Accel( uint8 fsMode, uint8 msb , uint8 lsb );
float runningAvg(   float *pSampArr, uint8 sampArrSz, float samp );
void printModiData( void );
void   ModiShieldISR( void );

void drive(uint8 motor_speed, bool motor_direction,uint8   motor_channel);


///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Main
///////////////////////////////////////////////////////////////////////////////////////////////////////

void   setup() 
{
  configPins();         // Configure GPIOs
  Wire.begin();         //   Join I2C bus as master
  
  pinMode(SDA, INPUT);  // Disable internal Pull-up   resistors for I2C SDA and SCL lines
  pinMode(SCL, INPUT);
  
  Serial.begin(9600);               // Start serial terminal
  modiInit(ACCEL_FS_2G);           //   Init MODIShield
  configInterrupts();              // Configure Interrupts
   //drive(125,FORWARD,MTR_A);
  //drive(125,FORWARD,MTR_B);
}

void   loop() 
{
  if( modiInt )                           // Check if MODI has generated   an interrupt
  {
    if( getModiData() )                   // Read all eight   data registers, convert to floats
    {                    
      distCm_favg   = runningAvg( sampleArr, SAMP_ARR_SZ, distCm_f ); // Perform running average on   distance measurement
      printModiData();                    // Print data   to COM port
      if (accelZ_f>= .6 || accelZ_f <= -.6 || accelY_f <= -.6 ||   accelY_f >= .6)
      {
         drive(0,FORWARD,MTR_A);  //stop motors
          drive(0,FORWARD,MTR_B);
         delay(3000);
      }
      else
       {
        if (distCm_f < 20 )
        {
          drive(150,REVERSE,MTR_A);
           drive(150,FORWARD,MTR_B);
          //delay(500);
        }
         /*else if (distCm_f < 20)
        {
          drive(126,REVERSE,MTR_A);
           drive(126,REVERSE,MTR_B);
        }
        */
        else if   (distCm_f > 20)
        {
          drive(160,FORWARD,MTR_A);
          drive(160,FORWARD,MTR_B);
         }       
      }
    }
    modiSendCommand( CMD_SLA_REL );       //   TX slave release command to MODI via I2C
    modiInt = FALSE;                      //   Clear app-lvl interrupt flag
  }
  //delay(250);                             //   Delay (for readability)
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Functions
///////////////////////////////////////////////////////////////////////////////////////////////////////
void   drive(uint8 motor_speed, bool motor_dir,uint8 motor_channel)
{
  if (motor_channel   == MTR_A)
  {
    digitalWrite(MTR_DIR_A, motor_dir);
    analogWrite(MTR_PWM_A,   motor_speed);
  }
  else if (motor_channel == MTR_B)
  {
    digitalWrite(MTR_DIR_B,   motor_dir);
    analogWrite(MTR_PWM_B, motor_speed);
  }
}



float   runningAvg( float *pSampArr, uint8 sampArrSz, float samp )
{
    uint8 i =   0;
    float avg = 0;

    // Sum last "arrSize" number of samples
     for ( i = 0; i < sampArrSz; i++ )
    {
       avg += pSampArr[i];
     }

    // Add current sample to sum
    avg += samp;
    // Divide   by array size plus 1 (to accomodate addition of current sample)
    avg = avg   / (sampArrSz + 1);


    // Shift out oldest sample
    for (i = (sampArrSz-1);   i > 0 ; i-- )
    {
        pSampArr[i] = pSampArr[i - 1];
    }

     // Shift in newest sample
    pSampArr[0] = samp;
            
    return   avg;
  
} // runningAvg

uint8 setAccelFullscale( uint8 fs )
{
   uint8 retVal =  modiWriteReg( ACCEL_FS_SEL_REG, fs );

  if( retVal == TX_SUCCESS   )
  {
    accelFSRange = fs;
  }
    
  return retVal;
  
}   // setAccelFullscale

bool getModiData( void )
{
  bool retVal = modiReadRegBlock(   DIST_LSB_REG, 8, modiDataBytes );           // Read data bytes for distance, accel   XYZ
  if( retVal )
  {                                                                           //   Convert rx'd bytes to floats
      distCm_f = modiBytes2Distance( modiDataBytes   + 0 );                     // cm  
      accelX_f = modiBytes2Accel( accelFSRange,   modiDataBytes[3], modiDataBytes[2] );    // G
      accelY_f = modiBytes2Accel(   accelFSRange, modiDataBytes[5], modiDataBytes[4] );    // G
      accelZ_f =   modiBytes2Accel( accelFSRange, modiDataBytes[7], modiDataBytes[6] );    // G
   }

  return retVal;
  
} // getModiData

void printModiData(   void )
{
    Serial.print("D(cm): ");        // Print Distance
    Serial.print(distCm_favg,2);
     
    Serial.print("  AX(g): ");      // Print Accel X
    Serial.print(accelX_f,4);

     Serial.print("  AY(g): ");      // Print Accel Y
    Serial.print(accelY_f,4);

     Serial.print("  AZ(g): ");      // Print Accel Z
    Serial.println(accelZ_f,4);
     Serial.println("***********************************************");
    
}   // printModiData

float modiBytes2Distance( uint8 *pBytes )
{
  uint16   distCode = ( ( (uint16)pBytes[1] ) << 8 ) + pBytes[0];   // concatenate bytes
   float dist = ( (float)distCode )*0.00050813*100.0;              // compute distance
   return dist;                                                    // return result   in cm
  
} // modiBytes2Distance

void modiInit( uint8 accelFs )
{
   modiSendCommand( CMD_SW_RST );
  Serial.println("Initializing MODIShield...");    
  delay(1000);
  
  uint8 rxBuff = 0;
  
  rxBuff = modiReadReg(   WHO_AM_I_REG );     // Read Who Am I Register
  Serial.print("Who am I: 0x");              // Print Who Am I ID to COM port
  Serial.println(rxBuff, HEX);    
  rxBuff = modiReadReg( DEV_INFO_REG );     // Read Device Info Register
   Serial.print("Device Info: 0x");          // Print Device Info to COM port
   Serial.println(rxBuff, HEX);

  if( modiReadReg( STAT_REG ) == 0x01 )     //   Read MODIShield Status Register
  {
    Serial.println("MODIShield Initialization:   SUCCESS");
  }
  else
  {
    Serial.println("MODIShield Initialization:   FAILURE");
    modiSendCommand( CMD_SW_RST );          // If device Init falied,   perform SW reset on MODIShield
  }
  
  Serial.print("Config Accel Full-scale   range...");   // Configure Accelerometer full-scale range
  
  if( setAccelFullscale(   accelFs ) == TX_SUCCESS )
  {
    Serial.println("SUCCESS");
  }
   else
  {
    Serial.println("FAILURE");
  }

  while( modiSendCommand(   CMD_ST_DAT ) != TX_SUCCESS )    //  Send start data collection command
  {
     delay(100);
  }
  
} // modiInit

uint8 modiSendCommand( uint8   cmd )
{
   return modiWriteReg( CMD_REG, cmd );
   
} // modiSendCommand

uint8   modiWriteReg( uint8 regAddr, uint8 regData )
{
  uint8 txBuff[2] = {regAddr,   regData};
  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to   MODIShield
  Wire.write( txBuff, 2 );                            // Pass outgoing   bytes to Wire lib API
  return Wire.endTransmission(I2C_STOP);              //   TX bytes, send STOP condition (release I2C bus) and return comm status
} // modiWriteReg

uint8   modiWriteRegBlock( uint8 regAddr, uint8 numBytes, uint8 *pBuff )
{
  Wire.beginTransmission(MODI_I2C_ADDR);               // Transmit to MODIShield
  Wire.write( pBuff, numBytes );                      //   Pass outgoing bytes to Wire lib API
  return Wire.endTransmission(I2C_STOP);               // TX bytes, send STOP condition (release I2C bus) and return comm   status
} // modiWriteRegBlock

uint8 modiReadReg( uint8 regAddr )
{
   uint8 regData = 0;
  if ( modiWriteReg( ADDR_PTR_REG, regAddr ) == TX_SUCCESS   )
  {
      Wire.requestFrom( MODI_I2C_ADDR, 1 );    // Request 1 byte
       
      while( Wire.available() )              // slave may send less than   requested
      {   
          regData = Wire.read();             // RX byte
       }
  }
  return regData;
  
} // modiReadReg

bool modiReadRegBlock(   uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff )
{
  bool retVal = FALSE;
   
  if ( modiWriteReg( ADDR_PTR_REG, stRegAddr ) == TX_SUCCESS )
  {
       Wire.requestFrom(MODI_I2C_ADDR, (int)numBytes );    // Request 'numBytes'   number of bytes
      uint8 i = 0; 
      while( Wire.available() )                     //   slave may send less than requested
      {   
          pRxBuff[i] = Wire.read();                  // RX byte
          i++;                                      //   Inc RX buff index
          if( i == numBytes )                       // Break   from loop if buffer size exceeded
            break;
      }

      retVal   = TRUE;
  }

  return retVal;
  
} // modiReadRegBlock

void   configPins( void )
{
  pinMode( MODI_INT_PIN, INPUT );           // Configure   MODIShield Interrupt pin as input ( with internal pull-up resistor disabled )
   pinMode(LED_PIN, OUTPUT);                 // Set LED pin as OUTPUT          
   digitalWrite(LED_PIN, LOW);               // Init LED OFF
  pinMode(MTR_DIR_A,OUTPUT);
   pinMode(MTR_DIR_B,OUTPUT);
  pinMode(MTR_PWM_A, OUTPUT);
  pinMode(MTR_PWM_B,   OUTPUT);
  pinMode(MTR_BRAKE_A, OUTPUT);
  pinMode(MTR_BRAKE_B, OUTPUT);
   digitalWrite(MTR_BRAKE_A,LOW);
  digitalWrite(MTR_BRAKE_B,LOW);
  
  
   
  
} // configPins

void configInterrupts( void )
{
  attachInterrupt(digitalPinToInterrupt(MODI_INT_PIN),   ModiShieldISR, FALLING);
  
} // configInterrupts

float modiBytes2Accel(   uint8 fsMode, uint8 msb , uint8 lsb )
{
  uint16 integerAccum = 0;
  uint16   fracAccum = 0;             // Fractional Accumulator
  uint16 accelWord_frac   = 0;        // Fractional portion of accel word
  uint8 accelWord_int = 0;
   
  float fOut = 0.0;
  bool neg = FALSE;
  
  uint16 accelWord = (   (uint16)msb ) << 8;      // Concatenate MSByte and LSByte into single word
  accelWord   |= lsb;
  accelWord &= 0xFFF0;                          // Mask out lower nibble   of LSByte register ( will be all zeros anyway since conversion result is left-justified   )

  if( msb > 0x7F )                              // If negative, perform   2's complement
  {
    accelWord = ( ~accelWord ) + 1;
    neg = TRUE;
   }

  if( fsMode == ACCEL_FS_2G )           // 2G
  {
     accelWord_int   = ( accelWord >> 14 ) & 0x01;
     accelWord_frac = accelWord << 2;   // Shift   out sign and integer bits (remaining frac bits are now left-justified)
  }
   else if( fsMode == ACCEL_FS_4G )      // 4G
  {
    accelWord_int = ( accelWord   >> 13 ) & 0x03;
    accelWord_frac = accelWord << 3;    // Shift out sign and   integer bits (remaining frac bits are now left-justified)
  }
  else                                  //   8G
  {
    accelWord_int = ( accelWord >> 12 ) & 0x07;
    accelWord_frac   = accelWord << 4;    // Shift out sign and integer bits (remaining frac bits are   now left-justified)
  }

  // Determine fractional portion
  if( accelWord_frac   & 0x8000 )
     fracAccum += FRAC_2d1;
  if( accelWord_frac & 0x4000 )
      fracAccum += FRAC_2d2;  
  if( accelWord_frac & 0x2000 )
     fracAccum   += FRAC_2d3;
  if( accelWord_frac & 0x1000 )
     fracAccum += FRAC_2d4;
   if( accelWord_frac & 0x0800 )
     fracAccum += FRAC_2d5;  
  if( accelWord_frac   & 0x0400 )
     fracAccum += FRAC_2d6;
  if( accelWord_frac & 0x0200 )
      fracAccum += FRAC_2d7;    
  if( accelWord_frac & 0x0100 )
     fracAccum   += FRAC_2d8;

  if( ( fsMode != ACCEL_FS_8G ) && ( accelWord_frac & 0x0080   )  )
  {
     fracAccum += FRAC_2d9;
  }

  if( ( fsMode == ACCEL_FS_2G   ) && ( accelWord_frac & 0x0040 )  )
  {
     fracAccum += FRAC_2d10;
  }

   fOut = ( (float)fracAccum ) / 10000.0;        // Convert to g's (1g = 9.81 m/s^2)
   fOut += ( (float)accelWord_int );
   
  if( neg )                                    //   If number was negative, convert float to a negative number as well
  {
    fOut   = fOut*-1.0;
  }
  
  return fOut;

} // modiBytes2Accel

///////////////////////////////////////////////////////////////////////////////////////////////////////
//   Interrupt Service Routines
///////////////////////////////////////////////////////////////////////////////////////////////////////

void   ModiShieldISR( void ) 
{
  modiInt = TRUE;       // Set app-level interrupt   flag
}

modi shield_boxmini code

arduino

The code is a tweaked version of the example MODI Shield code downloaded from Dweebots.com. I simply added a routine to drive the DC motors and make decisions based on the data from the IMU.

1///////////////////////////////////////////////////////////////////////////////////////////////////////
2//  Include
3///////////////////////////////////////////////////////////////////////////////////////////////////////
4
5#include  <Wire.h>
6
7///////////////////////////////////////////////////////////////////////////////////////////////////////
8//  Define
9///////////////////////////////////////////////////////////////////////////////////////////////////////
10
11#define  MODI_I2C_ADDR               (0xA8 >> 1)         // MODIShield I2C 8-bit Slave Address
12
13/*  *** GPIO Map ***
14MODIShield Pin --------- Arduino Uno Pin
15INT                      2
16SDA                      A4
17SCL                      A5
18 */
19#define MODI_INT_PIN                2                   // MODIShield Interrupt pin
20#define LED_PIN                     13                  // Arduino Uno Onboard LED pin
21
22//  Weight for fractional bits of accelerometer conversion
23#define FRAC_2d1                  5000  // 2^9
24#define FRAC_2d2                  2500  // 2^8
25#define FRAC_2d3                  1250  // 2^7
26#define FRAC_2d4                  625   // 2^6
27#define FRAC_2d5                  313   // 2^5
28#define FRAC_2d6                  156   // 2^4
29#define FRAC_2d7                  78    // 2^3
30#define FRAC_2d8                  39    // 2^2
31#define  FRAC_2d9                  20    // 2^1
32#define FRAC_2d10                 10    //  2^0
33
34// MODI Register Map Addr
35#define WHO_AM_I_REG                0
36#define  DEV_INFO_REG                1
37#define ADDR_PTR_REG                2
38#define  CMD_REG                     3
39#define STAT_REG                    4
40#define  DIST_LSB_REG                5
41#define DIST_MSB_REG                6
42#define  AX_LSB_REG                  7
43#define AX_MSB_REG                  8
44#define  AY_LSB_REG                  9
45#define AY_MSB_REG                  10
46#define  AZ_LSB_REG                  11
47#define AZ_MSB_REG                  12
48#define  SAMP_PER_REG                13
49#define ACCEL_FS_SEL_REG            14
50
51//  MODI Register Bit Map
52
53// Command Register
54#define CMD_SLA_REL                 0x01
55#define  CMD_SW_RST                  0x02
56#define CMD_ST_DAT                  0x03
57#define  CMD_SP_DAT                  0x04
58
59// Accelerometer Full-Scale Select Register
60#define  ACCEL_FS_2G                 1
61#define ACCEL_FS_4G                 2
62#define  ACCEL_FS_8G                 3
63
64#define TRUE                        1
65#define  FALSE                       0
66
67#define I2C_STOP                    TRUE
68#define  I2C_RPT_START               FALSE
69
70// Wire.endTransmission() return val
71#define  TX_SUCCESS                  0
72#define DATA_BUFF_OVFLW             1
73#define  SLA_ADDR_NACK               2
74#define DATA_NACK                   3
75#define  OTHER_ERR                   4
76
77// Running Average Array Size
78#define SAMP_ARR_SZ                 5
79
80//Motor Definitions
81#define MTR_DIR_A                    12
82#define  MTR_DIR_B                    13
83#define MTR_PWM_A                    3
84#define  MTR_PWM_B                    11
85#define MTR_BRAKE_A                  9
86#define  MTR_BRAKE_B                  8
87#define FORWARD                     LOW
88#define  REVERSE                      HIGH
89#define MTR_A                       MTR_PWM_A
90#define  MTR_B                       MTR_PWM_B
91
92///////////////////////////////////////////////////////////////////////////////////////////////////////
93//  Typedefs
94///////////////////////////////////////////////////////////////////////////////////////////////////////
95
96typedef  unsigned char       uint8;
97typedef unsigned short      uint16;
98typedef signed  short        int16;
99
100///////////////////////////////////////////////////////////////////////////////////////////////////////
101//  Global Variables
102///////////////////////////////////////////////////////////////////////////////////////////////////////
103
104volatile  bool              modiInt = FALSE;
105uint8                      modiDataBytes[8];
106float                      accelX_f = 0, accelY_f = 0, accelZ_f = 0;
107float                      distCm_f  = 0;
108float                      sampleArr[SAMP_ARR_SZ];
109float                      distCm_favg  = 0;
110uint8                      accelFSRange = 0;
111
112///////////////////////////////////////////////////////////////////////////////////////////////////////
113//  Prototypes
114///////////////////////////////////////////////////////////////////////////////////////////////////////
115
116void  configPins( void );
117void configInterrupts( void );
118
119void modiInit( uint8  accelFs );
120uint8 setAccelFullscale( uint8 fs );
121uint8 modiWriteReg( uint8  regAddr, uint8 regData );
122uint8 modiWriteRegBlock( uint8 regAddr, uint8 numBytes,  uint8 *pBuff );
123uint8 modiReadReg( uint8 regAddr );
124bool modiReadRegBlock(  uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff );
125uint8 modiSendCommand( uint8  cmd );
126
127bool getModiData( void );
128float modiBytes2Distance( uint8 *pBytes  );
129float modiBytes2Accel( uint8 fsMode, uint8 msb , uint8 lsb );
130float runningAvg(  float *pSampArr, uint8 sampArrSz, float samp );
131void printModiData( void );
132void  ModiShieldISR( void );
133
134void drive(uint8 motor_speed, bool motor_direction,uint8  motor_channel);
135
136
137///////////////////////////////////////////////////////////////////////////////////////////////////////
138//  Main
139///////////////////////////////////////////////////////////////////////////////////////////////////////
140
141void  setup() 
142{
143  configPins();         // Configure GPIOs
144  Wire.begin();         //  Join I2C bus as master
145  
146  pinMode(SDA, INPUT);  // Disable internal Pull-up  resistors for I2C SDA and SCL lines
147  pinMode(SCL, INPUT);
148  
149  Serial.begin(9600);              // Start serial terminal
150  modiInit(ACCEL_FS_2G);           //  Init MODIShield
151  configInterrupts();              // Configure Interrupts
152  //drive(125,FORWARD,MTR_A);
153  //drive(125,FORWARD,MTR_B);
154}
155
156void  loop() 
157{
158  if( modiInt )                           // Check if MODI has generated  an interrupt
159  {
160    if( getModiData() )                   // Read all eight  data registers, convert to floats
161    {                    
162      distCm_favg  = runningAvg( sampleArr, SAMP_ARR_SZ, distCm_f ); // Perform running average on  distance measurement
163      printModiData();                    // Print data  to COM port
164      if (accelZ_f>= .6 || accelZ_f <= -.6 || accelY_f <= -.6 ||  accelY_f >= .6)
165      {
166         drive(0,FORWARD,MTR_A);  //stop motors
167         drive(0,FORWARD,MTR_B);
168         delay(3000);
169      }
170      else
171      {
172        if (distCm_f < 20 )
173        {
174          drive(150,REVERSE,MTR_A);
175          drive(150,FORWARD,MTR_B);
176          //delay(500);
177        }
178        /*else if (distCm_f < 20)
179        {
180          drive(126,REVERSE,MTR_A);
181          drive(126,REVERSE,MTR_B);
182        }
183        */
184        else if  (distCm_f > 20)
185        {
186          drive(160,FORWARD,MTR_A);
187          drive(160,FORWARD,MTR_B);
188        }       
189      }
190    }
191    modiSendCommand( CMD_SLA_REL );       //  TX slave release command to MODI via I2C
192    modiInt = FALSE;                      //  Clear app-lvl interrupt flag
193  }
194  //delay(250);                             //  Delay (for readability)
195}
196
197///////////////////////////////////////////////////////////////////////////////////////////////////////
198//  Functions
199///////////////////////////////////////////////////////////////////////////////////////////////////////
200void  drive(uint8 motor_speed, bool motor_dir,uint8 motor_channel)
201{
202  if (motor_channel  == MTR_A)
203  {
204    digitalWrite(MTR_DIR_A, motor_dir);
205    analogWrite(MTR_PWM_A,  motor_speed);
206  }
207  else if (motor_channel == MTR_B)
208  {
209    digitalWrite(MTR_DIR_B,  motor_dir);
210    analogWrite(MTR_PWM_B, motor_speed);
211  }
212}
213
214
215
216float  runningAvg( float *pSampArr, uint8 sampArrSz, float samp )
217{
218    uint8 i =  0;
219    float avg = 0;
220
221    // Sum last "arrSize" number of samples
222    for ( i = 0; i < sampArrSz; i++ )
223    {
224       avg += pSampArr[i];
225    }
226
227    // Add current sample to sum
228    avg += samp;
229    // Divide  by array size plus 1 (to accomodate addition of current sample)
230    avg = avg  / (sampArrSz + 1);
231
232
233    // Shift out oldest sample
234    for (i = (sampArrSz-1);  i > 0 ; i-- )
235    {
236        pSampArr[i] = pSampArr[i - 1];
237    }
238
239    // Shift in newest sample
240    pSampArr[0] = samp;
241            
242    return  avg;
243  
244} // runningAvg
245
246uint8 setAccelFullscale( uint8 fs )
247{
248  uint8 retVal =  modiWriteReg( ACCEL_FS_SEL_REG, fs );
249
250  if( retVal == TX_SUCCESS  )
251  {
252    accelFSRange = fs;
253  }
254    
255  return retVal;
256  
257}  // setAccelFullscale
258
259bool getModiData( void )
260{
261  bool retVal = modiReadRegBlock(  DIST_LSB_REG, 8, modiDataBytes );           // Read data bytes for distance, accel  XYZ
262  if( retVal )
263  {                                                                           //  Convert rx'd bytes to floats
264      distCm_f = modiBytes2Distance( modiDataBytes  + 0 );                     // cm  
265      accelX_f = modiBytes2Accel( accelFSRange,  modiDataBytes[3], modiDataBytes[2] );    // G
266      accelY_f = modiBytes2Accel(  accelFSRange, modiDataBytes[5], modiDataBytes[4] );    // G
267      accelZ_f =  modiBytes2Accel( accelFSRange, modiDataBytes[7], modiDataBytes[6] );    // G
268  }
269
270  return retVal;
271  
272} // getModiData
273
274void printModiData(  void )
275{
276    Serial.print("D(cm): ");        // Print Distance
277    Serial.print(distCm_favg,2);
278    
279    Serial.print("  AX(g): ");      // Print Accel X
280    Serial.print(accelX_f,4);
281
282    Serial.print("  AY(g): ");      // Print Accel Y
283    Serial.print(accelY_f,4);
284
285    Serial.print("  AZ(g): ");      // Print Accel Z
286    Serial.println(accelZ_f,4);
287    Serial.println("***********************************************");
288    
289}  // printModiData
290
291float modiBytes2Distance( uint8 *pBytes )
292{
293  uint16  distCode = ( ( (uint16)pBytes[1] ) << 8 ) + pBytes[0];   // concatenate bytes
294  float dist = ( (float)distCode )*0.00050813*100.0;              // compute distance
295  return dist;                                                    // return result  in cm
296  
297} // modiBytes2Distance
298
299void modiInit( uint8 accelFs )
300{
301  modiSendCommand( CMD_SW_RST );
302  Serial.println("Initializing MODIShield...");   
303  delay(1000);
304  
305  uint8 rxBuff = 0;
306  
307  rxBuff = modiReadReg(  WHO_AM_I_REG );     // Read Who Am I Register
308  Serial.print("Who am I: 0x");             // Print Who Am I ID to COM port
309  Serial.println(rxBuff, HEX);   
310  rxBuff = modiReadReg( DEV_INFO_REG );     // Read Device Info Register
311  Serial.print("Device Info: 0x");          // Print Device Info to COM port
312  Serial.println(rxBuff, HEX);
313
314  if( modiReadReg( STAT_REG ) == 0x01 )     //  Read MODIShield Status Register
315  {
316    Serial.println("MODIShield Initialization:  SUCCESS");
317  }
318  else
319  {
320    Serial.println("MODIShield Initialization:  FAILURE");
321    modiSendCommand( CMD_SW_RST );          // If device Init falied,  perform SW reset on MODIShield
322  }
323  
324  Serial.print("Config Accel Full-scale  range...");   // Configure Accelerometer full-scale range
325  
326  if( setAccelFullscale(  accelFs ) == TX_SUCCESS )
327  {
328    Serial.println("SUCCESS");
329  }
330  else
331  {
332    Serial.println("FAILURE");
333  }
334
335  while( modiSendCommand(  CMD_ST_DAT ) != TX_SUCCESS )    //  Send start data collection command
336  {
337    delay(100);
338  }
339  
340} // modiInit
341
342uint8 modiSendCommand( uint8  cmd )
343{
344   return modiWriteReg( CMD_REG, cmd );
345   
346} // modiSendCommand
347
348uint8  modiWriteReg( uint8 regAddr, uint8 regData )
349{
350  uint8 txBuff[2] = {regAddr,  regData};
351  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to  MODIShield
352  Wire.write( txBuff, 2 );                            // Pass outgoing  bytes to Wire lib API
353  return Wire.endTransmission(I2C_STOP);              //  TX bytes, send STOP condition (release I2C bus) and return comm status
354} // modiWriteReg
355
356uint8  modiWriteRegBlock( uint8 regAddr, uint8 numBytes, uint8 *pBuff )
357{
358  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to MODIShield
359  Wire.write( pBuff, numBytes );                      //  Pass outgoing bytes to Wire lib API
360  return Wire.endTransmission(I2C_STOP);              // TX bytes, send STOP condition (release I2C bus) and return comm  status
361} // modiWriteRegBlock
362
363uint8 modiReadReg( uint8 regAddr )
364{
365  uint8 regData = 0;
366  if ( modiWriteReg( ADDR_PTR_REG, regAddr ) == TX_SUCCESS  )
367  {
368      Wire.requestFrom( MODI_I2C_ADDR, 1 );    // Request 1 byte
369      
370      while( Wire.available() )              // slave may send less than  requested
371      {   
372          regData = Wire.read();             // RX byte
373      }
374  }
375  return regData;
376  
377} // modiReadReg
378
379bool modiReadRegBlock(  uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff )
380{
381  bool retVal = FALSE;
382  
383  if ( modiWriteReg( ADDR_PTR_REG, stRegAddr ) == TX_SUCCESS )
384  {
385      Wire.requestFrom(MODI_I2C_ADDR, (int)numBytes );    // Request 'numBytes'  number of bytes
386      uint8 i = 0; 
387      while( Wire.available() )                     //  slave may send less than requested
388      {   
389          pRxBuff[i] = Wire.read();                 // RX byte
390          i++;                                      //  Inc RX buff index
391          if( i == numBytes )                       // Break  from loop if buffer size exceeded
392            break;
393      }
394
395      retVal  = TRUE;
396  }
397
398  return retVal;
399  
400} // modiReadRegBlock
401
402void  configPins( void )
403{
404  pinMode( MODI_INT_PIN, INPUT );           // Configure  MODIShield Interrupt pin as input ( with internal pull-up resistor disabled )
405  pinMode(LED_PIN, OUTPUT);                 // Set LED pin as OUTPUT          
406  digitalWrite(LED_PIN, LOW);               // Init LED OFF
407  pinMode(MTR_DIR_A,OUTPUT);
408  pinMode(MTR_DIR_B,OUTPUT);
409  pinMode(MTR_PWM_A, OUTPUT);
410  pinMode(MTR_PWM_B,  OUTPUT);
411  pinMode(MTR_BRAKE_A, OUTPUT);
412  pinMode(MTR_BRAKE_B, OUTPUT);
413  digitalWrite(MTR_BRAKE_A,LOW);
414  digitalWrite(MTR_BRAKE_B,LOW);
415  
416  
417  
418  
419} // configPins
420
421void configInterrupts( void )
422{
423  attachInterrupt(digitalPinToInterrupt(MODI_INT_PIN),  ModiShieldISR, FALLING);
424  
425} // configInterrupts
426
427float modiBytes2Accel(  uint8 fsMode, uint8 msb , uint8 lsb )
428{
429  uint16 integerAccum = 0;
430  uint16  fracAccum = 0;             // Fractional Accumulator
431  uint16 accelWord_frac  = 0;        // Fractional portion of accel word
432  uint8 accelWord_int = 0;
433  
434  float fOut = 0.0;
435  bool neg = FALSE;
436  
437  uint16 accelWord = (  (uint16)msb ) << 8;      // Concatenate MSByte and LSByte into single word
438  accelWord  |= lsb;
439  accelWord &= 0xFFF0;                          // Mask out lower nibble  of LSByte register ( will be all zeros anyway since conversion result is left-justified  )
440
441  if( msb > 0x7F )                              // If negative, perform  2's complement
442  {
443    accelWord = ( ~accelWord ) + 1;
444    neg = TRUE;
445  }
446
447  if( fsMode == ACCEL_FS_2G )           // 2G
448  {
449     accelWord_int  = ( accelWord >> 14 ) & 0x01;
450     accelWord_frac = accelWord << 2;   // Shift  out sign and integer bits (remaining frac bits are now left-justified)
451  }
452  else if( fsMode == ACCEL_FS_4G )      // 4G
453  {
454    accelWord_int = ( accelWord  >> 13 ) & 0x03;
455    accelWord_frac = accelWord << 3;    // Shift out sign and  integer bits (remaining frac bits are now left-justified)
456  }
457  else                                  //  8G
458  {
459    accelWord_int = ( accelWord >> 12 ) & 0x07;
460    accelWord_frac  = accelWord << 4;    // Shift out sign and integer bits (remaining frac bits are  now left-justified)
461  }
462
463  // Determine fractional portion
464  if( accelWord_frac  & 0x8000 )
465     fracAccum += FRAC_2d1;
466  if( accelWord_frac & 0x4000 )
467     fracAccum += FRAC_2d2;  
468  if( accelWord_frac & 0x2000 )
469     fracAccum  += FRAC_2d3;
470  if( accelWord_frac & 0x1000 )
471     fracAccum += FRAC_2d4;
472  if( accelWord_frac & 0x0800 )
473     fracAccum += FRAC_2d5;  
474  if( accelWord_frac  & 0x0400 )
475     fracAccum += FRAC_2d6;
476  if( accelWord_frac & 0x0200 )
477     fracAccum += FRAC_2d7;    
478  if( accelWord_frac & 0x0100 )
479     fracAccum  += FRAC_2d8;
480
481  if( ( fsMode != ACCEL_FS_8G ) && ( accelWord_frac & 0x0080  )  )
482  {
483     fracAccum += FRAC_2d9;
484  }
485
486  if( ( fsMode == ACCEL_FS_2G  ) && ( accelWord_frac & 0x0040 )  )
487  {
488     fracAccum += FRAC_2d10;
489  }
490
491  fOut = ( (float)fracAccum ) / 10000.0;        // Convert to g's (1g = 9.81 m/s^2)
492  fOut += ( (float)accelWord_int );
493   
494  if( neg )                                    //  If number was negative, convert float to a negative number as well
495  {
496    fOut  = fOut*-1.0;
497  }
498  
499  return fOut;
500
501} // modiBytes2Accel
502
503///////////////////////////////////////////////////////////////////////////////////////////////////////
504//  Interrupt Service Routines
505///////////////////////////////////////////////////////////////////////////////////////////////////////
506
507void  ModiShieldISR( void ) 
508{
509  modiInt = TRUE;       // Set app-level interrupt  flag
510}
511

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Include
///////////////////////////////////////////////////////////////////////////////////////////////////////

#include  <Wire.h>

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Define
///////////////////////////////////////////////////////////////////////////////////////////////////////

#define  MODI_I2C_ADDR               (0xA8 >> 1)         // MODIShield I2C 8-bit Slave Address

/*  *** GPIO Map ***
MODIShield Pin --------- Arduino Uno Pin
INT                      2
SDA                      A4
SCL                      A5
 */
#define MODI_INT_PIN                2                   // MODIShield Interrupt pin
#define LED_PIN                     13                  // Arduino Uno Onboard LED pin

//  Weight for fractional bits of accelerometer conversion
#define FRAC_2d1                  5000  // 2^9
#define FRAC_2d2                  2500  // 2^8
#define FRAC_2d3                  1250  // 2^7
#define FRAC_2d4                  625   // 2^6
#define FRAC_2d5                  313   // 2^5
#define FRAC_2d6                  156   // 2^4
#define FRAC_2d7                  78    // 2^3
#define FRAC_2d8                  39    // 2^2
#define  FRAC_2d9                  20    // 2^1
#define FRAC_2d10                 10    //  2^0

// MODI Register Map Addr
#define WHO_AM_I_REG                0
#define  DEV_INFO_REG                1
#define ADDR_PTR_REG                2
#define  CMD_REG                     3
#define STAT_REG                    4
#define  DIST_LSB_REG                5
#define DIST_MSB_REG                6
#define  AX_LSB_REG                  7
#define AX_MSB_REG                  8
#define  AY_LSB_REG                  9
#define AY_MSB_REG                  10
#define  AZ_LSB_REG                  11
#define AZ_MSB_REG                  12
#define  SAMP_PER_REG                13
#define ACCEL_FS_SEL_REG            14

//  MODI Register Bit Map

// Command Register
#define CMD_SLA_REL                 0x01
#define  CMD_SW_RST                  0x02
#define CMD_ST_DAT                  0x03
#define  CMD_SP_DAT                  0x04

// Accelerometer Full-Scale Select Register
#define  ACCEL_FS_2G                 1
#define ACCEL_FS_4G                 2
#define  ACCEL_FS_8G                 3

#define TRUE                        1
#define  FALSE                       0

#define I2C_STOP                    TRUE
#define  I2C_RPT_START               FALSE

// Wire.endTransmission() return val
#define  TX_SUCCESS                  0
#define DATA_BUFF_OVFLW             1
#define  SLA_ADDR_NACK               2
#define DATA_NACK                   3
#define  OTHER_ERR                   4

// Running Average Array Size
#define SAMP_ARR_SZ                 5

//Motor Definitions
#define MTR_DIR_A                    12
#define  MTR_DIR_B                    13
#define MTR_PWM_A                    3
#define  MTR_PWM_B                    11
#define MTR_BRAKE_A                  9
#define  MTR_BRAKE_B                  8
#define FORWARD                     LOW
#define  REVERSE                      HIGH
#define MTR_A                       MTR_PWM_A
#define  MTR_B                       MTR_PWM_B

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Typedefs
///////////////////////////////////////////////////////////////////////////////////////////////////////

typedef  unsigned char       uint8;
typedef unsigned short      uint16;
typedef signed  short        int16;

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Global Variables
///////////////////////////////////////////////////////////////////////////////////////////////////////

volatile  bool              modiInt = FALSE;
uint8                      modiDataBytes[8];
float                      accelX_f = 0, accelY_f = 0, accelZ_f = 0;
float                      distCm_f  = 0;
float                      sampleArr[SAMP_ARR_SZ];
float                      distCm_favg  = 0;
uint8                      accelFSRange = 0;

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Prototypes
///////////////////////////////////////////////////////////////////////////////////////////////////////

void  configPins( void );
void configInterrupts( void );

void modiInit( uint8  accelFs );
uint8 setAccelFullscale( uint8 fs );
uint8 modiWriteReg( uint8  regAddr, uint8 regData );
uint8 modiWriteRegBlock( uint8 regAddr, uint8 numBytes,  uint8 *pBuff );
uint8 modiReadReg( uint8 regAddr );
bool modiReadRegBlock(  uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff );
uint8 modiSendCommand( uint8  cmd );

bool getModiData( void );
float modiBytes2Distance( uint8 *pBytes  );
float modiBytes2Accel( uint8 fsMode, uint8 msb , uint8 lsb );
float runningAvg(  float *pSampArr, uint8 sampArrSz, float samp );
void printModiData( void );
void  ModiShieldISR( void );

void drive(uint8 motor_speed, bool motor_direction,uint8  motor_channel);


///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Main
///////////////////////////////////////////////////////////////////////////////////////////////////////

void  setup() 
{
  configPins();         // Configure GPIOs
  Wire.begin();         //  Join I2C bus as master
  
  pinMode(SDA, INPUT);  // Disable internal Pull-up  resistors for I2C SDA and SCL lines
  pinMode(SCL, INPUT);
  
  Serial.begin(9600);              // Start serial terminal
  modiInit(ACCEL_FS_2G);           //  Init MODIShield
  configInterrupts();              // Configure Interrupts
  //drive(125,FORWARD,MTR_A);
  //drive(125,FORWARD,MTR_B);
}

void  loop() 
{
  if( modiInt )                           // Check if MODI has generated  an interrupt
  {
    if( getModiData() )                   // Read all eight  data registers, convert to floats
    {                    
      distCm_favg  = runningAvg( sampleArr, SAMP_ARR_SZ, distCm_f ); // Perform running average on  distance measurement
      printModiData();                    // Print data  to COM port
      if (accelZ_f>= .6 || accelZ_f <= -.6 || accelY_f <= -.6 ||  accelY_f >= .6)
      {
         drive(0,FORWARD,MTR_A);  //stop motors
         drive(0,FORWARD,MTR_B);
         delay(3000);
      }
      else
      {
        if (distCm_f < 20 )
        {
          drive(150,REVERSE,MTR_A);
          drive(150,FORWARD,MTR_B);
          //delay(500);
        }
        /*else if (distCm_f < 20)
        {
          drive(126,REVERSE,MTR_A);
          drive(126,REVERSE,MTR_B);
        }
        */
        else if  (distCm_f > 20)
        {
          drive(160,FORWARD,MTR_A);
          drive(160,FORWARD,MTR_B);
        }       
      }
    }
    modiSendCommand( CMD_SLA_REL );       //  TX slave release command to MODI via I2C
    modiInt = FALSE;                      //  Clear app-lvl interrupt flag
  }
  //delay(250);                             //  Delay (for readability)
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Functions
///////////////////////////////////////////////////////////////////////////////////////////////////////
void  drive(uint8 motor_speed, bool motor_dir,uint8 motor_channel)
{
  if (motor_channel  == MTR_A)
  {
    digitalWrite(MTR_DIR_A, motor_dir);
    analogWrite(MTR_PWM_A,  motor_speed);
  }
  else if (motor_channel == MTR_B)
  {
    digitalWrite(MTR_DIR_B,  motor_dir);
    analogWrite(MTR_PWM_B, motor_speed);
  }
}



float  runningAvg( float *pSampArr, uint8 sampArrSz, float samp )
{
    uint8 i =  0;
    float avg = 0;

    // Sum last "arrSize" number of samples
    for ( i = 0; i < sampArrSz; i++ )
    {
       avg += pSampArr[i];
    }

    // Add current sample to sum
    avg += samp;
    // Divide  by array size plus 1 (to accomodate addition of current sample)
    avg = avg  / (sampArrSz + 1);


    // Shift out oldest sample
    for (i = (sampArrSz-1);  i > 0 ; i-- )
    {
        pSampArr[i] = pSampArr[i - 1];
    }

    // Shift in newest sample
    pSampArr[0] = samp;
            
    return  avg;
  
} // runningAvg

uint8 setAccelFullscale( uint8 fs )
{
  uint8 retVal =  modiWriteReg( ACCEL_FS_SEL_REG, fs );

  if( retVal == TX_SUCCESS  )
  {
    accelFSRange = fs;
  }
    
  return retVal;
  
}  // setAccelFullscale

bool getModiData( void )
{
  bool retVal = modiReadRegBlock(  DIST_LSB_REG, 8, modiDataBytes );           // Read data bytes for distance, accel  XYZ
  if( retVal )
  {                                                                           //  Convert rx'd bytes to floats
      distCm_f = modiBytes2Distance( modiDataBytes  + 0 );                     // cm  
      accelX_f = modiBytes2Accel( accelFSRange,  modiDataBytes[3], modiDataBytes[2] );    // G
      accelY_f = modiBytes2Accel(  accelFSRange, modiDataBytes[5], modiDataBytes[4] );    // G
      accelZ_f =  modiBytes2Accel( accelFSRange, modiDataBytes[7], modiDataBytes[6] );    // G
  }

  return retVal;
  
} // getModiData

void printModiData(  void )
{
    Serial.print("D(cm): ");        // Print Distance
    Serial.print(distCm_favg,2);
    
    Serial.print("  AX(g): ");      // Print Accel X
    Serial.print(accelX_f,4);

    Serial.print("  AY(g): ");      // Print Accel Y
    Serial.print(accelY_f,4);

    Serial.print("  AZ(g): ");      // Print Accel Z
    Serial.println(accelZ_f,4);
    Serial.println("***********************************************");
    
}  // printModiData

float modiBytes2Distance( uint8 *pBytes )
{
  uint16  distCode = ( ( (uint16)pBytes[1] ) << 8 ) + pBytes[0];   // concatenate bytes
  float dist = ( (float)distCode )*0.00050813*100.0;              // compute distance
  return dist;                                                    // return result  in cm
  
} // modiBytes2Distance

void modiInit( uint8 accelFs )
{
  modiSendCommand( CMD_SW_RST );
  Serial.println("Initializing MODIShield...");   
  delay(1000);
  
  uint8 rxBuff = 0;
  
  rxBuff = modiReadReg(  WHO_AM_I_REG );     // Read Who Am I Register
  Serial.print("Who am I: 0x");             // Print Who Am I ID to COM port
  Serial.println(rxBuff, HEX);   
  rxBuff = modiReadReg( DEV_INFO_REG );     // Read Device Info Register
  Serial.print("Device Info: 0x");          // Print Device Info to COM port
  Serial.println(rxBuff, HEX);

  if( modiReadReg( STAT_REG ) == 0x01 )     //  Read MODIShield Status Register
  {
    Serial.println("MODIShield Initialization:  SUCCESS");
  }
  else
  {
    Serial.println("MODIShield Initialization:  FAILURE");
    modiSendCommand( CMD_SW_RST );          // If device Init falied,  perform SW reset on MODIShield
  }
  
  Serial.print("Config Accel Full-scale  range...");   // Configure Accelerometer full-scale range
  
  if( setAccelFullscale(  accelFs ) == TX_SUCCESS )
  {
    Serial.println("SUCCESS");
  }
  else
  {
    Serial.println("FAILURE");
  }

  while( modiSendCommand(  CMD_ST_DAT ) != TX_SUCCESS )    //  Send start data collection command
  {
    delay(100);
  }
  
} // modiInit

uint8 modiSendCommand( uint8  cmd )
{
   return modiWriteReg( CMD_REG, cmd );
   
} // modiSendCommand

uint8  modiWriteReg( uint8 regAddr, uint8 regData )
{
  uint8 txBuff[2] = {regAddr,  regData};
  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to  MODIShield
  Wire.write( txBuff, 2 );                            // Pass outgoing  bytes to Wire lib API
  return Wire.endTransmission(I2C_STOP);              //  TX bytes, send STOP condition (release I2C bus) and return comm status
} // modiWriteReg

uint8  modiWriteRegBlock( uint8 regAddr, uint8 numBytes, uint8 *pBuff )
{
  Wire.beginTransmission(MODI_I2C_ADDR);              // Transmit to MODIShield
  Wire.write( pBuff, numBytes );                      //  Pass outgoing bytes to Wire lib API
  return Wire.endTransmission(I2C_STOP);              // TX bytes, send STOP condition (release I2C bus) and return comm  status
} // modiWriteRegBlock

uint8 modiReadReg( uint8 regAddr )
{
  uint8 regData = 0;
  if ( modiWriteReg( ADDR_PTR_REG, regAddr ) == TX_SUCCESS  )
  {
      Wire.requestFrom( MODI_I2C_ADDR, 1 );    // Request 1 byte
      
      while( Wire.available() )              // slave may send less than  requested
      {   
          regData = Wire.read();             // RX byte
      }
  }
  return regData;
  
} // modiReadReg

bool modiReadRegBlock(  uint8 stRegAddr, uint8 numBytes, uint8 *pRxBuff )
{
  bool retVal = FALSE;
  
  if ( modiWriteReg( ADDR_PTR_REG, stRegAddr ) == TX_SUCCESS )
  {
      Wire.requestFrom(MODI_I2C_ADDR, (int)numBytes );    // Request 'numBytes'  number of bytes
      uint8 i = 0; 
      while( Wire.available() )                     //  slave may send less than requested
      {   
          pRxBuff[i] = Wire.read();                 // RX byte
          i++;                                      //  Inc RX buff index
          if( i == numBytes )                       // Break  from loop if buffer size exceeded
            break;
      }

      retVal  = TRUE;
  }

  return retVal;
  
} // modiReadRegBlock

void  configPins( void )
{
  pinMode( MODI_INT_PIN, INPUT );           // Configure  MODIShield Interrupt pin as input ( with internal pull-up resistor disabled )
  pinMode(LED_PIN, OUTPUT);                 // Set LED pin as OUTPUT          
  digitalWrite(LED_PIN, LOW);               // Init LED OFF
  pinMode(MTR_DIR_A,OUTPUT);
  pinMode(MTR_DIR_B,OUTPUT);
  pinMode(MTR_PWM_A, OUTPUT);
  pinMode(MTR_PWM_B,  OUTPUT);
  pinMode(MTR_BRAKE_A, OUTPUT);
  pinMode(MTR_BRAKE_B, OUTPUT);
  digitalWrite(MTR_BRAKE_A,LOW);
  digitalWrite(MTR_BRAKE_B,LOW);
  
  
  
  
} // configPins

void configInterrupts( void )
{
  attachInterrupt(digitalPinToInterrupt(MODI_INT_PIN),  ModiShieldISR, FALLING);
  
} // configInterrupts

float modiBytes2Accel(  uint8 fsMode, uint8 msb , uint8 lsb )
{
  uint16 integerAccum = 0;
  uint16  fracAccum = 0;             // Fractional Accumulator
  uint16 accelWord_frac  = 0;        // Fractional portion of accel word
  uint8 accelWord_int = 0;
  
  float fOut = 0.0;
  bool neg = FALSE;
  
  uint16 accelWord = (  (uint16)msb ) << 8;      // Concatenate MSByte and LSByte into single word
  accelWord  |= lsb;
  accelWord &= 0xFFF0;                          // Mask out lower nibble  of LSByte register ( will be all zeros anyway since conversion result is left-justified  )

  if( msb > 0x7F )                              // If negative, perform  2's complement
  {
    accelWord = ( ~accelWord ) + 1;
    neg = TRUE;
  }

  if( fsMode == ACCEL_FS_2G )           // 2G
  {
     accelWord_int  = ( accelWord >> 14 ) & 0x01;
     accelWord_frac = accelWord << 2;   // Shift  out sign and integer bits (remaining frac bits are now left-justified)
  }
  else if( fsMode == ACCEL_FS_4G )      // 4G
  {
    accelWord_int = ( accelWord  >> 13 ) & 0x03;
    accelWord_frac = accelWord << 3;    // Shift out sign and  integer bits (remaining frac bits are now left-justified)
  }
  else                                  //  8G
  {
    accelWord_int = ( accelWord >> 12 ) & 0x07;
    accelWord_frac  = accelWord << 4;    // Shift out sign and integer bits (remaining frac bits are  now left-justified)
  }

  // Determine fractional portion
  if( accelWord_frac  & 0x8000 )
     fracAccum += FRAC_2d1;
  if( accelWord_frac & 0x4000 )
     fracAccum += FRAC_2d2;  
  if( accelWord_frac & 0x2000 )
     fracAccum  += FRAC_2d3;
  if( accelWord_frac & 0x1000 )
     fracAccum += FRAC_2d4;
  if( accelWord_frac & 0x0800 )
     fracAccum += FRAC_2d5;  
  if( accelWord_frac  & 0x0400 )
     fracAccum += FRAC_2d6;
  if( accelWord_frac & 0x0200 )
     fracAccum += FRAC_2d7;    
  if( accelWord_frac & 0x0100 )
     fracAccum  += FRAC_2d8;

  if( ( fsMode != ACCEL_FS_8G ) && ( accelWord_frac & 0x0080  )  )
  {
     fracAccum += FRAC_2d9;
  }

  if( ( fsMode == ACCEL_FS_2G  ) && ( accelWord_frac & 0x0040 )  )
  {
     fracAccum += FRAC_2d10;
  }

  fOut = ( (float)fracAccum ) / 10000.0;        // Convert to g's (1g = 9.81 m/s^2)
  fOut += ( (float)accelWord_int );
   
  if( neg )                                    //  If number was negative, convert float to a negative number as well
  {
    fOut  = fOut*-1.0;
  }
  
  return fOut;

} // modiBytes2Accel

///////////////////////////////////////////////////////////////////////////////////////////////////////
//  Interrupt Service Routines
///////////////////////////////////////////////////////////////////////////////////////////////////////

void  ModiShieldISR( void ) 
{
  modiInt = TRUE;       // Set app-level interrupt  flag
}

Downloadable files

Arduino and Sensor Schematic

Use this diagram to wire the MODI Shield sensor to the Arduino UNO board.

Arduino and Sensor Schematic

Comments

Only logged in users can leave comments

andr9e

0 Followers

•

0 Projects

Intro