Hardware components | ||||||
_ztBMuBhMHo.jpg?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff) |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
 |
| × | 1 | |||
 |
| × | 1 | |||
Hand tools and fabrication machines | ||||||
 |
| |||||
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_boxmini code
ArduinoThe 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
}
Comments