Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

RobotControl/MotionState.cpp@0:31f7be68e52d, 2013-02-07 (annotated)

Committer:
chrigelburri
Date:
Thu Feb 07 17:43:19 2013 +0000
Revision:
0:31f7be68e52d
Child:
6:48eeb41188dd
first steps

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 0:31f7be68e52d 19 MotionState::MotionState(float xposition, float yposition, float theta, float speed, float omega)
chrigelburri 0:31f7be68e52d 20 {
chrigelburri 0:31f7be68e52d 21 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 22 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 23 this->theta = theta;
chrigelburri 0:31f7be68e52d 24 this->speed = speed;
chrigelburri 0:31f7be68e52d 25 this->omega = omega;
chrigelburri 0:31f7be68e52d 26 acceleration = ACCELERATION;
chrigelburri 0:31f7be68e52d 27 thetaAcceleration = THETA_ACCELERATION;
chrigelburri 0:31f7be68e52d 28 }
chrigelburri 0:31f7be68e52d 29
chrigelburri 0:31f7be68e52d 30 MotionState::~MotionState()
chrigelburri 0:31f7be68e52d 31 {
chrigelburri 0:31f7be68e52d 32
chrigelburri 0:31f7be68e52d 33 }
chrigelburri 0:31f7be68e52d 34
chrigelburri 0:31f7be68e52d 35 void MotionState::setState(float xposition, float yposition, float theta, float speed, float omega)
chrigelburri 0:31f7be68e52d 36 {
chrigelburri 0:31f7be68e52d 37 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 38 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 39 this->theta = theta;
chrigelburri 0:31f7be68e52d 40 this->speed = speed;
chrigelburri 0:31f7be68e52d 41 this->omega = omega;
chrigelburri 0:31f7be68e52d 42 }
chrigelburri 0:31f7be68e52d 43
chrigelburri 0:31f7be68e52d 44 void MotionState::setState(MotionState* motionState)
chrigelburri 0:31f7be68e52d 45 {
chrigelburri 0:31f7be68e52d 46 xposition = motionState->xposition;
chrigelburri 0:31f7be68e52d 47 yposition = motionState->yposition;
chrigelburri 0:31f7be68e52d 48 theta = motionState->theta;
chrigelburri 0:31f7be68e52d 49 speed = motionState->speed;
chrigelburri 0:31f7be68e52d 50 omega = motionState->omega;
chrigelburri 0:31f7be68e52d 51 }
chrigelburri 0:31f7be68e52d 52
chrigelburri 0:31f7be68e52d 53 void MotionState::setxPosition(float xposition)
chrigelburri 0:31f7be68e52d 54 {
chrigelburri 0:31f7be68e52d 55 this->xposition = xposition;
chrigelburri 0:31f7be68e52d 56 }
chrigelburri 0:31f7be68e52d 57
chrigelburri 0:31f7be68e52d 58 float MotionState::getxPosition()
chrigelburri 0:31f7be68e52d 59 {
chrigelburri 0:31f7be68e52d 60 return xposition;
chrigelburri 0:31f7be68e52d 61 }
chrigelburri 0:31f7be68e52d 62
chrigelburri 0:31f7be68e52d 63 void MotionState::setyPosition(float yposition)
chrigelburri 0:31f7be68e52d 64 {
chrigelburri 0:31f7be68e52d 65 this->yposition = yposition;
chrigelburri 0:31f7be68e52d 66 }
chrigelburri 0:31f7be68e52d 67
chrigelburri 0:31f7be68e52d 68 float MotionState::getyPosition()
chrigelburri 0:31f7be68e52d 69 {
chrigelburri 0:31f7be68e52d 70 return yposition;
chrigelburri 0:31f7be68e52d 71 }
chrigelburri 0:31f7be68e52d 72
chrigelburri 0:31f7be68e52d 73 void MotionState::setTheta(float theta)
chrigelburri 0:31f7be68e52d 74 {
chrigelburri 0:31f7be68e52d 75 this->theta = theta;
chrigelburri 0:31f7be68e52d 76 }
chrigelburri 0:31f7be68e52d 77
chrigelburri 0:31f7be68e52d 78 float MotionState::getTheta()
chrigelburri 0:31f7be68e52d 79 {
chrigelburri 0:31f7be68e52d 80 return theta;
chrigelburri 0:31f7be68e52d 81 }
chrigelburri 0:31f7be68e52d 82
chrigelburri 0:31f7be68e52d 83 void MotionState::setSpeed(float speed)
chrigelburri 0:31f7be68e52d 84 {
chrigelburri 0:31f7be68e52d 85 this->speed = speed;
chrigelburri 0:31f7be68e52d 86 }
chrigelburri 0:31f7be68e52d 87
chrigelburri 0:31f7be68e52d 88 float MotionState::getSpeed()
chrigelburri 0:31f7be68e52d 89 {
chrigelburri 0:31f7be68e52d 90 return speed;
chrigelburri 0:31f7be68e52d 91 }
chrigelburri 0:31f7be68e52d 92
chrigelburri 0:31f7be68e52d 93 void MotionState::setOmega(float omega)
chrigelburri 0:31f7be68e52d 94 {
chrigelburri 0:31f7be68e52d 95 this->omega = omega;
chrigelburri 0:31f7be68e52d 96 }
chrigelburri 0:31f7be68e52d 97
chrigelburri 0:31f7be68e52d 98 float MotionState::getOmega()
chrigelburri 0:31f7be68e52d 99 {
chrigelburri 0:31f7be68e52d 100 return omega;
chrigelburri 0:31f7be68e52d 101 }
chrigelburri 0:31f7be68e52d 102
chrigelburri 0:31f7be68e52d 103 void MotionState::setAcceleration(float acceleration)
chrigelburri 0:31f7be68e52d 104 {
chrigelburri 0:31f7be68e52d 105 this->acceleration = acceleration;
chrigelburri 0:31f7be68e52d 106 }
chrigelburri 0:31f7be68e52d 107
chrigelburri 0:31f7be68e52d 108 float MotionState::getAcceleration()
chrigelburri 0:31f7be68e52d 109 {
chrigelburri 0:31f7be68e52d 110 return acceleration;
chrigelburri 0:31f7be68e52d 111 }
chrigelburri 0:31f7be68e52d 112
chrigelburri 0:31f7be68e52d 113 void MotionState::setThetaAcceleration(float thetaAcceleration)
chrigelburri 0:31f7be68e52d 114 {
chrigelburri 0:31f7be68e52d 115 this->thetaAcceleration = thetaAcceleration;
chrigelburri 0:31f7be68e52d 116 }
chrigelburri 0:31f7be68e52d 117
chrigelburri 0:31f7be68e52d 118 float MotionState::getThetaAcceleration()
chrigelburri 0:31f7be68e52d 119 {
chrigelburri 0:31f7be68e52d 120 return thetaAcceleration;
chrigelburri 0:31f7be68e52d 121 }
chrigelburri 0:31f7be68e52d 122
chrigelburri 0:31f7be68e52d 123 void MotionState::increment(float desiredSpeed, float desiredOmega, float period)
chrigelburri 0:31f7be68e52d 124 {
chrigelburri 0:31f7be68e52d 125 float acceleration = (desiredSpeed-speed)/period;
chrigelburri 0:31f7be68e52d 126 if (acceleration < -this->acceleration) acceleration = -this->acceleration;
chrigelburri 0:31f7be68e52d 127 else if (acceleration > this->acceleration) acceleration = this->acceleration;
chrigelburri 0:31f7be68e52d 128
chrigelburri 0:31f7be68e52d 129 float thetaAcceleration = (desiredOmega-omega)/period;
chrigelburri 0:31f7be68e52d 130 if (thetaAcceleration < -this->thetaAcceleration) thetaAcceleration = -this->thetaAcceleration;
chrigelburri 0:31f7be68e52d 131 else if (thetaAcceleration > this->thetaAcceleration) thetaAcceleration = this->thetaAcceleration;
chrigelburri 0:31f7be68e52d 132
chrigelburri 0:31f7be68e52d 133 speed += acceleration * period;
chrigelburri 0:31f7be68e52d 134 omega += thetaAcceleration * period;
chrigelburri 0:31f7be68e52d 135 }