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