Back Up Robot

Code for autonomous robot with PIR motion detection and ultrasound distance sensor.  Distance sensing decreases the chance of the robot getting stuck.  Robot also plays sounds while moving.

/* 
 * File:   BackupBotMain.c
 * Author: hmikelson
 *
 * Created on January 12, 2019, 11:32 AM
 * 
 * This program is used to control a robot with PIC16f1825 microcontroller
 * Uses a customized circuit board with a 1 watt amplified speaker
 * high intensity LED
 * Ultrasound sensor, and PIR sensor
 * Two motors: Forward, backward, right, left, stop
 * 6V battery power
 * Gear driven wheels
 */

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>

#pragma config MCLRE=OFF,CP=OFF,WDTE=OFF,FOSC=INTOSC
#define _XTAL_FREQ 16000000
unsigned char sPORTA=0b00000000, sPORTC=0b00000000, pwmax=63;

void init()
{
 //Configure GPIO Port
 //RA0=BI2,RA1=BI1,RA2=Stby,RC0=AI1,RC1=AI2,RC2=PWMA&B
 //RC3=PIR,RC4=Trig,RC5=Echo,RA3=NA,RA4=Audio,RA5=LED
 //ANSELC= 0b00000000; //Must disable analog inputs to read digital in
 TRISA = 0b11000000;  //O=all
 TRISC = 0b11101000; //I=3,5
 OSCCONbits.IRCF = 0b1111;
 OSCCONbits.SCS = 0b00;
 //Enable analog input on C3
 ANSELCbits.ANSC3 = 1;
 ADCON0 = 0b00011101; //Channel (CHS4:0 bits 6-2),ADON bit 1 enable Analog
 ADCON1bits.ADCS = 0b111; // clock select dedicated osc 
 ADCON1bits.ADNREF = 0; //neg ref
 ADCON1bits.ADPREF = 0; //config + voltage with vdd
 ADCON1bits.ADFM = 1; //format left justified
 ADRESH = 0x00;        //Init the AD Register
}

void vdelay(int n)
 {
  int i;
  for (i=0;i<=n;i++)
   {
    __delay_us(5);
   }
 }

// Read voltage from input
int read_v()
 {
  int val,v1,v2;
  __delay_ms(1);
  ADCON0bits.GO_nDONE = 1;
  while(ADCON0bits.GO_nDONE == 1);
  return (ADRESH<<8) + ADRESL;
 }

/* 
 * Get distance from ultra sound sensor
 * Sensor on pins C4 Trig, C5 Echo
 * 
 */
int getdist()
 {
  int i=0;
  sPORTC = sPORTC & 0b11101111;
   PORTC = sPORTC;
   __delay_us(40);
  sPORTC = sPORTC | 0b00010000;
   PORTC = sPORTC;
   __delay_us(10);
  sPORTC = sPORTC & 0b11101111;
   PORTC = sPORTC;
  while((PORTCbits.RC5 == 0) && (i<5000)) //Wait but not forever
   {
    i++;
   }
  i=0;
  while ((PORTCbits.RC5 == 1) && (i<1000)) //Wait but not forever
   {
    i++;
   }
  return i;
 }

// Drive robot forward
void RobotFd(int time, unsigned char speed)
 {
  int i, j;
  for (i=0;i<time;i++)
   {
    for (j=0;j<pwmax;j++)
     {
      if (j<speed)
       {
         PORTA = 0b00111110;  // LED,Aud,MCLR,En,BI2,BI1
        sPORTC=0b00000110 | sPORTC;
        sPORTC=0b11111110 & sPORTC;
         PORTC = sPORTC;  // Eco,Trg,PIR,PWM,AI1,AI2
        vdelay(pwmax/speed);
       }
      else
       {
         PORTA = 0b00000110;
        sPORTC = 0b00000010 | sPORTC;
        sPORTC = 0b11111010 & sPORTC;
         PORTC = sPORTC;
        vdelay(pwmax/speed);
       }       
     }    
   }
 }

// Move Robot backward
void RobotBk(int time, unsigned char speed)
 {
  int i, j;
  for (i=0;i<time;i++)
   {
    for (j=0;j<pwmax;j++)
     {
      if (j<speed)
       {
         PORTA = 0b00111101;
        sPORTC = 0b00000101 | sPORTC;
        sPORTC = 0b11111101 & sPORTC;
         PORTC = sPORTC;
        vdelay(pwmax/speed);
       }
      else
       {
         PORTA  = 0b00000101;
        sPORTC = 0b00000001 | sPORTC;
        sPORTC = 0b11111001 & sPORTC;
         PORTC = sPORTC;
        vdelay(pwmax/speed);
       }       
     }      
   }
 }

// Move the robot right for time at speed.
void RobotRt(int time, unsigned char speed)
 {
  int i, j;
  for (i=0;i<time;i++)
   {
    for (j=0;j<pwmax;j++)
     {
      if (j<speed)
       {
         PORTA = 0b00111110;
        sPORTC = 0b00000101 | sPORTC;
        sPORTC = 0b11110101 & sPORTC;
         PORTC = sPORTC;
        vdelay(pwmax/speed);
       }
      else
       {
         PORTA = 0b00000110;
        sPORTC = 0b00000001 | sPORTC;
        sPORTC = 0b11111001 & sPORTC;
         PORTC = sPORTC;
        vdelay(pwmax/speed);
       }        
     }        
   }
 }

//Move the Robot Left for given time and speed.
void RobotLt(int time, unsigned char speed)
{
 int i, j;
 for (i=0;i<time;i++)
  {
   for (j=0;j<pwmax;j++)
    {
     if (j<speed)
      {
        PORTA = 0b00111101;
       sPORTC = 0b00000110 | sPORTC;
       sPORTC = 0b11111110 & sPORTC;
        PORTC = sPORTC;
       vdelay(pwmax/speed);
      }
     else
      {
        PORTA = 0b00000101;
       sPORTC = 0b00000010 | sPORTC;
       sPORTC = 0b11111010 & sPORTC;
        PORTC = sPORTC;        
       vdelay(pwmax/speed);
      }      
    }     
  }
}

// Robot stops for the given amount of time
void RobotSt(int time)
{
    int i;
    for (i=0;i<time;i++)
     {
      PORTA = 0b00000100;
      sPORTC = 0b00000100 | sPORTC;
      sPORTC = 0b11111100 & sPORTC;
      PORTC = sPORTC;
      __delay_ms(10);
     }

}

void main()
 {
  int mt=100,dist=1000,i;
  unsigned char rspeed, raction, rtime, nacts=30, pir;
  init();

  RobotFd(200,pwmax/1.5);
  RobotRt(200,pwmax/1.5);
  RobotBk(200,pwmax/1.5);
  RobotLt(200,pwmax/1.5);
  while(1)
   {
    while (nacts==0)
     {
      RobotSt(200);
      pir = read_v();
      if (pir>50)
       {
        nacts = 30;
       }           
     }
    dist = getdist();
    if (dist<300)
     {
      RobotBk(mt,pwmax/2);
      RobotRt(mt,pwmax/2);
     }
    else
    {
     RobotFd(mt,pwmax/2);
    }
    nacts = nacts - 1;
    raction=(rand() % 5);
    rtime=(rand() % mt)/2+mt/2;
    rspeed=pwmax/((rand() % 15)+1)+pwmax/5;
    switch (raction)
     {
      case 1:
       RobotFd(rtime,rspeed);
       break;
      case 2:
       RobotBk(rtime,rspeed);
       break;
      case 3:
       RobotLt(rtime,rspeed);
       break;
      case 4:
       RobotRt(rtime,rspeed);
       break;
      default:
       RobotSt(rtime);
       break;
      }
  }
 }