Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

RobotControl/MotionState.cpp@6:48eeb41188dd, 2013-03-23 (annotated)

Committer:
chrigelburri
Date:
Sat Mar 23 13:52:48 2013 +0000
Revision:
6:48eeb41188dd
Parent:
0:31f7be68e52d
Child:
11:775ebb69d5e1
mit link und rechten Radradius

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 0:31f7be68e52d 1 #include "MotionState.h"
chrigelburri 0:31f7be68e52d 2
chrigelburri 0:31f7be68e52d 3 using namespace std;
chrigelburri 0:31f7be68e52d 4
chrigelburri 0:31f7be68e52d 5 MotionState::MotionState()
chrigelburri 0:31f7be68e52d 6 {
chrigelburri 0:31f7be68e52d 7 xposition = 0.0f;
chrigelburri 0:31f7be68e52d 8 yposition = 0.0f;
chrigelburri 0:31f7be68e52d 9 theta = 0.0f;
chrigelburri 0:31f7be68e52d 10 thetaCompass = 0.0f;
chrigelburri 0:31f7be68e52d 11 speed = 0.0f;
chrigelburri 0:31f7be68e52d 12 omega = 0.0f;
chrigelburri 0:31f7be68e52d 13
chrigelburri 0:31f7be68e52d 14 acceleration = ACCELERATION;
chrigelburri 0:31f7be68e52d 15 thetaAcceleration = THETA_ACCELERATION;
chrigelburri 0:31f7be68e52d 16 }
chrigelburri 0:31f7be68e52d 17
chrigelburri 0:31f7be68e52d 18
chrigelburri 6:48eeb41188dd 19 MotionState::MotionState(float xposition,
chrigelburri 6:48eeb41188dd 20 float yposition,
chrigelburri 6:48eeb41188dd 21 float theta,
chrigelburri 6:48eeb41188dd 22 float speed,
chrigelburri 6:48eeb41188dd 23 float omega)
chrigelburri 0:31f7be68e52d 24 {
chrigelburri 0:31f7be68e52d 25 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 26 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 27 this->theta = theta;
chrigelburri 0:31f7be68e52d 28 this->speed = speed;
chrigelburri 0:31f7be68e52d 29 this->omega = omega;
chrigelburri 0:31f7be68e52d 30 acceleration = ACCELERATION;
chrigelburri 0:31f7be68e52d 31 thetaAcceleration = THETA_ACCELERATION;
chrigelburri 0:31f7be68e52d 32 }
chrigelburri 0:31f7be68e52d 33
chrigelburri 0:31f7be68e52d 34 MotionState::~MotionState()
chrigelburri 0:31f7be68e52d 35 {
chrigelburri 0:31f7be68e52d 36
chrigelburri 0:31f7be68e52d 37 }
chrigelburri 0:31f7be68e52d 38
chrigelburri 6:48eeb41188dd 39 void MotionState::setState(float xposition,
chrigelburri 6:48eeb41188dd 40 float yposition,
chrigelburri 6:48eeb41188dd 41 float theta,
chrigelburri 6:48eeb41188dd 42 float speed,
chrigelburri 6:48eeb41188dd 43 float omega)
chrigelburri 0:31f7be68e52d 44 {
chrigelburri 0:31f7be68e52d 45 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 46 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 47 this->theta = theta;
chrigelburri 0:31f7be68e52d 48 this->speed = speed;
chrigelburri 0:31f7be68e52d 49 this->omega = omega;
chrigelburri 0:31f7be68e52d 50 }
chrigelburri 0:31f7be68e52d 51
chrigelburri 0:31f7be68e52d 52 void MotionState::setState(MotionState* motionState)
chrigelburri 0:31f7be68e52d 53 {
chrigelburri 0:31f7be68e52d 54 xposition = motionState->xposition;
chrigelburri 0:31f7be68e52d 55 yposition = motionState->yposition;
chrigelburri 0:31f7be68e52d 56 theta = motionState->theta;
chrigelburri 0:31f7be68e52d 57 speed = motionState->speed;
chrigelburri 0:31f7be68e52d 58 omega = motionState->omega;
chrigelburri 0:31f7be68e52d 59 }
chrigelburri 0:31f7be68e52d 60
chrigelburri 0:31f7be68e52d 61 void MotionState::setxPosition(float xposition)
chrigelburri 0:31f7be68e52d 62 {
chrigelburri 0:31f7be68e52d 63 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 64 }
chrigelburri 0:31f7be68e52d 65
chrigelburri 0:31f7be68e52d 66 float MotionState::getxPosition()
chrigelburri 0:31f7be68e52d 67 {
chrigelburri 0:31f7be68e52d 68 return xposition;
chrigelburri 0:31f7be68e52d 69 }
chrigelburri 0:31f7be68e52d 70
chrigelburri 0:31f7be68e52d 71 void MotionState::setyPosition(float yposition)
chrigelburri 0:31f7be68e52d 72 {
chrigelburri 0:31f7be68e52d 73 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 74 }
chrigelburri 0:31f7be68e52d 75
chrigelburri 0:31f7be68e52d 76 float MotionState::getyPosition()
chrigelburri 0:31f7be68e52d 77 {
chrigelburri 0:31f7be68e52d 78 return yposition;
chrigelburri 0:31f7be68e52d 79 }
chrigelburri 0:31f7be68e52d 80
chrigelburri 0:31f7be68e52d 81 void MotionState::setTheta(float theta)
chrigelburri 0:31f7be68e52d 82 {
chrigelburri 0:31f7be68e52d 83 this->theta = theta;
chrigelburri 0:31f7be68e52d 84 }
chrigelburri 0:31f7be68e52d 85
chrigelburri 0:31f7be68e52d 86 float MotionState::getTheta()
chrigelburri 0:31f7be68e52d 87 {
chrigelburri 0:31f7be68e52d 88 return theta;
chrigelburri 0:31f7be68e52d 89 }
chrigelburri 0:31f7be68e52d 90
chrigelburri 0:31f7be68e52d 91 void MotionState::setSpeed(float speed)
chrigelburri 0:31f7be68e52d 92 {
chrigelburri 0:31f7be68e52d 93 this->speed = speed;
chrigelburri 0:31f7be68e52d 94 }
chrigelburri 0:31f7be68e52d 95
chrigelburri 0:31f7be68e52d 96 float MotionState::getSpeed()
chrigelburri 0:31f7be68e52d 97 {
chrigelburri 0:31f7be68e52d 98 return speed;
chrigelburri 0:31f7be68e52d 99 }
chrigelburri 0:31f7be68e52d 100
chrigelburri 0:31f7be68e52d 101 void MotionState::setOmega(float omega)
chrigelburri 0:31f7be68e52d 102 {
chrigelburri 0:31f7be68e52d 103 this->omega = omega;
chrigelburri 0:31f7be68e52d 104 }
chrigelburri 0:31f7be68e52d 105
chrigelburri 0:31f7be68e52d 106 float MotionState::getOmega()
chrigelburri 0:31f7be68e52d 107 {
chrigelburri 0:31f7be68e52d 108 return omega;
chrigelburri 0:31f7be68e52d 109 }
chrigelburri 0:31f7be68e52d 110
chrigelburri 0:31f7be68e52d 111 void MotionState::setAcceleration(float acceleration)
chrigelburri 0:31f7be68e52d 112 {
chrigelburri 0:31f7be68e52d 113 this->acceleration = acceleration;
chrigelburri 0:31f7be68e52d 114 }
chrigelburri 0:31f7be68e52d 115
chrigelburri 0:31f7be68e52d 116 float MotionState::getAcceleration()
chrigelburri 0:31f7be68e52d 117 {
chrigelburri 0:31f7be68e52d 118 return acceleration;
chrigelburri 0:31f7be68e52d 119 }
chrigelburri 0:31f7be68e52d 120
chrigelburri 0:31f7be68e52d 121 void MotionState::setThetaAcceleration(float thetaAcceleration)
chrigelburri 0:31f7be68e52d 122 {
chrigelburri 0:31f7be68e52d 123 this->thetaAcceleration = thetaAcceleration;
chrigelburri 0:31f7be68e52d 124 }
chrigelburri 0:31f7be68e52d 125
chrigelburri 0:31f7be68e52d 126 float MotionState::getThetaAcceleration()
chrigelburri 0:31f7be68e52d 127 {
chrigelburri 0:31f7be68e52d 128 return thetaAcceleration;
chrigelburri 0:31f7be68e52d 129 }
chrigelburri 0:31f7be68e52d 130
chrigelburri 6:48eeb41188dd 131 void MotionState::increment(float desiredSpeed,
chrigelburri 6:48eeb41188dd 132 float desiredOmega,
chrigelburri 6:48eeb41188dd 133 float period)
chrigelburri 0:31f7be68e52d 134 {
chrigelburri 0:31f7be68e52d 135 float acceleration = (desiredSpeed-speed)/period;
chrigelburri 0:31f7be68e52d 136 if (acceleration < -this->acceleration) acceleration = -this->acceleration;
chrigelburri 0:31f7be68e52d 137 else if (acceleration > this->acceleration) acceleration = this->acceleration;
chrigelburri 0:31f7be68e52d 138
chrigelburri 0:31f7be68e52d 139 float thetaAcceleration = (desiredOmega-omega)/period;
chrigelburri 0:31f7be68e52d 140 if (thetaAcceleration < -this->thetaAcceleration) thetaAcceleration = -this->thetaAcceleration;
chrigelburri 0:31f7be68e52d 141 else if (thetaAcceleration > this->thetaAcceleration) thetaAcceleration = this->thetaAcceleration;
chrigelburri 0:31f7be68e52d 142
chrigelburri 0:31f7be68e52d 143 speed += acceleration * period;
chrigelburri 0:31f7be68e52d 144 omega += thetaAcceleration * period;
chrigelburri 0:31f7be68e52d 145 }