.::[Clemson University . ECE.496 . Spring 2004]::..
Back to the Main Page
Our Team | The Pendubot | Our Research
Motor | Encoders | Interface Boards | Links
QNX | QMotor | Design
Pendubot Pictures | Team Members | Pendubot Videos
Bonus Day | Final Competition | Final Presentation
subglobal7 link | subglobal7 link | subglobal7 link | subglobal7 link | subglobal7 link | subglobal7 link | subglobal7 link

.:/ Software /:.

 

A bunch of hardware would not do much if not for some sort of intelligence controlling it all. Our computer had to be able to simutaneously read data incoming from the two optical encoders, do caculations, and then send an instruction to the motor to turn the attached link. Our supplied computers ran a real-time operating system called QNX. On top of the operating system was an control application called QMotor which we used to do the actual interfacing to our hardware. QMotor accepts standard C syntax, so we were able to comfortably write our control code without having to relearn a new programming language.

 

QNX

When we booted QNX we found that it was a fairly straight forward *nix port running a GUI called Photon. Our familiarity with *nix allowed us to quickly become acclaimated to the operating system. The only time we had an issue was QNX was when some bad loops were writting and ran under QMotor. The infinite loop caused both QMotor and QNX to completely freeze - effectively rendering the computer useless. Only a hard reset was able to fix the problem. However, all in all QNX performed exactly like it was designed to, and we had very little problem with.

QMotor

According to the QMotor manual, Qmotor is a software package that "...provides a versatile framework for the implementation of advanced control algorithms as C++ programs." On top of allowing control, QMotor also has the ability to log control data in various formats. QMotor allows the implementation of control programs in software, eliminating the need for DSP boards.

QMotor only runs on QNX v4 and v6.

Design

Our software basically reads data from the encoders, then calculates velocities and positional errors based on desired angles of the links. These values are fed into a matrix P. Our system has a total of 4 states, with 0 being the starting state. With a state of 0, our Pendubot would turn the opposite direction of swing up until it reached a specified angle and then set the system state to 1. A state of 1 tells our control program to send the Pendubot into swing up mode. This allowed our system to gain a little more momentum in our swing up.
Once the swing up has reached a certain specified angle, the system would switch to state 2, our balancing state. State 2 is when our control algorithm kicks in.

One feature of our software is our safety algorithms, essentially states 3 and 4. If during swing up the Pendubot overshot or undershot state 2, our robot would move into states 3 and 4, respectively. This allowed the Pendubot to slowly, and safely, turn the links into a hanging state. This was designed to prevent damage to our Pendubot by being over-exerted.

After much trial and tribulation, we arrived at this final code:

// ========================================================================
// QMotor - A PC Based Real-Time Graphical Control Environment
// (c) 2000 QRTS
// ---------------------------------------------------------------
// Control Program : pendubot.cpp
// Description : ROBOTIC PENDULUM BALANCE CONTROL PROGRAM
// Author : GROUP H
// Start Date : Tue Mar 23 16:45:50 UTC 2004
// ========================================================================

#include "ControlProgram.hpp"
#include "IOBoardClient.hpp"
#include "ButterworthFilter.hpp"
#include "matrix.h"
#include <math.h>
#include <iostream.h>
#include <stdlib.h>

class pendubot : public ControlProgram
{
protected:

int safetyValue, state, channel;
double link1DesiredPosition, link2DesiredPosition, link1Angle, link2Angle, prevLink1Angle, prevLink2Angle;
double position1Error, position2Error, link1Velocity, link2Velocity, controlOutput, kp, kd, kf;
double matrixU[1][1], matrixP[4][1], matrixK[1][4], swingBackAngle, switchAngle;

ButterworthFilter<double> link1VelocityFilter;
ButterworthFilter<double> link2VelocityFilter;

IOBoardClient *iobc;

public:

pendubot(int argc, char *argv[]) : ControlProgram (argc, argv){};
~pendubot () {};

virtual int enterControl();
virtual int startControl();
virtual int control();
virtual int stopControl();
virtual int exitControl();
};

int pendubot::enterControl()
{
registerLogVariable(&link1Angle, "Link 1 Angle", "in Radians");
registerLogVariable(&link2Angle, "Link 2 Angle", "in Radians");
registerLogVariable(&link1Velocity, "Link 1 Velocity", "Estimated Velocity");
registerLogVariable(&link2Velocity, "Link 2 Velocity", "Estimated Velocity");
registerLogVariable(&position1Error, "Link 1 Position Error", "Link 1 Position Error in rads");
registerLogVariable(&position2Error, "Link 2 Position Error", "Link 2 Position Error in rads");
registerLogVariable(&controlOutput, "controlOutput", "Control Output in Nwtn/M");
registerLogVariable(&state, "State", "State Status");
registerControlParameter(&kp, "Kp", "Approx. -3.86");
registerControlParameter(&kd, "Kd", "Approx. 0.527");
registerControlParameter(&kf, "Kf", "Approx. 0.1");
registerControlParameter(&switchAngle, "Switch", "Approx. 0.1");
registerControlParameter(&swingBackAngle, "Swing Back", "Define swingback angle");

clearAllControlParameters();
d_messageStream
<< endl << "----- " << d_applicationName << " -----" << endl << endl;
return 0;
}

int pendubot::startControl()
{
clearAllLogVariables();
const char *ioboardServerName = d_config.getStringEntry("ioBoardServerName", "qrts/iobs0");
iobc = new IOBoardClient(ioboardServerName);

if (iobc->isStatusError())
{
d_status.setStatusError()
<< d_applicationName << ": [pendubot::startControl()] "
<< "Error connecting to IO board server " << ioboardServerName << endl;
delete iobc;
iobc = 0;
return -1;
}

link1VelocityFilter.setCutOffFrequency(100);
link1VelocityFilter.setDampingRatio(1);
link1VelocityFilter.setSamplingTime(d_controlPeriod);
link1VelocityFilter.setAutoInit();

link2VelocityFilter.setCutOffFrequency(100);
link2VelocityFilter.setDampingRatio(1);
link2VelocityFilter.setSamplingTime(d_controlPeriod);
link2VelocityFilter.setAutoInit();

link1DesiredPosition = 3.14;
prevLink1Angle = 0;
prevLink2Angle = 0;
state = 0;

iobc->setEncoderValue(4, 0);
iobc->setEncoderValue(6, 0);
iobc->setDacValue(7,0);
safetyValue = 0;

return 0;
}

int pendubot::control()
{
link1Angle = iobc->getEncoderValue(4) *(0.003141593);
link2Angle = iobc->getEncoderValue(6) *(-0.0015707);

link1Velocity = link1VelocityFilter.filter((link1Angle - prevLink1Angle)/d_controlPeriod);
link2Velocity = link2VelocityFilter.filter((link2Angle - prevLink2Angle)/d_controlPeriod);

position1Error = link1DesiredPosition - link1Angle;
position2Error = link2DesiredPosition - link2Angle;

matrixP[0][0] = position1Error;
matrixP[1][0] = position2Error;
matrixP[2][0] = link1Velocity;
matrixP[3][0] = link2Velocity;

matrixK[0][0] = 15.475;
matrixK[0][1] = 13.175;
matrixK[0][2] = -3.4875;
matrixK[0][3] = -2.1;

prevLink1Angle = link1Angle;
prevLink2Angle = link2Angle;

if(state == 0)
{
if(link1Angle > swingBackAngle)
{
controlOutput = -10;
}
else
{
state = 1;
}
}

if(state == 1)
{
position1Error = link1DesiredPosition - link1Angle;
controlOutput = -3 * (kp*position1Error + kd*link1Velocity - kf*link2Velocity);

if(link1Angle > switchAngle)
{ state = 2;
}
}

if(state == 2)
{
MatrixMul(matrixK, matrixP, matrixU);
controlOutput = -3* matrixU[0][0];

}

if(link1Angle > 3.93)
{
state = 3;
}

if(state == 3)
{
position1Error = 6.283 - link1Angle;
controlOutput = -3 * (-.5*position1Error +5*link1Velocity + 2*position2Error);
if(link1Angle > 6.1 && link1Velocity < .05 && link1Angle < 6.46)
{
return -1;
}
}

if(state == 2 && link1Angle < 2.356)
{
state = 4;
}

if(state == 4)
{
position1Error = 0 - link1Angle;
controlOutput = -3 * (-.5*position1Error + 5*link1Velocity + 2*position2Error);
if(link1Angle < 0.262 && link1Velocity < .05 && link1Angle > -.262)
{
return -1;
}
}

if(controlOutput > 10){
controlOutput = 10;
}
if(controlOutput < -10){
controlOutput = -10;
}

iobc->setDacValue(7, controlOutput);
return 0;
}

int pendubot::stopControl()
{
iobc->setEncoderValue(4, 0);
iobc->setEncoderValue(6, 0);
iobc->setDacValue(7,0);
delete iobc;

return 0;
}

int pendubot::exitControl()
{
return 0;
}

main (int argc, char *argv[])
{
pendubot *cp = new pendubot(argc, argv);
cp->run();
delete cp;
}

 

Home | Introduction | Hardware | Software | Gallery | Results | ©2004 Victor Trac