andr9e
Published © GPL3+

Project MODI

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

BeginnerFull instructions provided3,901
Project MODI

Things used in this project

Hardware components

Arduino UNO
Arduino UNO
×1
Arduino MotorShield Rev3
Arduino MotorShield Rev3
×1
MODI Shield IR Distance Sensor
×1
Box-Mini Robot Chassis Kit
×1
Female/Female Jumper Wires
Female/Female Jumper Wires
×1
Machine Screw, M3
Machine Screw, M3
×1

Hand tools and fabrication machines

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

Story

Read more

Schematics

Arduino and Sensor Schematic

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

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.
///////////////////////////////////////////////////////////////////////////////////////////////////////
// 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
}

Credits

andr9e

andr9e

0 projects • 1 follower

Comments