Skip to content

Instantly share code, notes, and snippets.

@Electronza
Created December 16, 2019 10:20
Embed
What would you like to do?
Mikroe Buggy: Obstacle Avoiding Robot 0
/*******************************************************************************
* 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();
}
// =============================================================================
/*******************************************************************************
* 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);
}
#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);
}
}
}
}
/*******************************************************************************
* 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment