Code-On: Robot

Sunday, March 3, 2019

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;
}
}
}

Sunday, September 9, 2018

Robot Code

This is the operating code for a robot I built called Dumbot. It only has a motion sensor and moves randomly. The code generates three random numbers: speed, time, movement type. There are five different movement types: Forward, backward, turn right, turn left, and stop. There is a check routine at the beginning: Forward, right, backward, left. To check to make sure the motors are installed properly. This code was tested on a PIC16F1825, using hobby motors from Sparkfun and the brushed motor controller breakout board from Sparkfun. A switched battery holder with three double A batteries from Jameco was used. The wheels and frame were 3D printed. An LED output and an audio output were included.

* File: R06Main.c

* Author: hmikelson

* Created on September 4, 2018, 6:11 PM

#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, sPORTC, pwmax=31;

void init()

{

//Configure GPIO Port

TRISA = 0b110000000; //Set all Output

TRISC = 0b110001000; //Set up pin 3 for input

OSCCONbits.IRCF = 0b1111;

OSCCONbits.SCS = 0b00;

//OPTION_REG = 0b01111111; //Global enable weak pullups

//WPUC = 0b00001000; //Enable weak pullups with a 1

// Set up analog input

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

ADRESL = 0x00; // I don't think this is needed

//OSCCONbits.SPLLEN = 0b1;

}

void vdelay(int n)

{

int i;

for (i=0;i<=n;i++)

{

__delay_us(100);

}

int read_v()

// Read voltage from input

{

int val,v1,v2;

__delay_ms(1);

ADCON0bits.GO_nDONE = 1;

while(ADCON0bits.GO_nDONE == 1);

v1 = ADRESH;

v2 = ADRESL;

val = (v1<<8) + v2;

return val;

}

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

PORTC = 0b00010110; // Enable on pin 4

vdelay(pwmax/speed);

}

else

{

PORTA = 0b00000110; // I think there is a clock output on pin 4 which I don't know how to disable

PORTC = 0b00010010; // Enable on pin 4

vdelay(pwmax/speed);

}

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

PORTC = 0b00010101;

vdelay(pwmax/speed);

}

else

{

PORTA = 0b00000101;

PORTC = 0b00010001;

vdelay(pwmax/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;

PORTC = 0b00010101;

vdelay(pwmax/speed);

}

else

{

PORTA = 0b00000110;

PORTC = 0b00001001;

vdelay(pwmax/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;

PORTC = 0b00110110;

vdelay(pwmax/speed);

}

else

{

PORTA = 0b00000101;

PORTC = 0b00001010;

vdelay(pwmax/speed);

}

void RobotSt(int time)

{

int i;

for (i=0;i<time;i++)

{

PORTA = 0b00000100;

PORTC = 0b00001100;

__delay_ms(10);

}

void main()

{

int rmode, rtime, volts,mt=31;

unsigned char rspeed, nactions=0;

init();

RobotFd(100,pwmax/2);

RobotSt(1);

RobotRt(100,pwmax/2);

RobotSt(1);

RobotBk(100,pwmax/2);

RobotSt(1);

RobotLt(100,pwmax/2);

RobotSt(1);

while(1)

{

if (nactions > 0)

{

rmode=(rand() & 7) + 1;

nactions=nactions-1;

}

else

{

rmode = 0;

}

rtime = (rand() & 7)+1;

rspeed = rand() & pwmax;

switch (rmode)

{

case 1:

RobotFd(rtime*mt,rspeed);

RobotSt(1);

break;

case 2:

RobotBk(rtime*mt,rspeed);

RobotSt(1);

break;

case 3:

RobotRt(rtime*mt,rspeed);

RobotSt(1);

break;

case 4:

RobotLt(rtime*mt,rspeed);

RobotSt(1);

break;

case 0:

RobotSt(1);

volts = read_v();

if (volts>50)

{

nactions = 10;

//blink(volts);

}

break;

default:

RobotSt(10);

break;

}