Difference between revisions of "Furuta Pendulum"

From Mech
Jump to navigationJump to search
Line 330: Line 330:


== Results ==
== Results ==
The pendulum ultimately worked! The vertical pendulum arm was kept upright by the horizontal adjustments of the arm attached to the motor shaft. It took a couple of alterations to the code and electrical setup to get the motor to react the right way, and for some reason it only worked with a power supply, not when power was supplied from the PIC, but as the video shows, the motor does keep the vertical arm up. See the [http://www.youtube.com/watch?v=7DtFLKgNUk4 full demonstration video]
The pendulum ultimately worked! The vertical pendulum arm was kept upright by the horizontal adjustments of the arm attached to the motor shaft. It took a couple of alterations to the code and electrical setup to get the motor to react the right way, and for some reason it only worked with a power supply, not when power was supplied from the PIC, but as the video shows, the motor does keep the vertical arm up. See the [http://www.youtube.com/watch?v=7DtFLKgNUk4 full demonstration video] [[Image:working furuta.png|humb|300 px|A working furuta pendulum captures in motion|center]]


==Future Steps==
==Future Steps==

Revision as of 12:13, 17 March 2010

Overview

We were tasked with constructing and programming a Furuta pendulum, the goal of which is to hold the vertical arm upright through horizontal rotation of the arm connected to the motor shaft. Essentially, there were two separate projects: constructing the physical pendulum assembly, and programming the PIC to control the motor so the pendulum would function. The basic mechanism consists of an encoder measuring the angle of rotation of the vertical arm. The PIC receives this information and controls the motor so as to move the horizontal arm in order to ultimately keep the pendulum in the vertical position. Click here for the final demonstration video.

Inverted pendulum maintaining balance using feedback control

Team Members

  • Matthew Luther (Electrical Engineering, Class of 2010)
  • Krystian Zimowski (Mechanical Engineering, Class of 2010)
  • Gabriel Haack (Mechanical Engineering, Class of 2011)

Mechanical Design

There are three sections of the pendulum that can each be addressed separately: the base assembly with motor housing (including the motor itself); the arm assembly, which includes the horizontal arm, the vertical arm, and the encoder apparatus; and the box into which the whole pendulum was placed, including mechanical stops, the PIC and electrical circuitry, and a button to start the program. Each section begins with a parts list; generic terms like "block" or "slab" indicate that these parts were made from aluminum scraps and can be just as effective with a different size or shape, or that they can be easily machined to fit any design.

Aluminum was used for most parts because it was readily available, easy to machine, light, and mechanically robust. We wanted as light a design as possible because the motor we used was had no gearhead, so the torque was naturally going to be lower. We decided to use the motor with no gearhead to avoid any backlash since the pendulum requires rapid adjustments in motor position and direction to keep the vertical arm up. However, we also wanted to minimize shaking from the pendulum operation, so the base had to be fairly heavy and sturdy.

Base sub-assembly to keep the motor upright and stabilize the pendulum

Base Assembly

The aluminum block used was cylindrical: a hole was end-milled into the center to fit the encoder on the motor. As you can see, the motor fits right in, preventing any rotation of the motor itself. Two holes were drilled into the block on either side of the motor hole for the support columns, which were cut to be even with the top of the motor casing (not the motor shaft) once they were press-fit into the holes. The tops of the columns were drilled and tapped. Acrylic was cut to the size of the turntable and clearance holes were drilled to screw it to the motor, the support columns, and the turntable and a hole was cut in the center for the motor shaft. Once the support columns were in place, the motor was placed in the base, the platform was screwed onto the motor and columns, and the turntable was screwed onto the platform.

Horizontal arm attached to the motor shaft and lazy susan
Encoder mounted in between two aluminum blocks with bearings

Horizontal and Vertical Arm Assemblies

  • Horizontal arm - 3/4" hollow aluminum tubing
  • Vertical arm - 1/2" hollow aluminum tubing
  • Press-fits for set screws - solid aluminum tubing with diameters equal to the inner diameter of the arms
  • Weight - small aluminum block
  • Bearing supports - flat aluminum slab
  • Encoder shaft - machined solid aluminum tube
  • Bearings - McMaster-Carr 1/4" ID 7/16" OD Needle Roller Bearings
  • Encoder - AME 1000V 600k

The horizontal arm was cut to be long enough to stick out well past the edge of the turntable. A short piece of aluminum stock was press-fit into one end, and a hole was drilled through the arm for the motor shaft to slip in and another perpendicular to it for a set screw. The biggest issue to overcome was translating the rotation of the pendulum arm into a motion that the encoder could read and detect. Encoders are usually mounted on a motor shaft in order to measure its angular rotation and position, therefore a similar setup was needed. This was accomplished by attaching a horizontal shaft to the vertical pendulum arm and mounting it between two aluminum blocks that contained bearings, allowing for free rotation of the shaft. The encoder was mounted in between both blocks and was attached to the shaft. This way the encoder could detect the position of the vertical pendulum arm.

The encoder shaft was set screwed onto and rotates with the vertical arm, providing a mechanism to send the angle of rotation back to the PIC. As you can see, the horizontal arm was cut in half with a band saw for a length of the shaft sufficient to fit the encoder and both bearing supports. To place the supports on the arm, the same tube used for the press-fit was machined to fit in the channel remaining after the top was sawed off. One piece was placed where each of the bearing supports would be, super-glued on, and end-milled to be perfectly flat on top. Clearance holes were drilled in each piece and the bottom of the supports were drilled and tapped so they could be screwed onto the arm. The bearings which support and stabilize the encoder shaft were press-fit into holes in the supports, and the encoder casing was screwed onto one of the supports. The picture shows where each part is in relation to the others. Meanwhile, the weight was drilled through so it would slide onto the vertical shaft, and a hole for a set screw was drilled and tapped.

Box assembly to enclose the pendulum and provide mechanical stoppers on either side

Box Assembly

  • Box and top - plywood
  • Dowel pins
  • Rubber bumpers
  • Paint
  • Base stand - wooden blocks

The box was not a critical part of the design in that it didn't affect the pendulum itself, but it was very important for aesthetics, for providing mechanical stops, and keeping the project together in one compact design. The box needed to be large enough to accommodate the electrical components, and the base stand elevated the pendulum so it wouldn't interfere with the circuit. Plywood was nailed together for the bottom and sides, and the top was machined so the pendulum was free to swing. Dowel pins were placed on top of the sides to match with holes drilled in the top so once everything was ready, the top could be put on to protect the components inside. The mechanical stops were glued onto the sides of the box with two set of bumpers: one for the pendulum to hit once it was on and in the vertical position and another to stop the horizontal arm if it was swinging out of control. The wooden blocks used for base stand were screwed together and glued to the bottom of the box, and at the end the pendulum was epoxied to the top of the stand. The entire box was painted before the pendulum was attached.

Electrical Design

The main function needed to be achieved through the electrical design process was to control the motor speed and direction by reading the angular position of an encoder attached to the vertical pendulum arm.

Wiring for the pendulum enclosed inside the box.

Controller

A PID controller for this project was chosen for multiple reasons. Firstly, a PI controller (proportional and integral) was chosen. The proportional term is a vital part to any controller, and the integral control is perfect for our desire of a steady state. The integral term allows our motor to hold the pendulum vertical, and eventually balance it – whether or not the balancing uses the motor once a steady state is found depends on how well constructed the apparatus is. If the pendulum has high friction and is evenly weighted, the controller will easily find a steady state. The derivative term was added because we knew that how fast the pendulum is swinging should be factored into the controller in a predictive measure – rather than the purely responsive manner that a proportional response allows.

Control Law

The control law we found is far from perfect. Through trial and error it was found that 240*error+20*sum+30*velocity worked sufficiently for the purposes of the project. In order to optimize this, many different methods could be employed. For the best control law, a set of differential equations governing the movement of the pendulum (vertical arm) dependent upon the movement of the horizontal arm would be needed. Less sophisticated methods would require the program randomly generating gains by itself, running the controller, and comparing the results in terms of watts used over time (or storing that data then sending it back to the PC). This would allow a measure of refinement, but at essence is still trial and error.

Circuit

Our circuit was nearly identical to the circuit diagram from the Mechatronics Lab 4. All that was added was an input to the PIC triggered by a push button attached to 3.3V.

Furuta circuit.png

The motor-encoder circuit can also be downloaded here


Code

Most of our code is taken from lab 4. We pared it down by removing all the RS232 communications functions, and decided to not use interrupts to run our control process, so all of those were removed. We mainly added constants, and modified the code inside the while loop. Our program runs completely inside the while loop, whereas lab 4 ran completely outside of the while loop. You can download the working version of the code here

The complete copy of the copy can be found below:

/* 
Team 12 Final Project
Furuta Pendulum

*/

/** INCLUDES ***************************************************/
#include "HardwareProfile.h"
//#include "HardwareProfileNU_32.h"


/** Constants **************************************************/
#define TRUE 					1
#define FALSE					0

// PWM
#define ENABLE_PIN				LATAbits.LATA2
#define DIRECTION_PIN			LATAbits.LATA3
#define START_BUTTON			PORTFbits.RF2 // start program button
#define LOOP_TIME_PIN			LATAbits.LATA14
#define MAX_RESOLUTION			0x0F9F		// Proportional to period of PWM

// Directions
#define FORWARD					0
#define REVERSE					1

// RS232
#define DESIRED_BAUDRATE    	(19200)      // The desired BaudRate 
#define NUM_DATA_POINTS 		640


/** Function Declarations **************************************/
void initMotorPWM();
void initInterruptController();
void initEncoder(void);
int getEncoderPosition(void);
int getReference(int index);
unsigned int setPWMandDirection(signed int error);
unsigned int getPWMmagn(signed int error);
void initUART2(int pbClk);
void sendDataRS232(void);


/** Global Variables *******************************************/
// Encoder
signed int bigcount = 0; 	// set encoder value initially to zero, it can go + or -
						 	// 32 bit number
short last0 = 0, last1 = 0; // 16 bit number, prev tmr4 and tmr5

// Controller
int globalIndex = 0;  // data point index
int refPeriod = 10000; // period in ms
int refAmplitude = 200; // in encoder counts


int offset = 100; // feedback offset (dead zone)
int sum = 0;  //initialize value
int Kp = 240; //proportional gain was operating at 200
int Ki = 20;  //integral gain
int Kd = 30;  //derivative gain

int num_data = 2;
int data_arr[2]; // 0 to num_data-1
int iteration = 10; // a 1.8deg change on a 500 quad encoder
int j;
	//encoder count goal - where the controller will attempt to hold the controller at
int goal = 0;
int current=0;
int movement=0;
int difference=0;
int anti_windup = 10;

int error=0;


int encoderCounts[NUM_DATA_POINTS]; // initialize array to hold encoder data
int referenceData[NUM_DATA_POINTS]; // initialize array to hold reference data



/** Main Function **********************************************/
int main(void)
{
	int	pbClk;
	//leave H Bridge off
	ENABLE_PIN = 0;
	//Set pin F2 to digital input
	TRISFbits.TRISF2 = 1;
	// Initialize board LEDs
	mInitAllLEDs();

	// Initialize PWM
	initMotorPWM();

	// Initialize encoder
	initEncoder();


	goal = 0;
	//initialize the velocity array to 0.
	data_arr[0]=0;
	data_arr[1]=0;
	while(1) // run forever
	{
		//resets loop index for derivative portion of control equation
		if (iteration > num_data)
			{iteration = 0;}

		//get the current position from the encoder
		current = getEncoderPosition();		
		//Resets encoder position and turns on H bridge when star button is pressed
		if (START_BUTTON == 1)
		{
			ENABLE_PIN = 1;
			bigcount = 0;
		}

		//turns off H bridge and thus motor if the pendulum wobbles too far.  Prevents a burn out from occurring
		if ((current > (49+goal)) && (current < (goal-49)))
			{ENABLE_PIN = 0;}

		//The guts of the program
		else if (ENABLE_PIN == 1)
		{
			//two values needed for the control equation
			error = goal - current;
			sum = sum + current;
			//stores last and current iteration of encoder position for derivative control
			data_arr[iteration] = current;
			
			//Anti wind-up parameters needed whenever using integral control
			if (sum>anti_windup)
				sum=anti_windup;
			if (sum<-anti_windup)
				sum = -anti_windup;
	
			//Actual control equation
			movement = Kp*error+Ki*sum+Kd*(abs(data_arr[0] - data_arr [1])); 
			//makes sure hte movement is in bounds
			if (abs(movement) > MAX_RESOLUTION)
				movement = MAX_RESOLUTION;
			
			//to rotate the motor backwards, a different PWM is needed
			if (error < 0 )
			{
				DIRECTION_PIN = REVERSE;
	 			SetDCOC1PWM(MAX_RESOLUTION-abs(movement));
			}
			//rotate the motor forwards
			if (error > 0 )
			{
				DIRECTION_PIN = FORWARD;
	 			SetDCOC1PWM(movement);
			}
			
			//move the counter for derivatrive control forward.
			iteration = iteration + 1;
		}
	}

	CloseOC1();
} //end main



/** Other Functions:  From Lab 4 ********************************************/
void initMotorPWM(void)
{
	//Set Enable, Direction and Loop Time Pins (A2, A3, A14) as digital outputs
	// Initialize as low
	LATA &= 0xBFF3; TRISA &= 0xBFF3;
	
	// init OC1 module, on pin D0
	OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
	
	// init Timer2 mode and period (PR2) // set for 20kHz
	OpenTimer2( T2_ON | T2_PS_1_1 | T2_SOURCE_INT, 0x0F9F); //0F9F = 3999, prescale = 1
}


void initEncoder(void)
{
	// init Timer4 and Timer5 mode and periods (PR4, PR5)
	OpenTimer4( T4_ON | T4_PS_1_1 | T4_SOURCE_EXT, 0xFFFF); 
	OpenTimer5( T5_ON | T5_PS_1_1 | T5_SOURCE_EXT, 0xFFFF); 
} 

int getEncoderPosition()
{
	short count0 = ReadTimer4();  // in your routine this must be done at least every 32000 encoder counts to avoid rollover ambiguity
	short count1 = ReadTimer5(); 	

	bigcount += count0 - last0; // add on the recent up-counts, since the last time

	// check for rollover
	if (count0 < last0)
	{
		bigcount += 65536; // count0 only increments, so if it got lower it must have rolled over
	}

	last0 = count0;

	bigcount -= count1 - last1; // we're not worrying about rollover of the 32 bit bigcount total

	if (count1 < last1)
	{
		bigcount -= 65536;
	}

	last1 = count1; 

	return bigcount;
 }

int getReference(int index)
{
	// get what reference signal should be given the globalindex
	if(index > refPeriod/2)
	{
		return refAmplitude;
	}
	else
	{
		return -refAmplitude;
	}

}

unsigned int setPWMandDirection(signed int error)
{
	unsigned int pwmMagn;
	
	pwmMagn = getPWMmagn(error);
	
	if (error > 0) 					// Go Forward r > y
	{
		DIRECTION_PIN = FORWARD;
		mLED_2_On();
        SetDCOC1PWM(pwmMagn);
	}
	else						// Go Reverse r < y
	{
		DIRECTION_PIN = REVERSE;
        mLED_2_Off();
		SetDCOC1PWM(MAX_RESOLUTION - pwmMagn);
	}
	
	return pwmMagn;

}

unsigned int getPWMmagn(signed int error)
{
	unsigned int pwmMagn = abs(error) * Kp + offset; // Proportional Controller

	// condition ? value if true : value if false
	return pwmMagn > MAX_RESOLUTION ? MAX_RESOLUTION : pwmMagn;
}

Results

The pendulum ultimately worked! The vertical pendulum arm was kept upright by the horizontal adjustments of the arm attached to the motor shaft. It took a couple of alterations to the code and electrical setup to get the motor to react the right way, and for some reason it only worked with a power supply, not when power was supplied from the PIC, but as the video shows, the motor does keep the vertical arm up. See the full demonstration video

A working furuta pendulum captures in motion

Future Steps

These would include optimization as mentioned above, and a “swing up” method. A swing up method would benefit greatly from equations of motion for the system, but could probably be done by reversing swing direction when velocity is 0. For this, I would recommend storing more than two data points, as we did. For this project, 360 degrees of freedom is recommended for the apparatus.

Reflections

We managed to burn out one PIC chip when using a higher quality H-Bridge. This was because of a wiring mistake that ran 12V through the 3.3V output, and into the connected laptop which caused a crash. Fortunately, no permanent damage seems to be done to the laptop. We did not reach a point where we even truly considered trying to code a swing-up operation, which would have been really cool, but also really complicated and there simply wan't enough time. However, the physical pendulum is still intact, so if anyone would like to work with it I'm sure professor Lynch would be willing to let you try it out.