Diff: RobotControl/RobotControl.cpp

Revision:

0:31f7be68e52d

Child:

1:6cd533a712c6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotControl/RobotControl.cpp	Thu Feb 07 17:43:19 2013 +0000
@@ -0,0 +1,210 @@
+#include "RobotControl.h"
+
+using namespace std;
+
+RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period) : Task(period)
+{
+    /* get peripherals */
+    this->motorControllerLeft = motorControllerLeft;
+    this->motorControllerRight = motorControllerRight;
+    this->compass = compass;
+    this->period = period;
+
+    /* initialize peripherals */
+    motorControllerLeft->enable(false);
+    motorControllerRight->enable(false);
+
+    /* initialize remaining state values */
+    speed = 0.0f;
+    omega = 0.0f;
+
+    motorControllerLeft->setPulses(0);
+    motorControllerRight->setPulses(0);
+
+    Desired.setAcceleration(ACCELERATION);
+    Desired.setThetaAcceleration(THETA_ACCELERATION);
+
+}
+
+RobotControl::~RobotControl() 
+{
+
+}
+
+void RobotControl::setEnable(bool enable)
+{
+    motorControllerLeft->enable(enable);
+    motorControllerRight->enable(enable);
+}
+
+bool RobotControl::isEnabled()
+{
+    return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
+}
+
+void RobotControl::setAcceleration(float acc)
+{
+    Desired.setAcceleration(acc);
+
+}
+
+void RobotControl::setThetaAcceleration(float acc)
+{
+    Desired.setThetaAcceleration(acc);
+}
+
+void RobotControl::setDesiredSpeed(float speed)
+{
+    this->speed = speed;
+}
+
+void RobotControl::setDesiredOmega(float omega)
+{
+    this->omega = omega;
+}
+
+void RobotControl::setxPosition(float position)
+{
+    Desired.xposition = position;
+}
+
+void RobotControl::setyPosition(float position)
+{
+    Desired.yposition = position;
+}
+
+void RobotControl::setTheta(float theta)
+{
+    Desired.theta = theta;
+}
+
+float RobotControl::getDesiredSpeed()
+{
+    return speed;
+}
+
+float RobotControl::getActualSpeed()
+{
+    return Actual.speed;
+}
+
+float RobotControl::getDesiredOmega()
+{
+    return omega;
+}
+
+float RobotControl::getActualOmega()
+{
+    return Actual.omega;
+}
+
+float RobotControl::getxActualPosition()
+{
+    return Actual.getxPosition();
+}
+
+float RobotControl::getxPositionError()
+{
+    return Desired.getxPosition()-Actual.getxPosition();
+}
+
+float RobotControl::getyActualPosition()
+{
+    return Actual.getyPosition();
+}
+
+float RobotControl::getyPositionError()
+{
+    return Desired.getyPosition()-Actual.getyPosition();
+}
+
+float RobotControl::getActualTheta()
+{
+    return Actual.getTheta();
+}
+
+float RobotControl::getThetaError()
+{
+    return Desired.getTheta()-Actual.getTheta();
+}
+
+void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
+{
+    Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f);
+    Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+    stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+    stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+    speed = 0.0f;
+    omega = 0.0f;
+}
+
+
+void RobotControl::run()
+{
+    /* motion planning */
+    if (isEnabled()) {
+        Desired.increment(speed, omega, period);
+    } else {
+        speed = 0.0f;
+        omega = 0.0f;
+        Desired.setState(&Actual);
+    }
+
+    /* position calculation */
+
+    /* Set the state of speed from Left und Right Wheel*/
+    stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+
+    /* translational speed of the Robot (average) */
+    Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
+
+    /* rotational speed of the Robot  */
+    Actual.omega = ( stateRight.speed - stateLeft.speed  ) / WHEEL_DISTANCE;
+
+    /* rotational theta of the Robot */
+    Actual.theta += Actual.omega * period;
+    if(Actual.theta <= -PI) {
+        Actual.theta  += 2*  PI;
+    } else if (Actual.theta > PI) {
+        Actual.theta  -= 2*  PI;
+    } else {
+        //nothing
+    }
+    Actual.theta += Actual.omega * period;
+    Actual.thetaCompass = compass->getFilteredAngle();
+
+       /* translational X and Y Position. integrate the speed with the time */ 
+  //  Actual.xposition += (Actual.speed * period * sin(Actual.theta));
+  //  Actual.yposition += (Actual.speed * period * cos(Actual.theta));
+    
+        /* translational X and Y Position. integrate the speed with the time theta from compass */
+    Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass));
+    Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass));
+    
+    
+    
+    
+    /* postition control calculate */
+    
+    rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
+    
+    
+    
+    
+
+    /* motor control */
+    if (isEnabled()) {
+
+        motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) );
+        motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) );
+
+
+    } else {
+
+        motorControllerLeft->setVelocity(0.0f);
+        motorControllerRight->setVelocity(0.0f);
+
+    }    
+}
+