2D mapping using servo and Sharp rangefinder(draft)

Wiring Diagram

20121029-220138.jpg

20121029-220108.jpg

Watch in HD to see my teeny text(Sorry!). Also it gave values of 0 on the 3:56 mark because one of the wires came loose.


// Basic 2D mapping using a servo motor and Sharp rangefinder
// Program written by Gian Lorenzo
// Last edited 29/10/2012
//

#include <libpic30.h>
#include <p30f4011.h>
#include <stdio.h>
#include <xc.h>
#include <i2c.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16);  // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                   // Watchdog timer off
_FBORPOR(MCLR_DIS);               // Disable reset pin

// Function prototypes
void setup();
unsigned int read_analog_channel(int n);
void pause(unsigned long ms);
void forward();
void back();
void forwardnum(unsigned int pdc);
void backnum(unsigned int pdc2);

int main()
{
	setup();
	int voltage;
	int status,x=1,y=1,a=0,b=1,c=0;
	int distance[36];
	int quadrant;
	int angle[5];
	int mag[5];
	int min_angle = 750, max_angle = 2230;
	
	PDC1 = min_angle;
	pause(1000);
	distance[0]=0;
	status=0;
	
	while(1)
	{
		voltage = read_analog_channel(0);
		while(x<35)
		{
			voltage = read_analog_channel(0);
			forwardnum(43);
			pause(300);
			if(voltage < 250)
			{
				distance[x]=0;
				status = 0;
			}
			else
			{
				distance[x]=voltage;
				status =1;
			}
			if(status == 1)
			{
				angle[b]=(x*5);
				mag[b]=distance[x-2];
				c=1;
				a++;
			}
			if(status == 0 && c==1 || (x==35 && c==1))
			{
				angle[b]= (angle[b] - ((a*5)/2))-5;
				c=0;
				b++;
				a=0;
			}
			
			x++;
		}
		if(x == 35)
		{
			printf("%d %d %d %d %d %d
                          %d %d %d %d %d %d
                          %d %d %d %d %d %d
                          %d %d %d %d %d %d
                          %d %d %d %d %d %d
                          %d %d %d %d %d\n\n",
			  distance[0],distance[1],distance[2],distance[3],distance[4],
              distance[5],distance[6],distance[7],distance[8],distance[9],
              distance[10],distance[11],distance[12],distance[13],
              distance[14],distance[15],distance[16],distance[17],
              distance[18],distance[19],distance[20],distance[21],
              distance[22],distance[23],distance[24],distance[25],
              distance[26],distance[27],distance[28],distance[29],
              distance[30],distance[31],distance[32],distance[33],
              distance[34]);
			if(angle[b]>179 || angle[b]<1)
			{
				b=b-1;
			}
			while(b>0)
			{
				if(mag[b]<400 && angle[b] > 90 && angle[b] < 145)
				{
					quadrant = 1;
				}
				if(mag[b]<400 && angle[b] > 45 && angle[b] < 90)
				{
					quadrant = 2;
				}
				if(mag[b]>400 && angle[b] > 0 && angle[b] < 90)
				{
					quadrant = 3;
				}
				if(mag[b]>400 && angle[b] > 90 && angle[b] < 180)
				{
					quadrant = 4;
				}
				printf("Object number %d magnitude = %d \r angle = %d and lies in quadrant %d\n\n", b, mag[b],angle[b], quadrant);
				b=b-1;
			}			
			x++;
		}
		pause(15000);
		PDC1=750;
		pause(2000);
		x=1;
		b=1;
		mag[1]=mag[2]=mag[3]=mag[4]=0;
		angle[1]=angle[2]=angle[3]=angle[4]=0;
	}

    return 0;
}

void forward()
{
	PDC1 = PDC1 + 10;
	pause(.5);
}
void back()
{
	PDC1 = PDC1 - 10;
	pause(.5);
}
void forwardnum(unsigned int pdc)
{
	PDC1 = PDC1 + pdc;
}
void backnum(unsigned int pdc2)
{
	PDC1 = PDC1 - pdc2;
	pause(10);
}

void setup()
{
    // Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART

    // Configure D ports as a digital outputs
    LATD = 0;
    TRISD = 0b11110000;

    // Configure analog inputs
    TRISB = 0x01FF;      /* Port B all inputs */
    ADPCFG = 0xFF00;     /* PORTB 0-7 are analog inputs */
    ADCON1 = 0;          /* Manually clear SAMP to end sampling, start conversion*/
    ADCON2 = 0;          /* Voltage reference from AVDD and AVSS */
    ADCON3 = 0x0005;     /* Manual Sample, ADCS=5 -> Tad = 3*Tcy = 0.1us */
    ADCON1bits.ADON = 1; /* Turn ADC ON */

    // Configure PWM
    // PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * 9470 = 20ms
    PWMCON1 = 0x00FF; // Enable all PWM pairs in complementary mode
    PTCON = 0;
    _PTCKPS = 3;      // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
    PTPER = 9470;     // 20ms PWM period (15-bit period value)
    PDC1 = 0;         // 0% duty cycle on channel 1 (max is 65536)
    PDC2 = 0;         // 0% duty cycle on channel 2 (max is 65536)
    PDC3 = 0;         // 0% duty cycle on channel 3 (max is 65536)
    PTMR = 0;         // Clear 15-bit PWM timer counter
    _PTEN = 1;        // Enable PWM time base
}
// Analogue input reading
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 1.2us
    return ADCBUF0;
}
void pause(unsigned long ms)
{
	__delay32(ms*30000L);
}

Example UART output of the program on the PICkit2 uart tool