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@1:79adfaec299a, 2020-03-20 (annotated)

Committer:
xenjamo
Date:
Fri Mar 20 19:36:22 2020 +0000
Revision:
1:79adfaec299a
Parent:
template.cpp@0:6e2aac29a2ea
Child:
3:7e7159e2f589
kinematics

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 1:79adfaec299a 44
xenjamo 1:79adfaec299a 45 // do each integral seperatly in the future
xenjamo 1:79adfaec299a 46
xenjamo 1:79adfaec299a 47 accel.x = ax;
xenjamo 1:79adfaec299a 48 accel.y = ay;
xenjamo 1:79adfaec299a 49 accel.z = az;
xenjamo 1:79adfaec299a 50
xenjamo 1:79adfaec299a 51 vel.x += (accel.x/dataRate);
xenjamo 1:79adfaec299a 52 vel.y += (accel.y/dataRate);
xenjamo 1:79adfaec299a 53 vel.z += (accel.z/dataRate);
xenjamo 0:6e2aac29a2ea 54
xenjamo 1:79adfaec299a 55 pos.x += vel.x/dataRate;
xenjamo 1:79adfaec299a 56 pos.y += vel.y/dataRate;
xenjamo 1:79adfaec299a 57 pos.z += vel.z/dataRate;
xenjamo 1:79adfaec299a 58 }
xenjamo 1:79adfaec299a 59
xenjamo 1:79adfaec299a 60 void Kinematics::resetAll(){
xenjamo 1:79adfaec299a 61 accel.reset();
xenjamo 1:79adfaec299a 62 vel.reset();
xenjamo 1:79adfaec299a 63 pos.reset();
xenjamo 1:79adfaec299a 64 timestamp = 0;
xenjamo 1:79adfaec299a 65 }
xenjamo 1:79adfaec299a 66
xenjamo 1:79adfaec299a 67 int Kinematics::getTime(){
xenjamo 1:79adfaec299a 68 return timestamp;
xenjamo 1:79adfaec299a 69 }
xenjamo 1:79adfaec299a 70
xenjamo 1:79adfaec299a 71 double Kinematics::getTimeSec(){
xenjamo 1:79adfaec299a 72 return timestamp/dataRate;
xenjamo 1:79adfaec299a 73 }
xenjamo 1:79adfaec299a 74
xenjamo 1:79adfaec299a 75 //////////////////////
xenjamo 1:79adfaec299a 76 //Acceleration class//
xenjamo 1:79adfaec299a 77 //////////////////////
xenjamo 1:79adfaec299a 78
xenjamo 1:79adfaec299a 79 LinAccel::LinAccel(){
xenjamo 1:79adfaec299a 80 reset();
xenjamo 1:79adfaec299a 81 }
xenjamo 1:79adfaec299a 82
xenjamo 1:79adfaec299a 83 double LinAccel::getabs(){
xenjamo 1:79adfaec299a 84 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 85
xenjamo 1:79adfaec299a 86 }
xenjamo 1:79adfaec299a 87
xenjamo 1:79adfaec299a 88 void LinAccel::reset(){
xenjamo 1:79adfaec299a 89 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 90 }
xenjamo 1:79adfaec299a 91
xenjamo 1:79adfaec299a 92 void LinAccel::set(double ax, double ay, double az){
xenjamo 1:79adfaec299a 93 x = ax; y = ay; z = az;
xenjamo 1:79adfaec299a 94 }
xenjamo 1:79adfaec299a 95
xenjamo 1:79adfaec299a 96 //////////////////
xenjamo 1:79adfaec299a 97 //Velocity class//
xenjamo 1:79adfaec299a 98 //////////////////
xenjamo 1:79adfaec299a 99
xenjamo 1:79adfaec299a 100 Velocity::Velocity(){
xenjamo 1:79adfaec299a 101 reset();
xenjamo 1:79adfaec299a 102 }
xenjamo 1:79adfaec299a 103
xenjamo 1:79adfaec299a 104 double Velocity::getabs(){
xenjamo 1:79adfaec299a 105 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 106 }
xenjamo 1:79adfaec299a 107
xenjamo 1:79adfaec299a 108 void Velocity::reset(){
xenjamo 1:79adfaec299a 109 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 110 }
xenjamo 1:79adfaec299a 111
xenjamo 1:79adfaec299a 112 void Velocity::set(double vx, double vy, double vz){
xenjamo 1:79adfaec299a 113 x = vx; y = vy; z = vz;
xenjamo 1:79adfaec299a 114 }
xenjamo 1:79adfaec299a 115
xenjamo 1:79adfaec299a 116 //////////////////
xenjamo 1:79adfaec299a 117 //Position class//
xenjamo 1:79adfaec299a 118 //////////////////
xenjamo 1:79adfaec299a 119 Position::Position(){
xenjamo 1:79adfaec299a 120 reset();
xenjamo 1:79adfaec299a 121 }
xenjamo 1:79adfaec299a 122
xenjamo 1:79adfaec299a 123 double Position::getabs(){
xenjamo 1:79adfaec299a 124 return sqrt(x*x+z*z+y*y);
xenjamo 1:79adfaec299a 125 }
xenjamo 1:79adfaec299a 126
xenjamo 1:79adfaec299a 127 double Position::getDistTo(double px, double py, double pz){
xenjamo 1:79adfaec299a 128 double dx = px-x;
xenjamo 1:79adfaec299a 129 double dy = py-y;
xenjamo 1:79adfaec299a 130 double dz = pz-z;
xenjamo 1:79adfaec299a 131 return sqrt(dx*dx+dy*dy+dz*dz);
xenjamo 1:79adfaec299a 132 }
xenjamo 1:79adfaec299a 133
xenjamo 1:79adfaec299a 134 void Position::reset(){
xenjamo 1:79adfaec299a 135 set(0.0, 0.0, 0.0);
xenjamo 1:79adfaec299a 136 }
xenjamo 1:79adfaec299a 137
xenjamo 1:79adfaec299a 138 void Position::set(double px, double py, double pz){
xenjamo 1:79adfaec299a 139 x = px; y = py; z = pz;
xenjamo 0:6e2aac29a2ea 140 }
xenjamo 0:6e2aac29a2ea 141
xenjamo 0:6e2aac29a2ea 142
xenjamo 1:79adfaec299a 143
xenjamo 1:79adfaec299a 144
xenjamo 1:79adfaec299a 145
xenjamo 0:6e2aac29a2ea 146
xenjamo 1:79adfaec299a 147
xenjamo 0:6e2aac29a2ea 148
xenjamo 0:6e2aac29a2ea 149
xenjamo 1:79adfaec299a 150