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

Advertisements

2 comments on “2D mapping using servo and Sharp rangefinder(draft)

  1. batchloaf says:

    Wow, what a great idea! It’s a brilliant demo as it is, but I wonder if we might have a chat about some simple add-ons in the lab on Thursday? I’m just thinking that it would be awesome to be able to see the 2D distance profile graphically. If we spend a few minutes on it in the lab, I think we could rustle up a very short C or Python program that would receive your incoming data packet after each scan and update a graph on-screen.

    Anyway, well done. Very nice work.

    • Gian Lorenzo says:

      Hey Ted,
      Yes actually I was looking for a way to display the distances in a way that would be more pleasing to the eye. Even make a sort of a 180 degree radar. Something like a graph or even a drawing that shows where the distances are would be cool. And again this is still a draft! Still need to type out the theory. By the way the post on the nunchuck is on it’s way. Moving the servo since thursday night but the values that the nunchuck gives aren’t exactly linear so what’s gonna be on the post is something simple. I’m getting excited just talking about it! And about your 2 nunchuck idea I think I may have a temporary solution if you just want two accelerometers. Cheers Ted.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s