RobotControl.cpp

00001 #include "RobotControl.h"
00002 
00003 using namespace std;
00004 
00005 RobotControl::RobotControl(MaxonESCON* motorControllerLeft,
00006                            MaxonESCON* motorControllerRight,
00007                            float period) : Task(period)
00008 {
00009     /* get peripherals */
00010     this->motorControllerLeft = motorControllerLeft;
00011     this->motorControllerRight = motorControllerRight;
00012     this->period = period;
00013 
00014     /* initialize peripherals */
00015     motorControllerLeft->enable(false);
00016     motorControllerRight->enable(false);
00017 
00018     /* initialize remaining state values */
00019     speed = 0.0f;
00020     omega = 0.0f;
00021 
00022     motorControllerLeft->setPulses(0);
00023     motorControllerRight->setPulses(0);
00024 
00025     motorControllerLeft->setVelocity(0.0f);
00026     motorControllerRight->setVelocity(0.0f);
00027 }
00028 
00029 RobotControl::~RobotControl()
00030 {
00031 
00032 }
00033 
00034 float RobotControl::PiRange(float theta)
00035 {
00036     if(theta <= -PI) {
00037         return theta  += 2*PI;
00038     } else if (theta > PI) {
00039         return theta  -= 2*PI;
00040     } else {
00041         return theta;
00042     }
00043 }
00044 
00045 void RobotControl::setEnable(bool enable)
00046 {
00047     motorControllerLeft->enable(enable);
00048     motorControllerRight->enable(enable);
00049 }
00050 
00051 bool RobotControl::isEnabled()
00052 {
00053     return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
00054 }
00055 
00056 void RobotControl::setDesiredSpeed(float speed)
00057 {
00058     this->speed = speed;
00059 }
00060 
00061 void RobotControl::setDesiredOmega(float omega)
00062 {
00063     this->omega = omega;
00064 }
00065 
00066 void RobotControl::setDesiredxPosition(float xposition)
00067 {
00068     Desired.xposition = xposition;
00069 }
00070 
00071 void RobotControl::setDesiredyPosition(float yposition)
00072 {
00073     Desired.yposition = yposition;
00074 }
00075 
00076 void RobotControl::setDesiredTheta(float theta)
00077 {
00078     Desired.theta = theta;
00079 }
00080 
00081 float RobotControl::getDesiredxPosition()
00082 {
00083     return Desired.xposition;
00084 }
00085 
00086 float RobotControl::getDesiredyPosition()
00087 {
00088     return Desired.yposition;
00089 }
00090 
00091 float RobotControl::getDesiredTheta()
00092 {
00093     return Desired.theta;
00094 }
00095 
00096 void RobotControl::setDesiredPositionAndAngle(float xposition,
00097         float yposition,
00098         float theta)
00099 {
00100     setDesiredxPosition(xposition);
00101     setDesiredyPosition(yposition);
00102     setDesiredTheta(theta);
00103 }
00104 
00105 float RobotControl::getTheta()
00106 {
00107     return Desired.theta;
00108 }
00109 
00110 float RobotControl::getDesiredSpeed()
00111 {
00112     return speed;
00113 }
00114 
00115 float RobotControl::getActualSpeed()
00116 {
00117     return Actual.speed;
00118 }
00119 
00120 float RobotControl::getDesiredOmega()
00121 {
00122     return omega;
00123 }
00124 
00125 float RobotControl::getActualOmega()
00126 {
00127     return Actual.omega;
00128 }
00129 
00130 float RobotControl::getxActualPosition()
00131 {
00132     return Actual.getxPosition();
00133 }
00134 
00135 float RobotControl::getxPositionError()
00136 {
00137     return Desired.getxPosition()-Actual.getxPosition();
00138 }
00139 
00140 float RobotControl::getyActualPosition()
00141 {
00142     return Actual.getyPosition();
00143 }
00144 
00145 float RobotControl::getyPositionError()
00146 {
00147     return Desired.getyPosition()-Actual.getyPosition();
00148 }
00149 
00150 float RobotControl::getActualTheta()
00151 {
00152     return Actual.getTheta();
00153 }
00154 
00155 float RobotControl::getThetaError()
00156 {
00157     return Desired.getTheta()-Actual.getTheta();
00158 }
00159 
00160 float RobotControl::getDistanceError()
00161 {
00162     return sqrt( ( getxPositionError() * getxPositionError() ) + ( getyPositionError() * getyPositionError() ) );
00163 }
00164 
00165 float RobotControl::getThetaErrorToGoal()
00166 {
00167     return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
00168 }
00169 
00170 float RobotControl::getThetaGoal()
00171 {
00172     return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
00173 }
00174 
00175 void RobotControl::setAllToZero(float xZeroPos,
00176                                 float yZeroPos,
00177                                 float theta)
00178 {
00179     Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f);
00180     Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
00181     stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
00182     stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
00183     speed = 0.0f;
00184     omega = 0.0f;
00185 }
00186 
00187 void RobotControl::run()
00188 {
00189     // When the Motor is note enable set the desired speed to the acutal speed.
00190     // only used by setting the speed and omega via the WII control.
00191     if (!isEnabled()) {
00192         speed = 0.0f;
00193         omega = 0.0f;
00194         Desired.setState(&Actual);
00195     }
00196 
00197     // position calculation
00198     // Set the state of speed from Left und Right Wheel
00199     stateLeft.speed = motorControllerLeft->getActualSpeed() *
00200                       2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
00201     stateRight.speed = - motorControllerRight->getActualSpeed() *
00202                        2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ;
00203 
00204     //translational speed of the Robot (average)
00205     Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f;
00206 
00207     //rotational speed of the Robot
00208     Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE;
00209 
00210     //rotational theta of the Robot integrate the omega with the time
00211     Actual.theta += Actual.omega * period;
00212     Actual.theta = PiRange(Actual.theta);
00213 
00214     //translational X and Y Position. integrate the speed with the time
00215     Actual.xposition += (Actual.speed * period * cos(Actual.theta));
00216     Actual.yposition += (Actual.speed * period * sin(Actual.theta));
00217 
00218     //motor control
00219     if ( /*isEnabled() && */ ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {
00220         
00221         motorControllerLeft->enable(true);
00222         motorControllerRight->enable(true);
00223         //postition control
00224         speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
00225         omega = K2 * getThetaErrorToGoal() +
00226                 K1 *
00227                 (
00228                     (
00229                         sin(getThetaErrorToGoal() ) * cos(getThetaErrorToGoal() )
00230                     ) / getThetaErrorToGoal()
00231                 ) *
00232                 ( getThetaErrorToGoal()
00233                   + K3 * getThetaGoal() );
00234 
00235         motorControllerLeft->setVelocity(
00236             ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) /
00237             (2 * WHEEL_RADIUS_LEFT * PI * GEAR)
00238         );
00239         motorControllerRight->setVelocity(
00240             -( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) /
00241             (2 * WHEEL_RADIUS_RIGHT * PI * GEAR)
00242         );
00243 
00244     } else {
00245         
00246         motorControllerLeft->enable(false);
00247         motorControllerRight->enable(false);
00248         motorControllerLeft->setVelocity(0.0f);
00249         motorControllerRight->setVelocity(0.0f);
00250     }
00251 }