Easy Training / Mbed 2 deprecated Kinematics_old

Dependencies:   mbed

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?

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 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