Sensor!!

Sensors!!

As you have noticed in my other blogs I have used a wide variety of sensors to demonsrate each device. These include:

  • Sharp Rangefiner
  • Colour sensor
  • 10K Pot
  • UART communication

Ok, so UART isnt exactly a sensor but the dsPIC can make decisions based on the signal sent via UART (just as it does with other sensors). For this post I wanted to use a different sensor. A loudspeaker.

As you may or may not know a loudspeaker can effectively be used as a microphone. Loudspeakers and microphones function on a similar principal. Both operate with a wire coil inside a magnetic field. While a microphone responds to sound waves and makes an electrical signal, a speaker respnds to electrical signal and produces a sound wave. This means that a speaker can work “in reverse” to act as a microphone.

My Loudspeaker Test

The test carried out used two speakers. The idea was to detect which speaker was closer to the source of a sound which would be indicated by lighting an LED. Red for one speaker, green for the other.

Practical Apllication: This method of sensing (on a more high-tech scale) could be used in security systems. Instead of the conventional way of security using just camera’s, something could be protected by microphones placed in strategic areas. By analysing the magnitudes of the sound an intruder could be located in a building by the computer, this inturn could prompt the security guard what area they are in, eliminating the searching and chance of escape.

By wiring two speakers through an amplifier to AN1 and AN2 the magnitudes can be recorded using standard deviation. The setup needed is shown below:

speakers

IMG_1960

IMG_1962

The code for this is shown below:

//
// dsPIC30F4011 audio DSP example
// Written by Eamos Fennell
// Last updated 04-12-2012
//
// This program uses the Timer 1 interrupt
// to drive sampling of audio signals on
// AN1 and AN2 at a sampling frequency of 10kHz.
// Whenever the sample counter is reset to
// zero, the ISR begins adding new samples
// to the audio buffer until it is full
// (the buffer length is 200), after which
// the ISR continues to be called every
// 100us, but samples are not added to the
// buffer.
//
// The while loop in the main function
// resets the sample counter, then waits
// for it to fill up, at which point it
// begins analysing the audio data. It
// calculates the average sample value
// and estimates the short term signal
// amplitude. Both values are printed
// to the PC via the UART.
//
// Depending on the magnitudes of each
// speaker, an LED will light representing
// one of the speakers closer to the source
// of the sound

#include <xc.h>
#include <stdio.h>
#include <libpic30.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 prototype for analog read
unsigned int read_analog_channel(int n);

// Function prototype for Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void);

// audio data buffer and counter
int n=0;
int v1[200], n1=0;
int v2[200], n2=0;
int read;

int main()
{
	// Declare variables
	int i, test;
	double average1, amplitude1;
	double average2, amplitude2;

	// Make all port D pins outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad=266ns, conversion time=12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART

	// Configure Timer 1
	PR1 = 3000;           // 100us sampling period
	TMR1 = 0;             // Reset Timer 1 counter
	IEC0bits.T1IE = 1;    // Enable Timer 1 interrupt
	T1CONbits.TCKPS = 0;  // Prescaler set to 1:1
	T1CONbits.TON = 1;    // Turn on Timer 1

	/*while(1)
	{
		test = read_analog_channel(3);
		printf("%d \n", test);
		__delay32(20000000);
	}*/

	while(1)
	{
		// Reset sample counter counter

		/*while (read_analog_channel(3) == 0);
		printf("AN3 gone high");
		while (read_analog_channel(3) > 100);
		//printf("AN3 gone low");*/
		n=0;

		while(n<200);

		// Find average (dc level of signal)
		average1 = 0;
		average2 = 0;

		for(i=0 ; i<200 ; ++i)
		{
			average1 = average1 + v1[i];
			average2 = average2 + v2[i];
		}

		average1 = average1 / 200.0;
		average2 = average2 / 200.0;

		// Estimate signal amplitude
		amplitude1 = 0;
		amplitude2 = 0;

		for (i=0 ; i<200 ; ++i)
		{
			// Calculate each sample's distance from
			// the average level and add it to the
			// total
			if (v1[i] < average1)
				amplitude1 = amplitude1 + (average1 - v1[i]);
			else
				amplitude1 = amplitude1 + (v1[i] - average1);

			if (v2[i] < average2)
				amplitude2 = amplitude2 + (average2 - v2[i]);
			else
				amplitude2 = amplitude2 + (v2[i] - average2);
		}
		amplitude1 = amplitude1 / 200.0;
		amplitude2 = amplitude2 / 200.0;

		// Printing message to screen
		printf("%f %f\n", amplitude1, amplitude2);

		// Toggle LED's depending on which amplitude is greater
		if (amplitude1 > amplitude2) LATD=0b1000;
		else if (amplitude1 < amplitude2) LATD=0b0010;
	}

	return 0;
}

// Reading sound
// Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
	// Clear Timer 1 interrupt flag
	IFS0bits.T1IF = 0;

	// Read voltages
	if (n<200)
	{
		v1[n] = read_analog_channel(0);
		v2[n] = read_analog_channel(1);
		n = n + 1;
	}
}

// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
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 = 3.2us
	return ADCBUF0;
}

Standard deviation works by getting the average of the signal and checking how far the signal varies from the average. In this case 200 samples (0-199) are taken and the variations are added up, then devided by 200 too get the average amplitude deviation. An example of two signals, the average of those signals and the deviation from average are shown below:

IMG_1963

Finally a video of the sensors are shown below:

Apologies about the angle of the video, it was the best I could do on my own. The green LED is hard to see lighting but the red LED is clearly visable. It can be seen that they toggle in a silent room as their magnitudes vary across each other (showing that they were well calibrated). It can also be seen that just one LED will light depending on which speaker is excited by sound.

Advertisements
Posted in dsPIC | Tagged , , , , , , , , , , , , , , | 2 Comments

Geared DC Motor!!

DC Motor!!

A DC motor changes a DC voltage/current into a mechanical shaft rotation. By varying the voltage into the motor, the speed of rotation can be controlled. In this example the voltage will be varied using PWM. If the direction of current flow is in the opposite direction, the motor will run in that direction.

As the speed of a DC motor is usually quiet high, they are geared. This reduces the speed of the motor and increases its Torque.

DC motors are used in a large variety of industries. Small hobby motors like I will be using may be used in rc cars, helicopters etc. They are ideal for these as they are small and can be designed to have a high speed or torque depending on gearing, and more obviously, these devices are usually battery opperated (DC).

PWM Control..

As said above, by adjusting the pulse width of the supply to the DC motor, the speed can be controlled. The first thing to do is check that the motor acts as predicted. This is done quiet simply by sending two different pulse widths to the motor with a delay (3 seconds in this case) between each pulse and observing the shift in speeds.

As the period is 20ms. Two pulse widths chosen to give a clear change in speed were 5ms and 19ms.

The set up is shown below:

And the code to achieve this..

//
// A short dsPIC30F4011 DC motor
// Written by Eamos Fennell
// Last updated 30-9-2012
//

#include <xc.h>
#include <libpic30.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

int main()
{
	// Configure port D to outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	// 	 PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
	PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
	PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
	PTMR = 0;             // Clear 15-bit PWM timer counter
	PTCONbits.PTEN = 1;   // Enable PWM time base

	while(1)
	{
		// Set the Motor to travel at two different speeds
		// 3 second Delay between each movement
		LATD=0b0010;

		// 5ms Pulse
		PDC1 = 4688;
		__delay32(90000000);
		// 19ms Pulse
		PDC1 = 17814;
		__delay32(90000000);
	}

	return 0;
}

As with my calculations on the servo motor, the PWM was calculated as follows:

Here is a little video of the result:

RangeFinder Control

Once the motor was acting as expected, a Sharp rangefinder was used to control the speed of the motor. As an object got closer to the rangefinder, the motor would slow down and eventually stop. To achieve this the motor was wired as above, while a sharp rangfinder is wired into an1. This is shown below:

The Code used to achieve this is as follows:


//
// dsPIC30F4011 Rangefinder controlling DC Motor speed
//
// Written by Eamos Fennell
// Last updated 03-10-2012
//

#include <xc.h>
#include <libpic30.h>
#include <stdio.h>
#include <math.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
unsigned int read_analog_channel(int n);

int main()
{
	int voltagerange=0;
	int PDCout=0;
	int calc;

	// Make all port D pins outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;      	  // 1.5ms pulse width on PWM channel 1
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART

	// Declare Max PDC (17814=19ms)
	int PDCmax = 17814;

	while(1)
	{
		// Set Motors Forward
		LATD=0b0001;

		//Read values from Rangefinder and PDC H
		voltagerange = read_analog_channel(1);
		PDCout = read_analog_channel(2);

		// Print values of Rangefinder (0-650) (0-3.1V),
		// Calculated PDC1 from formula (0-17814) (0-19ms),
		// PDC1 sent to motor (0-1023) (0-5V)
		printf("range=%d PDC1calc=%d PCDout=%d\n",voltagerange, PDC1, PDCout);
		__delay32(10000000);

		// Set value for PDC1 to control speed of motor
		PDC1 = PDCmax-((PDCmax)*((voltagerange)/600.0));

	}
	return 0;
}

// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
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 = 3.2us
	return ADCBUF0;
}

The value of 600 was chosen for the scaling value as the range recieved was approximately 0-650du (0-3.1volts).

With 600 as the  scaling factor, it was guaranteed to stop the motor when an object got close.

This can be seen working in the video below:

Colour Sensor Control

To demonstrate the bi-direcional control of the DC motor a colour sensor was added. Assuming that there is only two colours to choose from (black and white), the direction of drive can be determined.

If a colour sensor sends out an infra red signal it gets a voltage back depending on what colour the light reflects back off. This voltage is connected to analog channel 2 and is changed to du (0-1023)  within the PIC. Ideally the colour black will give back 1023du and the colour white will give back 0du. Since just two colours are used here a threshold of above or below 512du (half way) will be used.

See the code below:

//
// dsPIC30F4011
// Control Speed and Direction of a DC motor
// using colour sensor and rangefinder
//
// Written by Eamos Fennell
// Last updated 03-10-2012
//

#include <xc.h>
#include <libpic30.h>
#include <stdio.h>
#include <math.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
unsigned int read_analog_channel(int n);

int main()
{
	int voltagerange=0;
	int cs=0;

	// Make all port D pins outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;      	  // 1.5ms pulse width on PWM channel 1
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART

	// Declare Max PDC (17814=19ms)
	int PDCmax = 17814;

	while(1)
	{
		// Read colour sensor
		cs = read_analog_channel(2);

		//Read values from Rangefinder and PDC H
		voltagerange = read_analog_channel(1);

		// Set Motor Direction
		if (cs <= 512) LATD=0b0010;		// If on white, Drive Forward
        else if(cs > 512) LATD=0b0001;	   // If on black, Reverse

		// Set value for PDC1 to control speed of motor
		PDC1 = PDCmax-((PDCmax)*((voltagerange)/600.0));

	}

	return 0;
}

// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
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 = 3.2us
	return ADCBUF0;
}

The circuit:

Note: Please ignore the upper part of the image (two capacitors and op amp), these are part of another experiment I am working on and I found it easier to leave them in position rather than rewire them again.

Video of bi-directional control:

That pretty much demondtrates how a DC motor can be controlled (speed and direction) using a variety of sensors and PWM.

Posted in dsPIC | Tagged , , , , , , , , , , , | 2 Comments

Stepper Motor!!

So what is a Stepper Motor??

A stepper motor is a brushless DC electric motor that converts digital pulses into mechanical shaft rotation. Every revolution of the stepper motor is divided into a discrete number of steps (Typically 200+). The motor is sent a separate pulse for each step causing the motor to rotate at a precise angle.

They are used in many different areas that requre precise control such as the following:

  • Dot Matrix printers
  • Disk Drives, Floppy Drive
  • CD Drives
  • Robotics
  • Precise Industrial Controls

Our Stepper Motor!!

The first task with the stepper motor was a quick test to see will it act as expected. Quiet a simple program was used to turn the stepper in 90degree steps with a small rest between each step. Below is the setup used:

The code to achieve this:

//
// dsPIC30F4011 Stepper Example
// Written by Eamos Fennell
// Last updated 10-10-2012
//

#include <xc.h>
#include <libpic30.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 rotate(double degrees);
void step_forward();

// global variables
double rotor_angle;
double step_angle;
long step_time;

int main()
{
	// Make all port D pins outputs
	TRISD = 0;

	// Use initial position as angle origin
	rotor_angle = 0;

	// Stepper properties
	step_time = 300000L; // 10ms
	step_angle = 0.17578125; // 360.0 divided by 2048

	// Now rotate the stepper 90 degrees once every second
	while(1)
	{
		rotate(90.0); // Move forward 90 degrees
		__delay32(20000000); // Delay for rest
	}

	return 0;
}
// Rotate function
void rotate(double degrees)
{
	// The target angle is the current angle
	// plus the requested angle of rotation.
	double target_angle = rotor_angle + degrees;

	// Now keep stepping forward
	// until the target is reached.
	while(rotor_angle < target_angle) step_forward();
}
// Step forward function
void step_forward()
{
	// Turn off the previous phase and turn
	// on the next one.
	if (LATD == 0b0001) LATD = 0b0010;
	else if (LATD == 0b0010) LATD = 0b0100;
	else if (LATD == 0b0100) LATD = 0b1000;
	else if (LATD == 0b1000) LATD = 0b0001;
	else LATD = 0b0001;

	// Increment the global angle tracker.
	rotor_angle = rotor_angle + step_angle;

	// Delay to allow rotor to move.
	__delay32(step_time);
}

And a quick video of the example working:

SCARA arm!

Now that the stepper motor is working as intended, to demonstrate the use of the stepper motor I have decided to show a useful (industry) application rather than some sort of lab test. A SCARA arm is a common application of stepper motors. It is used in robotics and other precise industrial control. In its simplest form (X,Y plane) it has two motors (shoulder and elbow), two rigid sections (upper arm and lower arm) and an end effector (hand). By changing the angles of the motor shafts a SCARA arm can reach any point on a particular X,Y plane. Once the lenghts of the rigid sections and the desired X,Y coordinates are known the rotor angles can be calculated. This is called Inverse Kinematics and can be calculated as shown:

.

.

where θ1 is the shoulder angle, θ2 is the elbow angle, l1 and l2 are the lenghts of the rigid sections.

The SCARA arm used was quickly made using two pieces of plastic and two stepper motors. It is by no way mechanically sound but serves its purpose to demonstrate how a SCARA arm may work. To input the X,Y coordinates the PICkit UART tool was used.

The circuit diagram is shown below:

You will notice on the circuit diagram I used RB2-RB5. This was due to a fault on RBo with my chip. Be aware that the the chip has been programmed to use Rb2-RB5 rathar than traditionally starting with RB0 and should be considered if anybody is trying to replicate this work.

The code used is as follows:

//
// dsPIC30F4011 SCARA ARM CONTROLLED BY UART ///
//
// This program takes in X,Y co-ordinates from the UART
// and updates the angle of its to stepper motors to fins its co-ordinates.
//
// Written by Eamos Fennell
// Last Updated 01/11/2012
//

#include <xc.h>
#include <libpic30.h>
#include <stdio.h>
#include <math.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 rotate_m1(double degrees_m1);
void rotate_m2(double degrees_m2);
void step_forward_m1();
void step_forward_m2();
void step_back_m1();
void step_back_m2();
void setup();

// global variables
double rotor_angle_m1, rotor_angle_m2;
double target_angle_m1, target_angle_m2;
double step_angle_m1, step_angle_m2;
long step_time;

int main()
{
	// This is for the UART character being sent down
	double c, x, x1, x2, y, y1, y2;
	double l1=15, l2=15;
	char E, S;

	//Configures pins and sets up UART
	setup();

	// Use initial position as angle origin
	rotor_angle_m1 = 0;
	target_angle_m1 = 0;
	rotor_angle_m2 = 0;
	target_angle_m2 = 0;

	// Stepper properties
	step_time = 300000L; // 10ms
	step_angle_m1 = 0.17578125; // 360.0 divided by 2048
	step_angle_m2 = 0.17578125; // 360.0 divided by 2048

	// degrees value can have a decimal place
	double degrees_m1, degrees_m2;

	while(1)
	{
		// Check if any characters were received via UART
		if (U1STAbits.URXDA == 1)
		{
			// Parse 4 digits
			c = U1RXREG;
			if (c < '0' || c > '9') continue;
			x1 = c - 48;

			while (U1STAbits.URXDA == 0);
			c = U1RXREG;
			if (c < '0' || c > '9') continue;
			x2 = c - 48;

			while (U1STAbits.URXDA == 0);
			c = U1RXREG;
			if (c < '0' || c > '9') continue;
			y1 = c - 48;

			while (U1STAbits.URXDA == 0);
			c = U1RXREG;
			if (c < '0' || c > '9') continue;
			y2 = c - 48;

			// Calculate x and y
			x = ((10*x1)+x2);
			printf("X = %f\n", x);

			y = ((10*y1)+y2);
			printf("Y = %f\n", y);

			// Calculate target angles (Inverse Kinematics) of each motor by reading the UART
			target_angle_m1=((180/3.1416)*(acos( (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ) ));
			target_angle_m2=((180/3.1416)*((atan(y/x)) - (acos( (pow(x,2) + pow(y,2) + pow(l1,2) - pow(l2,2)) / (2*l1*(sqrt(pow(x,2) + pow(y,2))) ) ) ) ));

			//
			printf("Target m1 = %f, Target m2 = %f\n", target_angle_m1, target_angle_m2);

			// The ammount that the rotor needs to turn is the target angle -
			// the current angle of the rotor
			degrees_m1 = target_angle_m1 - rotor_angle_m1;
			degrees_m2 = target_angle_m2 - rotor_angle_m2;

			// Code to make the rotor turn either clockwise or anti-clockwise
			// depending on which is the quickest way to reach the targer angle
			if (degrees_m1 < -180) degrees_m1 = degrees_m1 + 360;
			if (degrees_m1 > 180) degrees_m1 = degrees_m1 - 360;
			if (degrees_m2 < -180) degrees_m2 = degrees_m2 + 360;
			if (degrees_m2 > 180) degrees_m2 = degrees_m2 - 360;

			// Rotates the motor to the specified angle
			rotate_m1(degrees_m1);
			rotate_m2(degrees_m2);

			printf("Position m1 = %f, Position m2 = %f\n", rotor_angle_m1, rotor_angle_m2);
		}
	}
}

void setup()
{
	// Make all port D and port B pins outputs
	TRISD = 0;
	TRISB = 0;

	// Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
}

////////////////////
// DEGREES OF M1 ///
////////////////////
void rotate_m1(double degrees_m1)
{
	// The target angle is the current angle
	// plus the requested angle of rotation.
	double target_angle_m1 = rotor_angle_m1 + degrees_m1;

	// Now keep stepping in the appropriate
	// direction until the target is reached.
	if (degrees_m1 > 0)
	{
		while(rotor_angle_m1 < target_angle_m1) step_forward_m1();
	}
	else
	{
		while(rotor_angle_m1 > target_angle_m1) step_back_m1();
	}

	if (rotor_angle_m1 > 360) rotor_angle_m1 = rotor_angle_m1 - 360;
	if (rotor_angle_m1 < 0) rotor_angle_m1 = rotor_angle_m1 + 360;
}

////////////////////
// DEGREES OF M2 ///
////////////////////
void rotate_m2(double degrees_m2)
{
	// The target angle is the current angle
	// plus the requested angle of rotation.
	double target_angle_m2 = rotor_angle_m2 + degrees_m2;

	// Now keep stepping in the appropriate
	// direction until the target is reached.
	if (degrees_m2 > 0)
	{
		while(rotor_angle_m2 < target_angle_m2) step_forward_m2();
	}
	else
	{
		while(rotor_angle_m2 > target_angle_m2) step_back_m2();
	}

	if (rotor_angle_m2 > 360) rotor_angle_m2 = rotor_angle_m2 - 360;
	if (rotor_angle_m2 < 0) rotor_angle_m2 = rotor_angle_m2 + 360;
}

///////////////////////////////////////
// SET UP STEP FUNCTIONS FOR MOTOR 1 //
///////////////////////////////////////
void step_forward_m1()
{
	// Turn off the previous phase and turn
	// on the next one.
	if (LATD == 0b0001) LATD = 0b0010;
	else if (LATD == 0b0010) LATD = 0b0100;
	else if (LATD == 0b0100) LATD = 0b1000;
	else if (LATD == 0b1000) LATD = 0b0001;
	else LATD = 0b0001;

	// Increment the global angle tracker.
	rotor_angle_m1 = rotor_angle_m1 + step_angle_m1;

	// Delay to allow rotor to move.
	__delay32(step_time);
}

void step_back_m1()
{
	// Turn off the previous phase and turn
	// on the next one.
	if (LATD == 0b0001) LATD = 0b1000;
	else if (LATD == 0b0010) LATD = 0b0001;
	else if (LATD == 0b0100) LATD = 0b0010;
	else if (LATD == 0b1000) LATD = 0b0100;
	else LATD = 0b0001;

	// Decrement the global angle tracker.
	rotor_angle_m1 = rotor_angle_m1 - step_angle_m1;

	// Delay to allow rotor to move.
	__delay32(step_time);
}

///////////////////////////////////////
// SET UP STEP FUNCTIONS FOR MOTOR 2 //
///////////////////////////////////////
void step_forward_m2()
{
	// Turn off the previous phase and turn
	// on the next one.
	if (LATB == 0b000100) LATB = 0b001000;
	else if (LATB == 0b001000) LATB = 0b010000;
	else if (LATB == 0b010000) LATB = 0b100000;
	else if (LATB == 0b100000) LATB = 0b000100;
	else LATB = 0b000100;

	// Increment the global angle tracker.
	rotor_angle_m2 = rotor_angle_m2 + step_angle_m2;

	// Delay to allow rotor to move.
	__delay32(step_time);
}

void step_back_m2()
{
	// Turn off the previous phase and turn
	// on the next one.
	if (LATB == 0b000100) LATB = 0b100000;
	else if (LATB == 0b001000) LATB = 0b000100;
	else if (LATB == 0b010000) LATB = 0b001000;
	else if (LATB == 0b100000) LATB = 0b010000;
	else LATB = 0b000100;

	// Decrement the global angle tracker.
	rotor_angle_m2 = rotor_angle_m2 - step_angle_m2;

	// Delay to allow rotor to move.
	__delay32(step_time);
}

Included in this code you can see functions for step forward and step back for each motor. These are used, along with the rotate functions to ensure the motors turn either clockwise or anticlockwise depending on which way is shorter.

The SCARA arm can be seen working in the video below:

On the video above, three points were used for the demonstration. In reality the SCARA arm could reach any point on this plane within a 21×21 cm square and some areas outside this square. This is calculated as follows:

This can be visually demonstrated by the image below:

.

Posted in dsPIC | Tagged , , , , , , , , , , , , , , | 2 Comments

Servo Motor!!

So what is a Servo Motor??

A Servo is a small device that incorporates a three wire DC motor, a gear train, a potentiometer, an integrated circuit, and an output shaft bearing. Of the three wires that stick out from the motor casing, one is for power, one is for ground, and one is a control input line. The shaft of the servo can be positioned to specific angular positions by sending a coded signal. As long as the coded signal exists on the input line, the servo will maintain the angular position of the shaft. If the coded signal changes, then the angular position of the shaft changes.

Servomotors can come in many different sizes, for many different applications. That said, most servo’s are small as seen above (similar to the one I will use) and are very commonly used in many radio-controlled  model airplanes, cars, boats, and helicopters.

PWM..

Before hooking up any Servo Motors it was time to investigate the PWM configure settings of the code.

 // Configure PWM for free running mode
 //
 //   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
 //   PWM pulse width = (Tcy/2) * prescale * PDCx
 //
 PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
 PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
 PTPER = 9375;         // 20ms PWM period (15-bit period value)
 PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
 PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
 PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
 PTMR = 0;             // Clear 15-bit PWM timer counter
 PTCONbits.PTEN = 1;   // Enable PWM time base

First we activate the PWM by writing a 1 to the first 8 bits in PWMCON1. (0x00FF = ob11111111).

We then need to pick the prescaler value as the input clock to PTMR needs to be scaled. This is done using the control bits PTCKPS in the PTCON SFR. It consists of 2 bits meaning it can have four (binary) values: 0-3 (0=1:1, 1=1:4, 2=1:16, 3=1:64). For more information on this see the dsPIC30f4011 datasheet section 15.1.5

This is how the 20ms period (PTPER) of the PWM is calculated:

To calculate how long the pulse (1.5ms here) is on for we use the following formula:

This can be done for each of the PWM channels (ie. change the pulse lenght). But the period (PTPER) of the PWM’s are all the same as calculated above.

Finally we clear the timer counter by writing 0 to PTMR. This counter can clear itself on some ocassions but when there is a value written to PTCON (prescaler) this does not clear and so must be cleared manually.

We can verify the calculations above using the UART tool as shown below.

From the screenshots above of the results of our UART logic analyser it can be seen that the 20ms period expected turned out to be 20.32ms and 1.5ms pulse turned out to be 1.56ms. This works out as 1.6% and 4% error respectively. This is a very acceptable result for the applications used here. Errors can be put down to rounding off errors and clock speed errors. For more accurate applications an external quartz crystal clock may be used.

Our Servo Motor…

So the first thing we did with the Servo Motor was a quick test. Using code supplied by Ted, we sent three different numbers (Pulses) down the motor. These pulses represented a certain angle to which the Servo would turn. This is the code used..

//
// A short dsPIC30F4011 servo example
// Written by Eamos Fennell
// Last updated 30-9-2012
//

#include <xc.h>
#include <libpic30.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

int main()
{
	TRISD = 0;

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
	PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
	PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
	PTMR = 0;             // Clear 15-bit PWM timer counter
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Flash LED for 1 Second on Start-up
	_LATD0 = 1;
	__delay32(30000000);
	_LATD0 = 0;

	while(1)
	{
		// Set the Motor to travel to three positions
		// One second Delay between each movement
		PDC1 = 800;
		__delay32(30000000);
		PDC1 = 1200;
		__delay32(30000000);
		PDC1 = 1600;
		__delay32(30000000);
	}

	return 0;
}

Connnecting the Servo..

You will note in the code above how I flash the LED for one second when the chip first starts up. I added this in as a test function as when tested, my Servo was not acting as predicted.

I will explain this a little further down. For now, this is how the circuit looked when built..

And this is my circuit diagram of my circuit.

So why the LED? As I said sometimes the Servo worked fine and found its 3 angular positions without a problem. However sometimes it did not. You may have noticed from the images above that the Servo was been powered directly from the PICkit2. If the current drain was too high in the motor, this would cause the chip’s voltage to drop and reset. This would be apparent by the flash of the LED. Here is a little video of one instance of the dsPIC resetting.

A more common thing to do is power the Servo Motor from its own supply. However as one was unavailable at the time I needed to resort to this method.

Servo 180° Test

Once the Servo was operating correctly I used ‘Trial and Error’ to find the 0° and 180° positions.. This was done by changing the value of PDC1 until 180° revolution was achieved. This turned out to be 435 and 2075 as shown in the code below.

//
// dsPIC30F4011 Servo 180 degree example
// Written by Eamos Fennell
// Last updated 03-10-2012
//

#include <xc.h>
#include <libpic30.h>
#include <stdio.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
unsigned int read_analog_channel(int n);

int main()
{

	// Make all port D pins outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART

	while(1)
	{
		// PDC1 = 435 @ 0 degrees
		// Delay 1 second
		PDC1=435;
		__delay32(30000000);
		// PDC1 = 2075 @ 180 degrees
		// Delay 1 second
		PDC1=2075;
		__delay32(30000000);
	}

	return 0;
}

Here is a video of the 180° revolution working.

Now that the parameters are known we can use the Servo safely without the risk of straining it by turning the rotor shaft too far.

Servo controlled by Pot..

As shown in the images below a pot was connected to AN0 (pin 2) via the pink cable.

The idea here was to have a user input controlling the position of the rotor angle on the Servo. As the pot changes from 0V to 5V, the rotor angle would change from 0° to 180°. This is done by connecting the pot to AN0 and changing the voltage to a digital 10bit number (0-1023du). It is worked out using the following formula:

PDCmin and PDCmax were worked out in the 180° Servo test previously. This can be seen working in the video below:

This is the code used to achieve this:

//
// dsPIC30F4011 potentiometer and servo example
//
// Written by Eamos Fennell
// Last updated 03-10-2012
//

#include <xc.h>
#include <libpic30.h>
#include <stdio.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
unsigned int read_analog_channel(int n);

int main()
{
	int voltage = 0;

	// Make all port D pins outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART

	// Declare Min/Max PDC
	int PDCmin = 435, PDCmax = 2075;

	while(1)
	{
		// Read analog voltage from AN0 (pin 2)
		// The number returned (between 0 and 1023)
		// represents a voltage in the range 0-5V.
		voltage = read_analog_channel(0);

		// If the voltage is greater than 2.5V
		// turn on the LED on RD0 (pin 23),
		// otherwise turn it off.
		if (voltage < 512) _LATD0 = 0;
		else _LATD0 = 1;

		// Display the voltage reading on screen
		printf("Voltage = %d\n", voltage);

		// Calculate and update the servo angle
		// based on potentiometer voltage on AN0.
		// In this case we want voltage values
		// between 0 and 1023 to be mapped onto
		// PDC1 values between 435 and 2075.
		PDC1 = PDCmin + (PDCmax-PDCmin)*(voltage/1023.0);

		// Delay 1/10 seconds.
		// This allows for our printed message (voltage)
		// to be printed without distortion.
		// (Without delay it can become disorted as there is not enough time to print)
		__delay32(3000000);

	}

	return 0;

You will notice in the code I set the LED to come on at 512du. This should represent 90°. When looking at the video above you can clearly see the LED toggle at 90°. Proof that the calculations are working accuratly.

Posted in dsPIC | Tagged , , , , , , , , , , | 4 Comments

Hello World!!

Hello! and Welcome to my Blog.

My name is Eamos Fennell. I am a 3rd year Electrical and Control Engineering student in Dublin Institute of Technology (DIT Kevin Street). One module I am taking is Robotics 3.1.

This Blog will be used to document the followig topics as we cover them:

  • Servo motor
  • Stepper motor
  • Geared DC motor
  • Sensors
  • Any other relevent items used

The type of things documented will be Theory, C-Code, Wiring Diagram’s (using Inkscape), Photo’s, Video links, Maths and anything else relevant to the subject.

A Mini-Project will also be documented on its own page.

Thanks you for visiting. If you have any questions/comments dont be shy.

 

Posted in Uncategorized | Leave a comment