Mikroe Buggy: Obstacle Avoiding Robot 0

BUGGY_PIC18_BARGRAPH.c

/*******************************************************************************
* Pin and function definitions for Bargraph click
* Bargraph Click should be installed in the mikroBUS socket #3 (rear)
*******************************************************************************/

sbit HC595_cs at RB4_bit;
sbit HC595_cs_direction at TRISB4_bit;
sbit HC595_rst at RJ2_bit;
sbit HC595_rst_direction at TRISJ2_bit;
sbit HC595_pwm at RJ7_bit;
sbit HC595_pwm_direction at TRISJ7_bit;

// BarGraph reset function
void hc_rst_fnc(void) {
  HC595_rst = 0;
  asm{nop};
  asm{nop};
  asm{nop};
  HC595_rst = 1;
}
// BarGraph CS select unction
void hc_cs_fnc(void) {
  HC595_cs = 0;
  asm{nop};
  asm{nop};
  asm{nop};
  HC595_cs = 1;
}

// Display graph on Bar Graph click (input value is from 0 to 10)
void BarGraph_Display(char value){
  char temp1, temp2;
  unsigned int temp;
  temp  = (1 << value) - 1;
  temp1 = (temp & 0x00FF);
  temp2 = (temp & 0xFF00) >> 8;
  SPI_Wr_Ptr(temp2);
  SPI_Wr_Ptr(temp1);
  hc_cs_fnc();
}
// =============================================================================

BUGGY_PIC18_FUNCTIONS.c

/*******************************************************************************
* Pin definitions for Clicker 2 for PIC18FJ
*******************************************************************************/

// LEDs
sbit LED1 at LATD4_bit;
sbit LED2 at LATE4_bit;
sbit LED1_Direction at TRISD4_bit;
sbit LED2_Direction at TRISE4_bit;
//Switches
sbit SW1 at RD7_bit;
sbit SW2 at RH3_bit;
sbit SW1_Direction at TRISD7_bit;
sbit SW2_Direction at TRISH3_bit;
// =============================================================================

/*******************************************************************************
* Pin and function definitions for BUGGY
*******************************************************************************/

// =============================================================================
// Pin definitions for LEDS on the BUGGY
sbit Brakelights at LATJ6_bit;
sbit Headlights at LATB5_bit;
sbit SignalL at LATF5_bit;
sbit SignalR at LATJ4_bit;
sbit MainBeam at LATJ3_bit;
sbit Brake_Direction at TRISJ6_bit;
sbit Headlights_Direction at TRISB5_bit;
sbit SignalL_Direction at TRISF5_bit;
sbit SignalR_Direction at TRISJ4_bit;
sbit MainBeam_direction at TRISJ3_bit;
// =============================================================================

// BUGGY Functions
unsigned int SPEED_L;
unsigned int SPEED_R;

void Init_Buggy() {
// INIT  LIGHTS
  Brake_Direction = 0; //brake lights
  Headlights_Direction = 0; // headlights
  MainBeam_direction = 0; // main beam
  SignalL_Direction = 0; // Headlights
  SignalR_Direction = 0; // headlights
//PWM
  SPEED_L = 0;
  SPEED_R = 0;
  PWM1_Init(5000);
  PWM2_Init(5000);                    //PWM_PERIOD;
  PWM5_Init(5000);
  PWM3_Init(5000);
  PWM1_Start();                       // start PWM1
  PWM2_Start();                       // start PWM2
  PWM3_Start();                       // start PWM3
  PWM5_Start();                       // start PWM5
  PWM1_Set_Duty(0);
  PWM2_Set_Duty(0);
  PWM5_Set_Duty(0);
  PWM3_Set_Duty(0);
}

void Break_Buggy() {
  SPEED_L = 0;
  SPEED_R = 0;
  PWM1_Start();                       // start PWM1
  PWM2_Start();                       // start PWM2
  PWM3_Start();                       // start PWM3
  PWM5_Start();                       // start PWM5
  PWM1_Set_Duty(0);
  PWM2_Set_Duty(0);
  PWM5_Set_Duty(0);
  PWM3_Set_Duty(0);
}

void Drive_Buggy(unsigned int speed) {
   // LEFT MOTORS
   PWM1_Set_Duty(speed);
   PWM2_Set_Duty(0);
   // RIGHT MOTORS
   PWM3_Set_Duty(speed);
   PWM5_Set_Duty(0);
}

void Back_Buggy(unsigned int speed) {
   PWM1_Set_Duty(0);
   PWM2_Set_Duty(speed);
   PWM3_Set_Duty(0);
   PWM5_Set_Duty(speed);
}

void Turn_Buggy(unsigned int speed) {
   PWM1_Set_Duty(0);
   PWM2_Set_Duty(speed);
   PWM3_Set_Duty(speed);
   PWM5_Set_Duty(0);
}

main.c

#include "VCNL40x0_PIC18_BUGGY.c"
#include "BUGGY_PIC18_FUNCTIONS.c"
#include "BUGGY_PIC18_BARGRAPH.c"

/*******************************************************************************
* Globals
*******************************************************************************/
unsigned int i;
unsigned int Distance;
const thresh_slow = 100;
const thresh_back = 400;
unsigned char InterruptStatus;
unsigned int SPEED;
unsigned int Avg_proxy;
//==============================================================================

unsigned int VCNL_update_proxi(){
    unsigned int local_distance;
    do{
      Command = VCNL_RdSingle(REGISTER_COMMAND);
    // data ready?
    }while (!(Command & (COMMAND_MASK_PROX_DATA_READY | COMMAND_MASK_AMBI_DATA_READY)));
    // read interrupt status register
    InterruptStatus = VCNL_RdSingle(REGISTER_INTERRUPT_STATUS);
    // check interrupt status for High Threshold
    if(InterruptStatus & INTERRUPT_MASK_STATUS_THRES_HI){
      VCNL_WrSingle(REGISTER_INTERRUPT_STATUS, InterruptStatus);
    }
    // proximity value ready for using
    if(Command & COMMAND_MASK_PROX_DATA_READY){
      // read proximity value
      VCNL_RdSeq(REGISTER_PROX_VALUE, &data_, 2);
      ProxiValue = (data_[0] << 8) | data_[1];
      // check the range of the proximity and update bargraph display
      if(ProxiValue < (AverageProxiValue + 50)){
        BarGraph_Display(1);
        ProxiValue = AverageProxiValue; // to avoid returning negative numbers
      }
      if((ProxiValue > (Avg_proxy + 50)) & (ProxiValue < (Avg_proxy + 100))){
        BarGraph_Display(1);
      }
      if((ProxiValue >= (Avg_proxy + 100)) & (ProxiValue < (Avg_proxy + 200))){
        BarGraph_Display(2);
      }
      if((ProxiValue >= (Avg_proxy + 200)) & (ProxiValue < (Avg_proxy + 300))){
       BarGraph_Display(3);
      }
      if((ProxiValue >= (Avg_proxy + 400)) & (ProxiValue < (Avg_proxy + 500))){
       BarGraph_Display(5);
      }
      if((ProxiValue >= (Avg_proxy + 500)) & (ProxiValue < (Avg_proxy + 700))){
       BarGraph_Display(6); // decizie de liuat inapoi
      }
      if((ProxiValue >= (Avg_proxy + 700)) & (ProxiValue < (Avg_proxy + 1000))){
        BarGraph_Display(7); // decizie de liuat inapoi
      }
      if((ProxiValue >= (Avg_proxy + 1000)) & (ProxiValue < (Avg_proxy + 1500))){
        BarGraph_Display(8);
      }
      if((ProxiValue >= (Avg_proxy + 1500)) & (ProxiValue < (Avg_proxy + 2000))){
        BarGraph_Display(9);
      }
      if(ProxiValue > (Avg_proxy + 2200)){
        BarGraph_Display(10);
      }
      local_distance = ProxiValue - Avg_proxy;
    }
    //return local_distance;
    return ProxiValue;
}

void main() {

LED1_Direction = 0;        // Set direction for LEDs
LED2_Direction = 0;

// BUGGY SELF-TEST
// Flash the LEDS on Clicker 2 for PIC18FJ
LED1 = 1;
LED2 = 1;
Delay_ms(1000);
LED1 = 0;
LED2 = 0;
Delay_ms(500);

// Configure bargraph
HC595_cs_direction  = 0;  // Set directions for control pins
HC595_rst_direction = 0;
HC595_pwm_direction = 0;
SPI1_Init();         // Initialize SPI
HC_RST_fnc();        // Reset bargraph circuit

//Self test for bargraph
for (i = 1; i<11; i++){
  BarGraph_Display(i);
  Delay_ms(1000);
}
BarGraph_Display(0);

// Configure the proximity sensor
// Init I2C
I2C2_Init(100000);
I2C2_Stop();
// Set Default mode of operation for VCNL4000 chip
VCNL_Set_default_Mode();
Avg_proxy = VCNL_measure_average();

Init_Buggy();
Break_Buggy();

  while(1){
    // Normal run
    // Only headlights are on
    Brakelights = 0;
    Headlights = 1;
    SignalL = 0;
    SignalR = 0;
    speed = 200;
    Drive_Buggy(speed);
    LED1 = 0;
    LED2 = 0;

    Distance = VCNL_update_proxi();
    // OBSTACLE DETECTED. APPLY BRAKES
    if ((Distance >= (Avg_proxy + thresh_slow)) & (Distance < (Avg_proxy + thresh_back))){
       Break_Buggy();
       Brakelights = 1;
       Delay_ms(100);
    }

    // TOO CLOSE. TURN BACK
    if (Distance > (Avg_proxy + thresh_back)){
       // slowly go back for 3 seconds while flashing all lights
       Brakelights = 0;
       speed = 200;
       Back_Buggy(speed);
       // go back
       for (i = 0; i < 3; i++){
           VCNL_update_proxi();
           SignalL = 1;
           SignalR = 1;
           Delay_ms(500);
           VCNL_update_proxi();
           SignalL = 0;
           SignalR = 0;
           Delay_ms(500);
       }
       // now turn around
       SignalL = 0;
       speed = 200;
       Turn_Buggy(speed);
       for (i = 0; i < 2; i++){
           VCNL_update_proxi();
           SignalR = 1;
           Delay_ms(600);
           VCNL_update_proxi();
           SignalR = 0;
           Delay_ms(600);
       }
    }
  }
}

VCNL40x0_PIC18_BUGGY.c

/*******************************************************************************
* PIN definitions for BUGGY MikroBus #1 (front-left)
*******************************************************************************/

/*******************************************************************************
* Define some parameters
*******************************************************************************/
#define VCNL40x0_ADDRESS 0x26 // 8bit address
// Registers
#define REGISTER_COMMAND (0x80)
#define REGISTER_ID (0x81)
#define REGISTER_PROX_RATE (0x82)
#define REGISTER_PROX_CURRENT (0x83)
#define REGISTER_AMBI_PARAMETER (0x84)
#define REGISTER_AMBI_VALUE (0x85)
#define REGISTER_PROX_VALUE (0x87)
#define REGISTER_INTERRUPT_CONTROL (0x89)
#define REGISTER_INTERRUPT_LOW_THRES (0x8a)
#define REGISTER_INTERRUPT_HIGH_THRES (0x8c)
#define REGISTER_INTERRUPT_STATUS (0x8e)
#define REGISTER_PROX_TIMING (0xf9)
#define REGISTER_AMBI_IR_LIGHT_LEVEL (0x90)
#define COMMAND_ALL_DISABLE (0x00)
#define COMMAND_SELFTIMED_MODE_ENABLE (0x01)
#define COMMAND_PROX_ENABLE (0x02)
#define COMMAND_AMBI_ENABLE (0x04)
#define COMMAND_MASK_PROX_DATA_READY (0x20)
#define COMMAND_MASK_AMBI_DATA_READY (0x40)
#define COMMAND_MASK_LOCK (0x80)
// Bits in Product ID Revision Register (0x81)
#define PRODUCT_MASK_REVISION_ID (0x0f)
#define PRODUCT_MASK_PRODUCT_ID (0xf0)
#define PROX_MEASUREMENT_RATE_31 (0x04)
#define PROX_MASK_LED_CURRENT (0x3f) // DEFAULT = 2
#define PROX_MASK_FUSE_PROG_ID (0xc0)
#define AMBI_PARA_AVERAGE_32 (0x05) // DEFAULT
#define AMBI_MASK_PARA_AVERAGE (0x07)
#define AMBI_PARA_AUTO_OFFSET_ENABLE (0x08) // DEFAULT enable
#define AMBI_MASK_PARA_AUTO_OFFSET (0x08)
#define AMBI_PARA_MEAS_RATE_2 (0x10) // DEFAULT
// Bits in Interrupt Control Register (x89)
#define INTERRUPT_THRES_SEL_PROX (0x00)
#define INTERRUPT_THRES_SEL_ALS (0x01)
#define INTERRUPT_THRES_ENABLE (0x02)
#define INTERRUPT_ALS_READY_ENABLE (0x04)
#define INTERRUPT_PROX_READY_ENABLE (0x08)
#define INTERRUPT_COUNT_EXCEED_1 (0x00) // DEFAULT
// Bits in Interrupt Status Register (x8e)
#define INTERRUPT_STATUS_THRES_HI (0x01)
#define INTERRUPT_STATUS_THRES_LO (0x02)
#define INTERRUPT_STATUS_ALS_READY (0x04)
#define INTERRUPT_STATUS_PROX_READY (0x08)
#define INTERRUPT_MASK_STATUS_THRES_HI (0x01)
#define INTERRUPT_MASK_THRES_LO (0x02)
#define INTERRUPT_MASK_ALS_READY (0x04)
#define INTERRUPT_MASK_PROX_READY (0x08)

extern unsigned short data_[16];
extern unsigned short rcvData[16];

extern void VCNL_WrSingle(unsigned short wAddr, unsigned short wData);
extern unsigned short VCNL_RdSingle(unsigned short rAddr);
extern void VCNL_RdSeq(unsigned short rAddr, unsigned char *rcvData, unsigned long rLen);
extern void void VCNL_WrSeq(unsigned short wAddr, unsigned char *wrData, unsigned long rLen);

unsigned short data_[16];
unsigned char i_loop;
unsigned char Command;
unsigned int ProxiValue;
unsigned int AverageProxiValue;
unsigned long int SummProxiValue;
unsigned char LoByte, HiByte;
//==============================================================================
// VCNL 4010 functions

/*******************************************************************************
* Writes data to VCNL40x0 - single location
*******************************************************************************/
void VCNL_WrSingle(unsigned short wAddr, unsigned short wData) {

  // issue I2C start signal
  I2C2_Start();
  I2C2_Wr(VCNL40x0_ADDRESS);
  I2C2_Wr(wAddr);
  I2C2_Wr(wData);
  I2C2_Stop();
}

/*******************************************************************************
* Reads data from VCNL40x0 - single location (random)
*******************************************************************************/
unsigned short VCNL_RdSingle(unsigned short rAddr) {

  I2C2_Start();
  I2C2_Wr(VCNL40x0_ADDRESS);
  I2C2_Wr(rAddr);
  I2C2_Repeated_Start();
  I2C2_Wr(VCNL40x0_ADDRESS + 1);
  data_[0] = I2C2_Rd(_I2C_NACK);
  I2C2_Stop();

  return data_[0];
}

/*******************************************************************************
* Reads data from VCNL40x0 - sequential read
*******************************************************************************/
void VCNL_RdSeq(unsigned short rAddr,
                        unsigned char *rcvData,
                        unsigned long rLen) {
char i_loop;

  I2C2_Start();
  I2C2_Wr(VCNL40x0_ADDRESS);
  I2C2_Wr(rAddr);
  I2C2_Repeated_Start();
  I2C2_Wr(VCNL40x0_ADDRESS + 1);
  for (i_loop = 0; i_loop < rLen - 1; i_loop++)
    *rcvData++ = I2C2_Rd(_I2C_ACK);
  *rcvData = I2C2_Rd(_I2C_NACK);
  I2C2_Stop();
}


/*******************************************************************************
* Writes data to VCNL40x0 - sequential read
*******************************************************************************/
void VCNL_WrSeq(unsigned short wAddr,
                        unsigned char *wrData,
                        unsigned long rLen) {
  unsigned short i_loop;

  I2C2_Start();
  I2C2_Wr(VCNL40x0_ADDRESS);
  I2C2_Wr(wAddr);
  for (i_loop = 0; i_loop < rLen; i_loop++)
    I2C2_Wr(*wrData);
  I2C2_Stop();
}

/*******************************************************************************
* Sets default mode of operation for the proximity click
*******************************************************************************/
void VCNL_Set_default_Mode(){
   VCNL_WrSingle(REGISTER_PROX_CURRENT, 20);
   //Delay_ms(5);
   VCNL_WrSingle(REGISTER_PROX_RATE, PROX_MEASUREMENT_RATE_31);
   //Delay_ms(5);
   VCNL_WrSingle(REGISTER_COMMAND, COMMAND_PROX_ENABLE | COMMAND_AMBI_ENABLE | COMMAND_SELFTIMED_MODE_ENABLE);
   //Delay_ms(5);
   VCNL_WrSingle(REGISTER_INTERRUPT_CONTROL, INTERRUPT_THRES_SEL_PROX | INTERRUPT_THRES_ENABLE | INTERRUPT_COUNT_EXCEED_1);
   //Delay_ms(5);
   VCNL_WrSingle(REGISTER_AMBI_PARAMETER, AMBI_PARA_AVERAGE_32 | AMBI_PARA_AUTO_OFFSET_ENABLE | AMBI_PARA_MEAS_RATE_2);
   //Delay_ms(5);
}

/*******************************************************************************
* Measure average of proximity value. It will be used to set threshold value
*******************************************************************************/
unsigned int VCNL_measure_average(){
  SummProxiValue = 0;
  for (i_loop = 0; i_loop < 16; i_loop++) {
    do { // wait on prox data ready bit
      Command = VCNL_RdSingle(REGISTER_COMMAND);
    } while (!(Command & COMMAND_MASK_PROX_DATA_READY)); // prox data ready ?
    VCNL_RdSeq(REGISTER_PROX_VALUE, &data_, 2);
    ProxiValue = (data_[0] << 8) | data_[1];
    SummProxiValue += ProxiValue; // Summ of all measured prox values
  }
  AverageProxiValue = SummProxiValue / 16; // calculate average
  LoByte = (unsigned char)((AverageProxiValue + 20) & 0x00FF);
  HiByte = (unsigned char)(((AverageProxiValue + 20) & 0xFF00) >> 8);
  VCNL_WrSingle(REGISTER_INTERRUPT_HIGH_THRES, HiByte);
  VCNL_WrSingle(REGISTER_INTERRUPT_HIGH_THRES + 1, LoByte);
  return AverageProxiValue;
}