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
kinematics.cpp@3:7e7159e2f589, 2020-03-30 (annotated)
- Committer:
- xenjamo
- Date:
- Mon Mar 30 09:22:22 2020 +0000
- Revision:
- 3:7e7159e2f589
- Parent:
- 1:79adfaec299a
- Child:
- 4:ad48484f3a9e
updated some functions and added orientation and rawdata class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
xenjamo | 0:6e2aac29a2ea | 1 | /* |
xenjamo | 1:79adfaec299a | 2 | kinematics library |
xenjamo | 1:79adfaec299a | 3 | by clemo |
xenjamo | 0:6e2aac29a2ea | 4 | |
xenjamo | 0:6e2aac29a2ea | 5 | |
xenjamo | 1:79adfaec299a | 6 | go to header file for instructions |
xenjamo | 0:6e2aac29a2ea | 7 | |
xenjamo | 0:6e2aac29a2ea | 8 | |
xenjamo | 0:6e2aac29a2ea | 9 | */ |
xenjamo | 0:6e2aac29a2ea | 10 | |
xenjamo | 0:6e2aac29a2ea | 11 | |
xenjamo | 1:79adfaec299a | 12 | #include "kinematics.h" |
xenjamo | 1:79adfaec299a | 13 | #include <math.h> |
xenjamo | 1:79adfaec299a | 14 | |
xenjamo | 1:79adfaec299a | 15 | |
xenjamo | 1:79adfaec299a | 16 | ////////////////// |
xenjamo | 1:79adfaec299a | 17 | //Kinetics Class// |
xenjamo | 1:79adfaec299a | 18 | ////////////////// |
xenjamo | 1:79adfaec299a | 19 | Kinematics::Kinematics(int dataRate_){ |
xenjamo | 1:79adfaec299a | 20 | |
xenjamo | 1:79adfaec299a | 21 | this->dataRate = dataRate_; |
xenjamo | 1:79adfaec299a | 22 | this->inv_dataRate = 1/dataRate_; |
xenjamo | 1:79adfaec299a | 23 | timestamp = 0; |
xenjamo | 1:79adfaec299a | 24 | |
xenjamo | 1:79adfaec299a | 25 | |
xenjamo | 1:79adfaec299a | 26 | |
xenjamo | 1:79adfaec299a | 27 | accel.reset(); |
xenjamo | 1:79adfaec299a | 28 | vel.reset(); |
xenjamo | 1:79adfaec299a | 29 | pos.reset(); |
xenjamo | 1:79adfaec299a | 30 | } |
xenjamo | 0:6e2aac29a2ea | 31 | |
xenjamo | 1:79adfaec299a | 32 | double Kinematics::uptadeDistTraveled(){ |
xenjamo | 1:79adfaec299a | 33 | //distTraveled += vel.getabs(); |
xenjamo | 1:79adfaec299a | 34 | return distTraveled; |
xenjamo | 1:79adfaec299a | 35 | } |
xenjamo | 1:79adfaec299a | 36 | |
xenjamo | 1:79adfaec299a | 37 | int Kinematics::updateTime(){ |
xenjamo | 1:79adfaec299a | 38 | timestamp++; |
xenjamo | 1:79adfaec299a | 39 | return timestamp; |
xenjamo | 1:79adfaec299a | 40 | } |
xenjamo | 1:79adfaec299a | 41 | |
xenjamo | 1:79adfaec299a | 42 | void Kinematics::updateAll(double ax, double ay, double az){ |
xenjamo | 1:79adfaec299a | 43 | updateTime(); |
xenjamo | 3:7e7159e2f589 | 44 | accel.set(ax,ay,az); |
xenjamo | 3:7e7159e2f589 | 45 | updateVel(); |
xenjamo | 3:7e7159e2f589 | 46 | updatePos(); |
xenjamo | 1:79adfaec299a | 47 | |
xenjamo | 3:7e7159e2f589 | 48 | } |
xenjamo | 3:7e7159e2f589 | 49 | |
xenjamo | 3:7e7159e2f589 | 50 | void Kinematics::updateAll(LinAccel accel_){ |
xenjamo | 3:7e7159e2f589 | 51 | updateTime(); |
xenjamo | 3:7e7159e2f589 | 52 | accel = accel_; |
xenjamo | 3:7e7159e2f589 | 53 | updateVel(); |
xenjamo | 3:7e7159e2f589 | 54 | updatePos(); |
xenjamo | 1:79adfaec299a | 55 | |
xenjamo | 3:7e7159e2f589 | 56 | } |
xenjamo | 3:7e7159e2f589 | 57 | |
xenjamo | 3:7e7159e2f589 | 58 | void Kinematics::updateVel(){ |
xenjamo | 1:79adfaec299a | 59 | vel.x += (accel.x/dataRate); |
xenjamo | 1:79adfaec299a | 60 | vel.y += (accel.y/dataRate); |
xenjamo | 1:79adfaec299a | 61 | vel.z += (accel.z/dataRate); |
xenjamo | 3:7e7159e2f589 | 62 | } |
xenjamo | 3:7e7159e2f589 | 63 | |
xenjamo | 3:7e7159e2f589 | 64 | void Kinematics::updatePos(){ |
xenjamo | 1:79adfaec299a | 65 | pos.x += vel.x/dataRate; |
xenjamo | 1:79adfaec299a | 66 | pos.y += vel.y/dataRate; |
xenjamo | 3:7e7159e2f589 | 67 | pos.z += vel.z/dataRate; |
xenjamo | 1:79adfaec299a | 68 | } |
xenjamo | 1:79adfaec299a | 69 | |
xenjamo | 1:79adfaec299a | 70 | void Kinematics::resetAll(){ |
xenjamo | 1:79adfaec299a | 71 | accel.reset(); |
xenjamo | 1:79adfaec299a | 72 | vel.reset(); |
xenjamo | 1:79adfaec299a | 73 | pos.reset(); |
xenjamo | 1:79adfaec299a | 74 | timestamp = 0; |
xenjamo | 1:79adfaec299a | 75 | } |
xenjamo | 1:79adfaec299a | 76 | |
xenjamo | 1:79adfaec299a | 77 | int Kinematics::getTime(){ |
xenjamo | 1:79adfaec299a | 78 | return timestamp; |
xenjamo | 1:79adfaec299a | 79 | } |
xenjamo | 1:79adfaec299a | 80 | |
xenjamo | 1:79adfaec299a | 81 | double Kinematics::getTimeSec(){ |
xenjamo | 1:79adfaec299a | 82 | return timestamp/dataRate; |
xenjamo | 1:79adfaec299a | 83 | } |
xenjamo | 1:79adfaec299a | 84 | |
xenjamo | 1:79adfaec299a | 85 | ////////////////////// |
xenjamo | 1:79adfaec299a | 86 | //Acceleration class// |
xenjamo | 1:79adfaec299a | 87 | ////////////////////// |
xenjamo | 1:79adfaec299a | 88 | |
xenjamo | 1:79adfaec299a | 89 | LinAccel::LinAccel(){ |
xenjamo | 1:79adfaec299a | 90 | reset(); |
xenjamo | 1:79adfaec299a | 91 | } |
xenjamo | 1:79adfaec299a | 92 | |
xenjamo | 1:79adfaec299a | 93 | double LinAccel::getabs(){ |
xenjamo | 1:79adfaec299a | 94 | return sqrt(x*x+z*z+y*y); |
xenjamo | 1:79adfaec299a | 95 | |
xenjamo | 1:79adfaec299a | 96 | } |
xenjamo | 1:79adfaec299a | 97 | |
xenjamo | 1:79adfaec299a | 98 | void LinAccel::reset(){ |
xenjamo | 1:79adfaec299a | 99 | set(0.0, 0.0, 0.0); |
xenjamo | 1:79adfaec299a | 100 | } |
xenjamo | 1:79adfaec299a | 101 | |
xenjamo | 1:79adfaec299a | 102 | void LinAccel::set(double ax, double ay, double az){ |
xenjamo | 1:79adfaec299a | 103 | x = ax; y = ay; z = az; |
xenjamo | 1:79adfaec299a | 104 | } |
xenjamo | 1:79adfaec299a | 105 | |
xenjamo | 1:79adfaec299a | 106 | ////////////////// |
xenjamo | 1:79adfaec299a | 107 | //Velocity class// |
xenjamo | 1:79adfaec299a | 108 | ////////////////// |
xenjamo | 1:79adfaec299a | 109 | |
xenjamo | 1:79adfaec299a | 110 | Velocity::Velocity(){ |
xenjamo | 1:79adfaec299a | 111 | reset(); |
xenjamo | 1:79adfaec299a | 112 | } |
xenjamo | 1:79adfaec299a | 113 | |
xenjamo | 1:79adfaec299a | 114 | double Velocity::getabs(){ |
xenjamo | 1:79adfaec299a | 115 | return sqrt(x*x+z*z+y*y); |
xenjamo | 1:79adfaec299a | 116 | } |
xenjamo | 1:79adfaec299a | 117 | |
xenjamo | 1:79adfaec299a | 118 | void Velocity::reset(){ |
xenjamo | 1:79adfaec299a | 119 | set(0.0, 0.0, 0.0); |
xenjamo | 1:79adfaec299a | 120 | } |
xenjamo | 1:79adfaec299a | 121 | |
xenjamo | 1:79adfaec299a | 122 | void Velocity::set(double vx, double vy, double vz){ |
xenjamo | 1:79adfaec299a | 123 | x = vx; y = vy; z = vz; |
xenjamo | 1:79adfaec299a | 124 | } |
xenjamo | 1:79adfaec299a | 125 | |
xenjamo | 1:79adfaec299a | 126 | ////////////////// |
xenjamo | 1:79adfaec299a | 127 | //Position class// |
xenjamo | 1:79adfaec299a | 128 | ////////////////// |
xenjamo | 1:79adfaec299a | 129 | Position::Position(){ |
xenjamo | 1:79adfaec299a | 130 | reset(); |
xenjamo | 1:79adfaec299a | 131 | } |
xenjamo | 1:79adfaec299a | 132 | |
xenjamo | 1:79adfaec299a | 133 | double Position::getabs(){ |
xenjamo | 1:79adfaec299a | 134 | return sqrt(x*x+z*z+y*y); |
xenjamo | 1:79adfaec299a | 135 | } |
xenjamo | 1:79adfaec299a | 136 | |
xenjamo | 1:79adfaec299a | 137 | double Position::getDistTo(double px, double py, double pz){ |
xenjamo | 1:79adfaec299a | 138 | double dx = px-x; |
xenjamo | 1:79adfaec299a | 139 | double dy = py-y; |
xenjamo | 1:79adfaec299a | 140 | double dz = pz-z; |
xenjamo | 1:79adfaec299a | 141 | return sqrt(dx*dx+dy*dy+dz*dz); |
xenjamo | 1:79adfaec299a | 142 | } |
xenjamo | 1:79adfaec299a | 143 | |
xenjamo | 1:79adfaec299a | 144 | void Position::reset(){ |
xenjamo | 1:79adfaec299a | 145 | set(0.0, 0.0, 0.0); |
xenjamo | 1:79adfaec299a | 146 | } |
xenjamo | 1:79adfaec299a | 147 | |
xenjamo | 1:79adfaec299a | 148 | void Position::set(double px, double py, double pz){ |
xenjamo | 1:79adfaec299a | 149 | x = px; y = py; z = pz; |
xenjamo | 0:6e2aac29a2ea | 150 | } |
xenjamo | 0:6e2aac29a2ea | 151 | |
xenjamo | 3:7e7159e2f589 | 152 | ///////////////////// |
xenjamo | 3:7e7159e2f589 | 153 | //Orientation class// |
xenjamo | 3:7e7159e2f589 | 154 | ///////////////////// |
xenjamo | 0:6e2aac29a2ea | 155 | |
xenjamo | 3:7e7159e2f589 | 156 | Orientation::Orientation(){ |
xenjamo | 3:7e7159e2f589 | 157 | resetQuat(); |
xenjamo | 3:7e7159e2f589 | 158 | resetEuler(); |
xenjamo | 3:7e7159e2f589 | 159 | } |
xenjamo | 1:79adfaec299a | 160 | |
xenjamo | 3:7e7159e2f589 | 161 | void Orientation::setQuat(double qw_, double qx_, double qy_, double qz_){ |
xenjamo | 3:7e7159e2f589 | 162 | qw = qw_; |
xenjamo | 3:7e7159e2f589 | 163 | qx = qx_; |
xenjamo | 3:7e7159e2f589 | 164 | qy = qy_; |
xenjamo | 3:7e7159e2f589 | 165 | qz = qz_; |
xenjamo | 3:7e7159e2f589 | 166 | } |
xenjamo | 3:7e7159e2f589 | 167 | |
xenjamo | 3:7e7159e2f589 | 168 | void Orientation::resetQuat(){ |
xenjamo | 3:7e7159e2f589 | 169 | setQuat(0,0,0,0); |
xenjamo | 3:7e7159e2f589 | 170 | } |
xenjamo | 1:79adfaec299a | 171 | |
xenjamo | 3:7e7159e2f589 | 172 | void Orientation::setEuler(double r_, double p_, double y_){ |
xenjamo | 3:7e7159e2f589 | 173 | r = r_; |
xenjamo | 3:7e7159e2f589 | 174 | p = p_; |
xenjamo | 3:7e7159e2f589 | 175 | y = y_; |
xenjamo | 3:7e7159e2f589 | 176 | } |
xenjamo | 1:79adfaec299a | 177 | |
xenjamo | 3:7e7159e2f589 | 178 | void Orientation::resetEuler(){ |
xenjamo | 3:7e7159e2f589 | 179 | setEuler(0,0,0); |
xenjamo | 3:7e7159e2f589 | 180 | } |
xenjamo | 3:7e7159e2f589 | 181 | |
xenjamo | 3:7e7159e2f589 | 182 | void Orientation::setGRV(double grvX_, double grvY_, double grvZ_){ |
xenjamo | 3:7e7159e2f589 | 183 | grvX = grvX_; |
xenjamo | 3:7e7159e2f589 | 184 | grvY = grvY_; |
xenjamo | 3:7e7159e2f589 | 185 | grvZ = grvZ_; |
xenjamo | 3:7e7159e2f589 | 186 | } |
xenjamo | 0:6e2aac29a2ea | 187 | |
xenjamo | 3:7e7159e2f589 | 188 | void Orientation::resetGRV(){ |
xenjamo | 3:7e7159e2f589 | 189 | setGRV(0,0,0); |
xenjamo | 3:7e7159e2f589 | 190 | } |
xenjamo | 3:7e7159e2f589 | 191 | ///////////////// |
xenjamo | 3:7e7159e2f589 | 192 | //RawData class// |
xenjamo | 3:7e7159e2f589 | 193 | ///////////////// |
xenjamo | 1:79adfaec299a | 194 | |
xenjamo | 3:7e7159e2f589 | 195 | RawData::RawData(){ |
xenjamo | 3:7e7159e2f589 | 196 | ax = 0; |
xenjamo | 3:7e7159e2f589 | 197 | ay = 0; |
xenjamo | 3:7e7159e2f589 | 198 | az = 0; |
xenjamo | 3:7e7159e2f589 | 199 | resetMag(); |
xenjamo | 3:7e7159e2f589 | 200 | } |
xenjamo | 3:7e7159e2f589 | 201 | void RawData::setMag(double mx_, double my_, double mz_){ |
xenjamo | 3:7e7159e2f589 | 202 | mx = mx_; |
xenjamo | 3:7e7159e2f589 | 203 | my = my_; |
xenjamo | 3:7e7159e2f589 | 204 | mz = mz_; |
xenjamo | 3:7e7159e2f589 | 205 | } |
xenjamo | 0:6e2aac29a2ea | 206 | |
xenjamo | 3:7e7159e2f589 | 207 | void RawData::resetMag(){ |
xenjamo | 3:7e7159e2f589 | 208 | setMag(0,0,0); |
xenjamo | 3:7e7159e2f589 | 209 | } |
xenjamo | 0:6e2aac29a2ea | 210 | |
xenjamo | 3:7e7159e2f589 | 211 | void RawData::setA(double ax_, double ay_, double az_){ |
xenjamo | 3:7e7159e2f589 | 212 | ax = ax_; |
xenjamo | 3:7e7159e2f589 | 213 | ay = ay_; |
xenjamo | 3:7e7159e2f589 | 214 | az = az_; |
xenjamo | 3:7e7159e2f589 | 215 | } |
xenjamo | 1:79adfaec299a | 216 | |
xenjamo | 3:7e7159e2f589 | 217 | void RawData::resetA(){ |
xenjamo | 3:7e7159e2f589 | 218 | setA(0,0,0); |
xenjamo | 3:7e7159e2f589 | 219 | } |
xenjamo | 3:7e7159e2f589 | 220 | |
xenjamo | 3:7e7159e2f589 | 221 | void RawData::setG(double gx_, double gy_, double gz_){ |
xenjamo | 3:7e7159e2f589 | 222 | gx = gx_; |
xenjamo | 3:7e7159e2f589 | 223 | gy = gy_; |
xenjamo | 3:7e7159e2f589 | 224 | gz = gz_; |
xenjamo | 3:7e7159e2f589 | 225 | } |
xenjamo | 3:7e7159e2f589 | 226 | |
xenjamo | 3:7e7159e2f589 | 227 | void RawData::resetG(){ |
xenjamo | 3:7e7159e2f589 | 228 | setG(0,0,0); |
xenjamo | 3:7e7159e2f589 | 229 | } |
xenjamo | 3:7e7159e2f589 | 230 |