Christian Burri
This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
Fork of
autonomousRobotAndroid
by 
Christian Burri
Revision 1:6cd533a712c6, committed 2013-03-02
- Comitter:
- chrigelburri
- Date:
- Sat Mar 02 09:39:34 2013 +0000
- Parent:
- 0:31f7be68e52d
- Child:
- 2:d8e1613dc38b
- Commit message:
- Pos Regler funktioniert getestet im leerlauf;
Changed in this revision
--- a/Actuators/Hallsensor/Hallsensor.h Thu Feb 07 17:43:19 2013 +0000 +++ b/Actuators/Hallsensor/Hallsensor.h Sat Mar 02 09:39:34 2013 +0000 @@ -8,7 +8,7 @@ * * @section LICENSE * - * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe + * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe * All rights reserved. * * @section DESCRIPTION
--- a/Actuators/MaxonESCON/MaxonESCON.cpp Thu Feb 07 17:43:19 2013 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.cpp Sat Mar 02 09:39:34 2013 +0000
@@ -57,9 +57,9 @@
void MaxonESCON::enable(bool enb)
{
if(enb == false) {
- _enb = 0;
+ _enb = 1;
} else {
- _enb = 1;
+ _enb = 0;
}
}
@@ -70,12 +70,6 @@
} else {
return false;
}
- /*
- if(_enb == 0) {
- return false;
- } else {
- return true;
- }*/
}
int MaxonESCON::getPulses(void)
--- a/Actuators/MaxonESCON/MaxonESCON.h Thu Feb 07 17:43:19 2013 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.h Sat Mar 02 09:39:34 2013 +0000
@@ -10,7 +10,7 @@
*
* @section LICENSE
*
- * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
* @section DESCRIPTION
@@ -49,8 +49,9 @@
* @param enb DigitalOut, set high for enable
* @param isenb DigitalIn, high for enable
* @param pwm PwmOut pin, set the Velocity
- * @param hall1 HALL Object
- * @param Actual Speed AnalogIn Filtered Signal for ActualSpeed from Motor
+ * @param hall HALL Object
+ * @param actualSpeed AnalogIn Filtered Signal for ActualSpeed from Motor
+ * @param hall The Object of the Hallsensor from Motor
*/
MaxonESCON(PinName enb,
PinName isenb,
@@ -60,7 +61,7 @@
/** Set the speed of the motor with a pwm for 10%..90%
* 50% PWM is 0rpm
- * Caclulate from 1/s in 1/min plus the Facotr of the ESCON
+ * Caclulate from [1/s] in [1/min] and the Factor of the ESCON
* @param speed The speed of the motor as a normalised value in [1/s]
*/
void setVelocity(float speed);
@@ -77,13 +78,13 @@
*
* Wrapper for PwmOut::period()
*
- * @param seconds - Pwm duty cycle in seconds.
+ * @param period Pwm duty cycle in seconds.
*/
void period(float period);
/** Set the Motor to a enable sate
*
- * @param enable - 0 for disable 1 for enable.
+ * @param enb <code>0</code> for disable <code>1</code> for enable.
*/
void enable (bool enb);
--- a/RobotControl/MotionState.h Thu Feb 07 17:43:19 2013 +0000
+++ b/RobotControl/MotionState.h Sat Mar 02 09:39:34 2013 +0000
@@ -9,7 +9,7 @@
*
* @section LICENSE
*
- * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
* @section DESCRIPTION
@@ -52,7 +52,7 @@
* @param yposition the initial position value of this motion state, given in [m].
* @param theta the initial angle value of this motion state, given in [rad].
* @param speed the initial speed value of this motion state, given in [m/s].
- * @param omega the initial omega speed value of this motion state, given in [rad/s].
+ * @param omega the initial ω speed value of this motion state, given in [rad/s].
*/
MotionState(float xposition, float yposition, float theta, float speed, float omega);
@@ -67,7 +67,7 @@
* @param yposition the initial position value of this motion state, given in [m].
* @param theta the initial angle value of this motion state, given in [rad].
* @param speed the initial speed value of this motion state, given in [m/s].
- * @param omega the initial omega speed value of this motion state, given in [rad/s].
+ * @param omega the initial ω speed value of this motion state, given in [rad/s].
*/
void setState(float xposition, float yposition, float theta, float speed, float omega);
@@ -79,9 +79,9 @@
/**
* Sets the X-position value.
- * @param the desired xposition value of this motion state, given in [m].
+ * @param xposition the desired xposition value of this motion state, given in [m].
*/
- void setxPosition(float position);
+ void setxPosition(float xposition);
/**
* Gets the X-position value.
@@ -91,7 +91,7 @@
/**
* Sets the Y-position value.
- * @param the desired yposition value of this motion state, given in [m].
+ * @param yposition the desired yposition value of this motion state, given in [m].
*/
void setyPosition(float yposition);
@@ -103,7 +103,7 @@
/**
* Sets the theta value.
- * @param the desired theta value of this motion state, given in [m].
+ * @param theta the desired theta value of this motion state, given in [m].
*/
void setTheta(float theta);
@@ -126,49 +126,45 @@
float getSpeed();
/**
- * Sets the omega value.
- * @param omega the desired omega value of this motion state, given in [rad/s].
+ * Sets the ω value.
+ * @param omega the desired ω value of this motion state, given in [rad/s].
*/
void setOmega(float omega);
/**
- * Gets the omega value.
- * @return the omega value of this motion state, given in [rad/s].
+ * Gets the ω value.
+ * @return the ω value of this motion state, given in [rad/s].
*/
float getOmega();
/**
* Sets the maximum acceleration value.
- * @param acceleration the maximum acceleration value to use for the calculation
- * of the motion trajectory, given in [m/s²].
+ * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
*/
void setAcceleration(float acceleration);
/**
* Gets the maximum acceleration value.
- * @return the maximum acceleration value used for the calculation
- * of the motion trajectory, given in [m/s²].
+ * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
*/
float getAcceleration();
/**
* Sets the maximum acceleration value of rotation.
- * @param acceleration the maximum acceleration value to use for the calculation
- * of the motion trajectory, given in [rad/s²].
+ * @param thetaAcceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>].
*/
void setThetaAcceleration(float thetaAcceleration);
/**
* Gets the maximum acceleration value of rotation.
- * @return the maximum acceleration value used for the calculation
- * of the motion trajectory, given in [rad/s²].
+ * @return the maximum acceleration value used for the calculation of the motion trajectory, given in [rad/<SUP>2</SUP>].
*/
float getThetaAcceleration();
/**
* Increments the current motion state towards a given desired speed.
* @param desiredSpeed the desired speed given in [m/s].
- * @param desiredOmega the desired omega given in [rad/s].
+ * @param desiredOmega the desired ω given in [rad/s].
* @param period the time period to increment the motion state for, given in [s].
*/
void increment(float desiredSpeed, float desiredOmega, float period);
--- a/RobotControl/RobotControl.cpp Thu Feb 07 17:43:19 2013 +0000
+++ b/RobotControl/RobotControl.cpp Sat Mar 02 09:39:34 2013 +0000
@@ -2,12 +2,12 @@
using namespace std;
-RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period) : Task(period)
+RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period) : Task(period)
{
/* get peripherals */
this->motorControllerLeft = motorControllerLeft;
this->motorControllerRight = motorControllerRight;
- this->compass = compass;
+ // this->compass = compass;
this->period = period;
/* initialize peripherals */
@@ -26,7 +26,7 @@
}
-RobotControl::~RobotControl()
+RobotControl::~RobotControl()
{
}
@@ -42,15 +42,14 @@
return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
}
-void RobotControl::setAcceleration(float acc)
+void RobotControl::setAcceleration(float acceleration)
{
- Desired.setAcceleration(acc);
-
+ Desired.setAcceleration(acceleration);
}
-void RobotControl::setThetaAcceleration(float acc)
+void RobotControl::setThetaAcceleration(float acceleration)
{
- Desired.setThetaAcceleration(acc);
+ Desired.setThetaAcceleration(acceleration);
}
void RobotControl::setDesiredSpeed(float speed)
@@ -63,14 +62,14 @@
this->omega = omega;
}
-void RobotControl::setxPosition(float position)
+void RobotControl::setxPosition(float xposition)
{
- Desired.xposition = position;
+ Desired.xposition = xposition;
}
-void RobotControl::setyPosition(float position)
+void RobotControl::setyPosition(float yposition)
{
- Desired.yposition = position;
+ Desired.yposition = yposition;
}
void RobotControl::setTheta(float theta)
@@ -78,6 +77,11 @@
Desired.theta = theta;
}
+float RobotControl::getTheta()
+{
+ return Desired.theta;
+}
+
float RobotControl::getDesiredSpeed()
{
return speed;
@@ -128,9 +132,24 @@
return Desired.getTheta()-Actual.getTheta();
}
-void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
+float RobotControl::getDistanceError()
+{
+ return sqrt( ( getxPositionError() * getxPositionError() ) + (getyPositionError() * getyPositionError() ) );
+}
+
+float RobotControl::getThetaErrorToGoal()
{
- Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f);
+ return atan2(getyPositionError(),getxPositionError()) - getActualTheta(); // PI ist weg weil auch negative Zahlen zugelassen werden.
+}
+
+float RobotControl::getThetaGoal()
+{
+ return atan2(getyPositionError(),getxPositionError()) - getTheta();
+}
+
+void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta)
+{
+ Actual.setState(xZeroPos, yZeroPos, theta, 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);
@@ -141,9 +160,15 @@
void RobotControl::run()
{
+ ///// DAs kan glaub raus ab hier
+
/* motion planning */
if (isEnabled()) {
+ ///// DAs kan glaub raus bis hier
Desired.increment(speed, omega, period);
+
+ ///// DAs kan glaub raus vis hier
+
} else {
speed = 0.0f;
omega = 0.0f;
@@ -153,8 +178,8 @@
/* 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;
+ stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
+ stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
/* translational speed of the Robot (average) */
Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
@@ -162,7 +187,7 @@
/* rotational speed of the Robot */
Actual.omega = ( stateRight.speed - stateLeft.speed ) / WHEEL_DISTANCE;
- /* rotational theta of the Robot */
+ /* rotational theta of the Robot integrate the omega with the time*/
Actual.theta += Actual.omega * period;
if(Actual.theta <= -PI) {
Actual.theta += 2* PI;
@@ -171,40 +196,33 @@
} 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 * cos(Actual.theta));
+ Actual.yposition += (Actual.speed * period * sin(Actual.theta));
- /* 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));
-
-
-
+
+ // Actual.thetaCompass = compass->getFilteredAngle();
+ /* translational X and Y Position. integrate the speed with the time theta from compass */
+ // Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass));
+ // Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass));
- /* postition control calculate */
-
- rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
-
-
-
-
+ /* motor control */
+ if ( isEnabled() && ( getDistanceError() >= MIN_DISTANCE_ERROR ) ) {
- /* motor control */
- if (isEnabled()) {
+ /* postition control */
+
+ speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
+ omega = K2 * getThetaErrorToGoal() + K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
- motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) );
- motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) );
-
+ motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) * GEAR );
+ motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) * GEAR );
} else {
motorControllerLeft->setVelocity(0.0f);
motorControllerRight->setVelocity(0.0f);
- }
+ }
}
--- a/RobotControl/RobotControl.h Thu Feb 07 17:43:19 2013 +0000
+++ b/RobotControl/RobotControl.h Sat Mar 02 09:39:34 2013 +0000
@@ -15,7 +15,7 @@
*
* @section LICENSE
*
- * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
* @section DESCRIPTION
@@ -36,7 +36,7 @@
MaxonESCON* motorControllerLeft;
MaxonESCON* motorControllerRight;
- HMC6352* compass;
+// HMC6352* compass;
AnalogIn* battery;
MotionState Desired;
MotionState Actual;
@@ -45,10 +45,14 @@
float period;
float speed;
float omega;
-
+
float rho; /* Distance to goal [m]*/
float gamma; /* Angle of the start position to goal position [rad]*/
-
+ float delta; /* Angle of the goal postition [rad]*/
+
+ float deltaXPostition; /* differenz of the actual X-value and the desired [m] */
+ float deltaYPostition; /* differenz of the actual X-value and the desired [m] */
+
public:
@@ -61,7 +65,7 @@
* @param compass Modul HMC5883L
* @param period the sampling period of the run loop of this controller, given in [s].
*/
- RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, float period);
+ RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period);
/**
* Destructor of the Object to destroy the Object.
@@ -83,17 +87,15 @@
/**
* Sets the maximum acceleration value.
- * @param acceleration the maximum acceleration value to use for the calculation
- * of the motion trajectory, given in [m/s²].
+ * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [m/s<SUP>2</SUP>].
*/
- void setAcceleration(float acc);
+ void setAcceleration(float acceleration);
/**
* Sets the maximum acceleration value of rotation.
- * @param acceleration the maximum acceleration value to use for the calculation
- * of the motion trajectory, given in [rad/s²].
+ * @param acceleration the maximum acceleration value to use for the calculation of the motion trajectory, given in [rad/s<SUP>2</SUP>].
*/
- void setThetaAcceleration(float acc);
+ void setThetaAcceleration(float acceleration);
/**
* Sets the desired translational speed of the robot.
@@ -103,27 +105,33 @@
/**
* Sets the desired rotational speed of the robot.
- * @param speed the desired speed, given in [rad/s].
+ * @param omega the desired rotational speed, given in [rad/s].
*/
void setDesiredOmega(float omega);
/**
* Sets the desired X- position of the robot.
- * @param xpostion the desired position, given in [m].
+ * @param xposition the desired position, given in [m].
*/
- void setxPosition(float position);
+ void setxPosition(float xposition);
/**
* Sets the desired Y-position of the robot.
- * @param ypostion the desired position, given in [m].
+ * @param yposition the desired position, given in [m].
*/
- void setyPosition(float position);
+ void setyPosition(float yposition);
/**
* Sets the angle of the robot.
* @param theta the desired angle, given in [rad].
*/
void setTheta(float theta);
+
+ /**
+ * Gets the desired Theta. Angle of the goal Point.
+ * @return the desired Theta, given in [rad].
+ */
+ float getTheta();
/**
* Gets the desired translational speed of the robot.
@@ -183,14 +191,35 @@
* Gets the orientation following error of the robot.
* @return the orientation following error, given in [rad].
*/
+
float getThetaError();
+
+ /**
+ * Gets the Distance to Disired point. Calculate witch pythagoras
+ * @return distance to goal, given in [m].
+ */
+ float getDistanceError();
+
+ /**
+ * Gets the Angle ot the pointing vector to the goal right the unicycle axis from actual point
+ * @return theta to goal, given in [rad].
+ */
+ float getThetaErrorToGoal();
+
+ /**
+ * Gets the Angle ot the pointing vector to the goal right the unicycle main axis
+ * @return theta from the goal, given in [rad].
+ */
+ float getThetaGoal();
+
/**
* Set all state to zero
- * @param Sets the start X-position [m].
- * @param Sets the start y-position [m].
+ * @param xZeroPos Sets the start X-position [m].
+ * @param yZeroPos Sets the start y-position [m].
+ * @param theta Sets the start angle [rad].
*/
- void setAllToZero(float xZeroPos, float xZeroPos);
+ void setAllToZero(float xZeroPos, float yZeroPos, float theta);
void run();
};
--- a/StateDefines/State.cpp Thu Feb 07 17:43:19 2013 +0000
+++ b/StateDefines/State.cpp Sat Mar 02 09:39:34 2013 +0000
@@ -8,14 +8,14 @@
// LocalFileSystem
LocalFileSystem local("local");
-State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, AnalogIn* battery, float period) : Task(period)
+State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period) : Task(period)
{
/* get peripherals */
this->s = s;
this->robotControl = robotControl;
this->motorControllerLeft = motorControllerLeft;
this->motorControllerRight = motorControllerRight;
- this->compass = compass;
+ // this->compass = compass;
this->battery =battery;
this->period = period;
@@ -159,7 +159,7 @@
s->xAxisError = robotControl->getxPositionError();
s->yAxisError = robotControl->getyPositionError();
s->angleError = robotControl->getThetaError() * 180 / PI;
- s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+ // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
setBatteryBit();
--- a/StateDefines/State.h Thu Feb 07 17:43:19 2013 +0000
+++ b/StateDefines/State.h Sat Mar 02 09:39:34 2013 +0000
@@ -15,7 +15,7 @@
*
* @section LICENSE
*
- * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
* All rights reserved.
*
* @section DESCRIPTION
@@ -33,7 +33,7 @@
RobotControl* robotControl;
MaxonESCON* motorControllerLeft;
MaxonESCON* motorControllerRight;
- HMC6352* compass;
+ // HMC6352* compass;
AnalogIn* battery;
Timer timer;
float period;
@@ -46,16 +46,16 @@
/**
- * Creates a robot control object and initializes all private
- * state variables.
- * @param RobotControl Objekt to read the state.
+ * Creates a robot control object and initializes all private state variables.
+ * @param s struct to read and write the state.
+ * @param robotControl Object to read the state.
* @param motorControllerLeft a reference to the servo drive for the left motor.
* @param motorControllerRight a reference to the servo drive for the right motor.
* @param compass Modul HMC5883L
- * @param Analog Input Battery Voltage input
+ * @param battery Analog Input Battery Voltage input
* @param period the sampling period of the run loop of this controller, given in [s].
*/
- State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, HMC6352* compass, AnalogIn* battery, float period);
+ State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period);
/**
* Destructor of the Object to destroy the Object.
@@ -91,6 +91,10 @@
*/
float readBattery();
+ /**
+ * Starts the timer from zero
+ * The timer is for the logging Time.
+ */
void startTimerFromZero();
/**
--- a/StateDefines/defines.h Thu Feb 07 17:43:19 2013 +0000 +++ b/StateDefines/defines.h Sat Mar 02 09:39:34 2013 +0000 @@ -13,7 +13,7 @@ // Physical Dimension of the car #define WHEEL_RADIUS 0.042f // radius of the wheel, given in [m] -#define WHEEL_DISTANCE 0.176f // Distance of the wheel, given in [m] +#define WHEEL_DISTANCE 0.18f // Distance of the wheel, given in [m] // State Bits of the car #define STATE_STOP 1u // Bit0 = stop pressed @@ -31,7 +31,15 @@ // Maximum Aceeleration #define ACCELERATION 0.25f // maximum translational acceleration, given in [m/s2] -#define THETA_ACCELERATION 1.0f // maximum rotational acceleration, given in [rad/s2 +#define THETA_ACCELERATION 1.0f // maximum rotational acceleration, given in [rad/s2] + +// Gains of the position controller +#define GAIN 0.2f +#define K1 1.0f * GAIN +#define K2 3.0f * GAIN +#define K3 2.0f * GAIN + +#define MIN_DISTANCE_ERROR 0.1 // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m] // LiPo Batterie #define BAT_MULTIPLICATOR 21.633333333f // R2 / (R1 + R2) = 0.153 R2= 10k , R1 = 1.8k
--- a/Task/Task.h Thu Feb 07 17:43:19 2013 +0000 +++ b/Task/Task.h Sat Mar 02 09:39:34 2013 +0000 @@ -8,8 +8,9 @@ * * @section LICENSE * - * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe + * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe * All rights reserved. + * * @section DESCRIPTION * The <code>Task</code> class allows to install periodic, time-triggered * tasks. An example of a simple user-defined task is given below:
--- a/main.cpp Thu Feb 07 17:43:19 2013 +0000
+++ b/main.cpp Sat Mar 02 09:39:34 2013 +0000
@@ -1,87 +1,114 @@
-/*
- * main.cpp
- * Copyright (c) 2012, Pren Team1 HSLU T&A
- * All rights reserved.
- */
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// INCLUDES
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-#include "mbed.h"
-#include "math.h"
-#include "defines.h"
-#include "State.h"
-#include "HMC5883L.h"
-#include "HMC6352.h"
-#include "RobotControl.h"
-#include "Ping.h"
-#include "PowerControl/EthernetPowerControl.h"
-#include "Android.h"
-
-
-// LiPo Batterie
-AnalogIn battery(p15); // Battery check
-
-// compass
-//HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
-HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
-
-
-// ultrasonic sensor
-//Ping ultrasonic(p30);
-
-//Hallsensor
-//hall1, hall2, hall3
-Hallsensor hallLeft(p18, p17, p16);
-//hall1, hall2, hall3
-Hallsensor hallRight(p27, p28, p29);
-
-// Motors
-//enb, ready, pwm, actualSpeed, Hallsensor object
-MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
-//enb, ready, pwm, actualSpeed, Hallsensor object
-MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
-
-// Robot Control
-RobotControl robotControl (&leftMotor, &rightMotor, &compass, PERIOD_ROBOTCONTROL);
-
-// Logging & State
-state_t s; // stuct state
-State state(&s, &robotControl, &leftMotor, &rightMotor, &compass, &battery, PERIOD_STATE);
-
-// Communication
-
-//Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);
-
-// PC USB communications
-Serial pc(USBTX, USBRX);
-
-DigitalOut myled(LED1);
-
-float magout[3] = {0};
-
-// LiPo Batterie
-float batterie_voltage;
-
-int main()
-{
-
- /** Normal mbed power level for this setup is around 690mW
- * assuming 5V used on Vin pin
- * If you don't need networking...
- * Power down Ethernet interface - saves around 175mW
- * Also need to unplug network cable - just a cable sucks power
- */
- PHY_PowerDown();
-
- robotControl.start();
- compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
- compass.start();
-
- robotControl.setEnable(false);
- wait(1);
- robotControl.setEnable(true);
- wait(1);
-
-
-}
+/*
+ * main.cpp
+ * Copyright (c) 2012, Pren Team1 HSLU T&A
+ * All rights reserved.
+ */
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// INCLUDES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+#include "mbed.h"
+#include "math.h"
+#include "defines.h"
+#include "State.h"
+#include "HMC5883L.h"
+#include "HMC6352.h"
+#include "RobotControl.h"
+#include "Ping.h"
+#include "PowerControl/EthernetPowerControl.h"
+#include "Android.h"
+
+
+// LiPo Batterie
+AnalogIn battery(p15); // Battery check
+
+// compass
+//HMC5883L compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
+//HMC6352 compass(p9, p10, PERIOD_COMPASS); // sda, sdl (I2C)
+
+
+// ultrasonic sensor
+//Ping ultrasonic(p30);
+
+//Hallsensor
+//hall1, hall2, hall3
+Hallsensor hallLeft(p18, p17, p16);
+//hall1, hall2, hall3
+Hallsensor hallRight(p27, p28, p29);
+
+// Motors
+//enb, ready, pwm, actualSpeed, Hallsensor object
+MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);
+//enb, ready, pwm, actualSpeed, Hallsensor object
+MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);
+
+// Robot Control
+RobotControl robotControl (&leftMotor, &rightMotor, /*&compass,*/ PERIOD_ROBOTCONTROL);
+
+// Logging & State
+state_t s; // stuct state
+State state(&s, &robotControl, &leftMotor, &rightMotor, /*&compass,*/ &battery, PERIOD_STATE);
+
+// Communication
+
+//Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);
+
+// PC USB communications
+Serial pc(USBTX, USBRX);
+
+DigitalOut myled(LED1);
+
+//float magout[3] = {0};
+
+// LiPo Batterie
+float batterie_voltage;
+
+int main()
+{
+ pc.printf("blabal");
+ /** Normal mbed power level for this setup is around 690mW
+ * assuming 5V used on Vin pin
+ * If you don't need networking...
+ * Power down Ethernet interface - saves around 175mW
+ * Also need to unplug network cable - just a cable sucks power
+ */
+ PHY_PowerDown();
+ state.startTimerFromZero();
+ state.initPlotFile();
+
+ // robotControl.start();
+ // compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ // compass.start();
+ robotControl.start();
+ robotControl.setEnable(false);
+ wait(0.01);
+ robotControl.setEnable(true);
+ wait(0.01);
+ robotControl.setAllToZero(0, 0, PI/2 );
+ leftMotor.setPulses(0);
+ rightMotor.setPulses(0);
+ state.start();
+ pc.printf("start");
+
+ robotControl.setxPosition(-1.0);
+ robotControl.setyPosition(1.0);
+ robotControl.setTheta( PI);
+ while(!(s.millis >= 15000)) {
+ state.savePlotFile(s);
+ pc.printf("rhho: %f, gamma: %f, delta: %f thetaacutal: %f %f %f atan2: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal() * 180/PI, robotControl.getThetaGoal()*180/PI, robotControl.getActualTheta()*180/PI, robotControl.getyPositionError(), robotControl.getxPositionError(), atan2(robotControl.getyPositionError(),robotControl.getxPositionError()) * 180/PI);
+ };
+ pc.printf("nextpoint");
+ robotControl.setxPosition(-2.0);
+ robotControl.setyPosition(3.0);
+ robotControl.setTheta( PI/2 );
+ while(!(s.millis >= 30000)) {
+ state.savePlotFile(s);
+ pc.printf("rho: %f, gamma: %f, delta: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal() * 180/PI, robotControl.getThetaGoal()*180/PI);
+ };
+ state.savePlotFile(s);
+ state.closePlotFile();
+ state.stop();
+ robotControl.setEnable(false);
+ pc.printf("\n");
+
+}