Easy Training / Mbed 2 deprecated Kinematics_old

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@4:ad48484f3a9e, 2020-04-07 (annotated)

Committer:
xenjamo
Date:
Tue Apr 07 11:05:00 2020 +0000
Revision:
4:ad48484f3a9e
Parent:
3:7e7159e2f589 
now added struct for data saving; use makeRM to create a Rotation matrix

Who changed what in which revision?

UserRevisionLine numberNew 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 4:ad48484f3a9e 14 #include "mbed.h"
xenjamo 1:79adfaec299a 15
xenjamo 1:79adfaec299a 16
xenjamo 1:79adfaec299a 17 //////////////////
xenjamo 1:79adfaec299a 18 //Kinetics Class//
xenjamo 1:79adfaec299a 19 //////////////////
xenjamo 1:79adfaec299a 20 Kinematics::Kinematics(int dataRate_){
xenjamo 1:79adfaec299a 21
xenjamo 1:79adfaec299a 22 this->dataRate = dataRate_;
xenjamo 1:79adfaec299a 23 this->inv_dataRate = 1/dataRate_;
xenjamo 1:79adfaec299a 24 timestamp = 0;
xenjamo 1:79adfaec299a 25
xenjamo 1:79adfaec299a 26
xenjamo 1:79adfaec299a 27
xenjamo 4:ad48484f3a9e 28 acc.reset();
xenjamo 4:ad48484f3a9e 29 lacc.reset();
xenjamo 1:79adfaec299a 30 vel.reset();
xenjamo 1:79adfaec299a 31 pos.reset();
xenjamo 1:79adfaec299a 32 }
xenjamo 0:6e2aac29a2ea 33
xenjamo 1:79adfaec299a 34 double Kinematics::uptadeDistTraveled(){
xenjamo 1:79adfaec299a 35 //distTraveled += vel.getabs();
xenjamo 1:79adfaec299a 36 return distTraveled;
xenjamo 1:79adfaec299a 37 }
xenjamo 1:79adfaec299a 38
xenjamo 4:ad48484f3a9e 39 void Kinematics::transver_vector(){
xenjamo 4:ad48484f3a9e 40
xenjamo 4:ad48484f3a9e 41 ///*** Rotation Matrix approach
xenjamo 4:ad48484f3a9e 42 ori.makeRM();
xenjamo 4:ad48484f3a9e 43 double ix,iy,iz;
xenjamo 4:ad48484f3a9e 44 ix = acc.x;
xenjamo 4:ad48484f3a9e 45 iy = acc.y;
xenjamo 4:ad48484f3a9e 46 iz = acc.z;
xenjamo 4:ad48484f3a9e 47 lacc.x = ix * ori.rm[0] + iy * ori.rm[1] + iz * ori.rm[2];
xenjamo 4:ad48484f3a9e 48 lacc.y = ix * ori.rm[3] + iy * ori.rm[4] + iz * ori.rm[5];
xenjamo 4:ad48484f3a9e 49 lacc.z = ix * ori.rm[6] + iy * ori.rm[7] + iz * ori.rm[8];
xenjamo 4:ad48484f3a9e 50 //*/
xenjamo 4:ad48484f3a9e 51
xenjamo 4:ad48484f3a9e 52 /*// vector approach
xenjamo 4:ad48484f3a9e 53 double tx,ty,tz;
xenjamo 4:ad48484f3a9e 54 double qw,qx,qy,qz;
xenjamo 4:ad48484f3a9e 55 qw = ori.qw;
xenjamo 4:ad48484f3a9e 56 qx = ori.qx;
xenjamo 4:ad48484f3a9e 57 qy = ori.qy;
xenjamo 4:ad48484f3a9e 58 qz = ori.qz;
xenjamo 4:ad48484f3a9e 59 double ix,iy,iz;
xenjamo 4:ad48484f3a9e 60 ix = acc.x;
xenjamo 4:ad48484f3a9e 61 iy = acc.y;
xenjamo 4:ad48484f3a9e 62 iz = acc.z;
xenjamo 4:ad48484f3a9e 63
xenjamo 4:ad48484f3a9e 64 tx = qy*iz - qz*iy + qw*ix;
xenjamo 4:ad48484f3a9e 65 ty = qz*ix - qx*iz + qw*iy;
xenjamo 4:ad48484f3a9e 66 tz = qx*iy - qy*ix + qw*iz;
xenjamo 4:ad48484f3a9e 67
xenjamo 4:ad48484f3a9e 68 //printf("%f\t%f\t%f\t\n",qw,qx,qy);
xenjamo 4:ad48484f3a9e 69
xenjamo 4:ad48484f3a9e 70 lacc.x = ix + (qy+qy)*tz - (qz+qz)*ty;
xenjamo 4:ad48484f3a9e 71 lacc.y = iy + (qz+qz)*tx - (qx+qx)*tz;
xenjamo 4:ad48484f3a9e 72 lacc.z = iz + (qx+qx)*ty - (qy+qy)*tx;
xenjamo 4:ad48484f3a9e 73 */
xenjamo 4:ad48484f3a9e 74 }
xenjamo 4:ad48484f3a9e 75
xenjamo 4:ad48484f3a9e 76
xenjamo 1:79adfaec299a 77 int Kinematics::updateTime(){
xenjamo 1:79adfaec299a 78 timestamp++;
xenjamo 1:79adfaec299a 79 return timestamp;
xenjamo 1:79adfaec299a 80 }
xenjamo 1:79adfaec299a 81
xenjamo 4:ad48484f3a9e 82 void Kinematics::updateAll(double ax, double ay, double az){ //mainly used for testing
xenjamo 1:79adfaec299a 83 updateTime();
xenjamo 4:ad48484f3a9e 84 lacc.set(ax,ay,az);
xenjamo 3:7e7159e2f589 85 updateVel();
xenjamo 3:7e7159e2f589 86 updatePos();
xenjamo 1:79adfaec299a 87
xenjamo 3:7e7159e2f589 88 }
xenjamo 3:7e7159e2f589 89
xenjamo 4:ad48484f3a9e 90 void Kinematics::updateAll(Accel lacc_){
xenjamo 3:7e7159e2f589 91 updateTime();
xenjamo 4:ad48484f3a9e 92 lacc = lacc_;
xenjamo 3:7e7159e2f589 93 updateVel();
xenjamo 3:7e7159e2f589 94 updatePos();
xenjamo 1:79adfaec299a 95
xenjamo 3:7e7159e2f589 96 }
xenjamo 3:7e7159e2f589 97
xenjamo 3:7e7159e2f589 98 void Kinematics::updateVel(){
xenjamo 4:ad48484f3a9e 99 vel.x += (lacc.x/dataRate);
xenjamo 4:ad48484f3a9e 100 vel.y += (lacc.y/dataRate);
xenjamo 4:ad48484f3a9e 101 vel.z += (lacc.z/dataRate);
xenjamo 3:7e7159e2f589 102 }
xenjamo 3:7e7159e2f589 103
xenjamo 3:7e7159e2f589 104 void Kinematics::updatePos(){
xenjamo 1:79adfaec299a 105 pos.x += vel.x/dataRate;
xenjamo 1:79adfaec299a 106 pos.y += vel.y/dataRate;
xenjamo 3:7e7159e2f589 107 pos.z += vel.z/dataRate;
xenjamo 1:79adfaec299a 108 }
xenjamo 1:79adfaec299a 109
xenjamo 1:79adfaec299a 110 void Kinematics::resetAll(){
xenjamo 4:ad48484f3a9e 111 acc.reset();
xenjamo 4:ad48484f3a9e 112 lacc.reset();
xenjamo 1:79adfaec299a 113 vel.reset();
xenjamo 1:79adfaec299a 114 pos.reset();
xenjamo 1:79adfaec299a 115 timestamp = 0;
xenjamo 1:79adfaec299a 116 }
xenjamo 1:79adfaec299a 117
xenjamo 1:79adfaec299a 118 int Kinematics::getTime(){
xenjamo 1:79adfaec299a 119 return timestamp;
xenjamo 1:79adfaec299a 120 }
xenjamo 1:79adfaec299a 121
xenjamo 1:79adfaec299a 122 double Kinematics::getTimeSec(){
xenjamo 1:79adfaec299a 123 return timestamp/dataRate;
xenjamo 1:79adfaec299a 124 }
xenjamo 1:79adfaec299a 125
xenjamo 1:79adfaec299a 126 //////////////////////
xenjamo 1:79adfaec299a 127 //Acceleration class//
xenjamo 1:79adfaec299a 128 //////////////////////
xenjamo 1:79adfaec299a 129
xenjamo 4:ad48484f3a9e 130 Accel::Accel(){
xenjamo 1:79adfaec299a 131 reset();
xenjamo 1:79adfaec299a 132 }
xenjamo 1:79adfaec299a 133
xenjamo 4:ad48484f3a9e 134 double Accel::getabs(){
xenjamo 1:79adfaec299a 135 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 136
xenjamo 1:79adfaec299a 137 }
xenjamo 1:79adfaec299a 138
xenjamo 4:ad48484f3a9e 139 void Accel::reset(){
xenjamo 1:79adfaec299a 140 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 141 }
xenjamo 1:79adfaec299a 142
xenjamo 4:ad48484f3a9e 143 void Accel::set(double ax, double ay, double az){
xenjamo 1:79adfaec299a 144 x = ax; y = ay; z = az;
xenjamo 1:79adfaec299a 145 }
xenjamo 1:79adfaec299a 146
xenjamo 1:79adfaec299a 147 //////////////////
xenjamo 1:79adfaec299a 148 //Velocity class//
xenjamo 1:79adfaec299a 149 //////////////////
xenjamo 1:79adfaec299a 150
xenjamo 1:79adfaec299a 151 Velocity::Velocity(){
xenjamo 1:79adfaec299a 152 reset();
xenjamo 1:79adfaec299a 153 }
xenjamo 1:79adfaec299a 154
xenjamo 1:79adfaec299a 155 double Velocity::getabs(){
xenjamo 1:79adfaec299a 156 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 157 }
xenjamo 1:79adfaec299a 158
xenjamo 1:79adfaec299a 159 void Velocity::reset(){
xenjamo 1:79adfaec299a 160 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 161 }
xenjamo 1:79adfaec299a 162
xenjamo 1:79adfaec299a 163 void Velocity::set(double vx, double vy, double vz){
xenjamo 1:79adfaec299a 164 x = vx; y = vy; z = vz;
xenjamo 1:79adfaec299a 165 }
xenjamo 1:79adfaec299a 166
xenjamo 1:79adfaec299a 167 //////////////////
xenjamo 1:79adfaec299a 168 //Position class//
xenjamo 1:79adfaec299a 169 //////////////////
xenjamo 1:79adfaec299a 170 Position::Position(){
xenjamo 1:79adfaec299a 171 reset();
xenjamo 1:79adfaec299a 172 }
xenjamo 1:79adfaec299a 173
xenjamo 1:79adfaec299a 174 double Position::getabs(){
xenjamo 1:79adfaec299a 175 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 176 }
xenjamo 1:79adfaec299a 177
xenjamo 1:79adfaec299a 178 double Position::getDistTo(double px, double py, double pz){
xenjamo 1:79adfaec299a 179 double dx = px-x;
xenjamo 1:79adfaec299a 180 double dy = py-y;
xenjamo 1:79adfaec299a 181 double dz = pz-z;
xenjamo 1:79adfaec299a 182 return sqrt(dx*dx+dy*dy+dz*dz);
xenjamo 1:79adfaec299a 183 }
xenjamo 1:79adfaec299a 184
xenjamo 1:79adfaec299a 185 void Position::reset(){
xenjamo 1:79adfaec299a 186 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 187 }
xenjamo 1:79adfaec299a 188
xenjamo 1:79adfaec299a 189 void Position::set(double px, double py, double pz){
xenjamo 1:79adfaec299a 190 x = px; y = py; z = pz;
xenjamo 0:6e2aac29a2ea 191 }
xenjamo 0:6e2aac29a2ea 192
xenjamo 3:7e7159e2f589 193 /////////////////////
xenjamo 3:7e7159e2f589 194 //Orientation class//
xenjamo 3:7e7159e2f589 195 /////////////////////
xenjamo 0:6e2aac29a2ea 196
xenjamo 3:7e7159e2f589 197 Orientation::Orientation(){
xenjamo 3:7e7159e2f589 198 resetQuat();
xenjamo 3:7e7159e2f589 199 resetEuler();
xenjamo 3:7e7159e2f589 200 }
xenjamo 1:79adfaec299a 201
xenjamo 3:7e7159e2f589 202 void Orientation::setQuat(double qw_, double qx_, double qy_, double qz_){
xenjamo 3:7e7159e2f589 203 qw = qw_;
xenjamo 3:7e7159e2f589 204 qx = qx_;
xenjamo 3:7e7159e2f589 205 qy = qy_;
xenjamo 3:7e7159e2f589 206 qz = qz_;
xenjamo 3:7e7159e2f589 207 }
xenjamo 3:7e7159e2f589 208
xenjamo 3:7e7159e2f589 209 void Orientation::resetQuat(){
xenjamo 3:7e7159e2f589 210 setQuat(0,0,0,0);
xenjamo 3:7e7159e2f589 211 }
xenjamo 1:79adfaec299a 212
xenjamo 3:7e7159e2f589 213 void Orientation::setEuler(double r_, double p_, double y_){
xenjamo 3:7e7159e2f589 214 r = r_;
xenjamo 3:7e7159e2f589 215 p = p_;
xenjamo 3:7e7159e2f589 216 y = y_;
xenjamo 3:7e7159e2f589 217 }
xenjamo 1:79adfaec299a 218
xenjamo 3:7e7159e2f589 219 void Orientation::resetEuler(){
xenjamo 3:7e7159e2f589 220 setEuler(0,0,0);
xenjamo 3:7e7159e2f589 221 }
xenjamo 3:7e7159e2f589 222
xenjamo 3:7e7159e2f589 223 void Orientation::setGRV(double grvX_, double grvY_, double grvZ_){
xenjamo 3:7e7159e2f589 224 grvX = grvX_;
xenjamo 3:7e7159e2f589 225 grvY = grvY_;
xenjamo 3:7e7159e2f589 226 grvZ = grvZ_;
xenjamo 3:7e7159e2f589 227 }
xenjamo 0:6e2aac29a2ea 228
xenjamo 3:7e7159e2f589 229 void Orientation::resetGRV(){
xenjamo 3:7e7159e2f589 230 setGRV(0,0,0);
xenjamo 3:7e7159e2f589 231 }
xenjamo 4:ad48484f3a9e 232
xenjamo 4:ad48484f3a9e 233 void Orientation::makeRM(){
xenjamo 4:ad48484f3a9e 234 /*****************
xenjamo 4:ad48484f3a9e 235 rotationmatrix structure:
xenjamo 4:ad48484f3a9e 236 /rm[0] rm[1] rm[2]\
xenjamo 4:ad48484f3a9e 237 ( rm[3] rm[4] rm[5] )
xenjamo 4:ad48484f3a9e 238 \rm[9] rm[7] rm[8]/
xenjamo 4:ad48484f3a9e 239
xenjamo 4:ad48484f3a9e 240 ******************/
xenjamo 4:ad48484f3a9e 241 double a=qw,b=qx,c=qy,d=qz;
xenjamo 4:ad48484f3a9e 242 rm[0]=a*a+b*b-c*c-d*d;
xenjamo 4:ad48484f3a9e 243 rm[1]=2*b*c-2*a*d;
xenjamo 4:ad48484f3a9e 244 rm[2]=2*b*d+2*a*c;
xenjamo 4:ad48484f3a9e 245 rm[3]=2*b*c+2*a*d;
xenjamo 4:ad48484f3a9e 246 rm[4]=a*a-b*b+c*c-d*d;
xenjamo 4:ad48484f3a9e 247 rm[5]=2*c*d-2*a*b;
xenjamo 4:ad48484f3a9e 248 rm[6]=2*b*d-2*a*c;
xenjamo 4:ad48484f3a9e 249 rm[7]=2*c*d+2*a*b;
xenjamo 4:ad48484f3a9e 250 rm[8]=a*a-b*b-c*c+d*d;
xenjamo 4:ad48484f3a9e 251 }
xenjamo 4:ad48484f3a9e 252
xenjamo 4:ad48484f3a9e 253
xenjamo 3:7e7159e2f589 254 /////////////////
xenjamo 3:7e7159e2f589 255 //RawData class//
xenjamo 3:7e7159e2f589 256 /////////////////
xenjamo 1:79adfaec299a 257
xenjamo 3:7e7159e2f589 258 RawData::RawData(){
xenjamo 3:7e7159e2f589 259 ax = 0;
xenjamo 3:7e7159e2f589 260 ay = 0;
xenjamo 3:7e7159e2f589 261 az = 0;
xenjamo 3:7e7159e2f589 262 resetMag();
xenjamo 3:7e7159e2f589 263 }
xenjamo 3:7e7159e2f589 264 void RawData::setMag(double mx_, double my_, double mz_){
xenjamo 3:7e7159e2f589 265 mx = mx_;
xenjamo 3:7e7159e2f589 266 my = my_;
xenjamo 3:7e7159e2f589 267 mz = mz_;
xenjamo 3:7e7159e2f589 268 }
xenjamo 0:6e2aac29a2ea 269
xenjamo 3:7e7159e2f589 270 void RawData::resetMag(){
xenjamo 3:7e7159e2f589 271 setMag(0,0,0);
xenjamo 3:7e7159e2f589 272 }
xenjamo 0:6e2aac29a2ea 273
xenjamo 3:7e7159e2f589 274 void RawData::setA(double ax_, double ay_, double az_){
xenjamo 3:7e7159e2f589 275 ax = ax_;
xenjamo 3:7e7159e2f589 276 ay = ay_;
xenjamo 3:7e7159e2f589 277 az = az_;
xenjamo 3:7e7159e2f589 278 }
xenjamo 1:79adfaec299a 279
xenjamo 3:7e7159e2f589 280 void RawData::resetA(){
xenjamo 3:7e7159e2f589 281 setA(0,0,0);
xenjamo 3:7e7159e2f589 282 }
xenjamo 3:7e7159e2f589 283
xenjamo 3:7e7159e2f589 284 void RawData::setG(double gx_, double gy_, double gz_){
xenjamo 3:7e7159e2f589 285 gx = gx_;
xenjamo 3:7e7159e2f589 286 gy = gy_;
xenjamo 3:7e7159e2f589 287 gz = gz_;
xenjamo 3:7e7159e2f589 288 }
xenjamo 3:7e7159e2f589 289
xenjamo 3:7e7159e2f589 290 void RawData::resetG(){
xenjamo 3:7e7159e2f589 291 setG(0,0,0);
xenjamo 3:7e7159e2f589 292 }
xenjamo 3:7e7159e2f589 293