Published March 4, 2021 © GPL3+

Project MODI

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

BeginnerFull instructions provided3,905

Things used in this project

Hardware components

Arduino UNO

Arduino MotorShield Rev3

MODI Shield IR Distance Sensor

Box-Mini Robot Chassis Kit

Female/Female Jumper Wires

Machine Screw, M3

Hand tools and fabrication machines

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

Story

Project MODI was made to primarily explore the usefulness of the MODI Shield IR distance sensor with an Arduino when paired with a robot chassis. One cool feature of the MODI Shield IR distance sensor is that is has an IMU, which can be used to help control some basic functions of the robot such as starting and stoping..

The project should be very straight forward as the robot chassis is already assembled. We must first install the Arduino Uno to the robot chassis using three M3 screws.

Securing Arduino Uno

Wire battery and Motor wires to the motor shield as shown below:

Battery and motor wiring

Locate the accessory clip, and holder for the MODI Shield :

MODI Shield with holder and accessory clip

Assemble MODI Shield to the holder as shown below :

MODI Shield with holder

Using the accessory clip, attach the MODI Shield with holder to the robot.

Box-Mini Robot Assembled with MODI Shield and Arduino

Follow the Schematic to wire the MODI Shield to the Arduino.

Below is a video of the project being tested. Note: The robot will stop movement if the orientation is anything but right side up.

MODI Shield robot video

Code

modi shield_boxmini code

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

0 projects • 1 follower

Project MODI

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Schematics

Arduino and Sensor Schematic

Code

modi shield_boxmini code

Credits

andr9e

Comments

Embed the widget on your own site

Project MODI

Project MODI

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Schematics

Arduino and Sensor Schematic

Code

modi shield_boxmini code

Credits

andr9e

Comments

Related channels and tags