Diff: kinematics.cpp

Revision:

1:79adfaec299a

Parent:

0:6e2aac29a2ea

Child:

3:7e7159e2f589

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kinematics.cpp	Fri Mar 20 19:36:22 2020 +0000
@@ -0,0 +1,150 @@
+/*
+kinematics library
+by clemo
+
+
+go to header file for instructions
+
+
+*/
+
+
+#include "kinematics.h"
+#include <math.h>
+
+
+//////////////////
+//Kinetics Class//
+//////////////////
+Kinematics::Kinematics(int dataRate_){
+    
+    this->dataRate = dataRate_;
+    this->inv_dataRate = 1/dataRate_;
+    timestamp = 0;
+    
+    
+    
+    accel.reset();
+    vel.reset();
+    pos.reset();
+}
+
+double Kinematics::uptadeDistTraveled(){
+    //distTraveled += vel.getabs();
+    return distTraveled;
+}
+
+int Kinematics::updateTime(){
+    timestamp++;
+    return timestamp;
+}
+
+void Kinematics::updateAll(double ax, double ay, double az){
+    updateTime();
+    
+    // do each integral seperatly in the future
+    
+    accel.x = ax;
+    accel.y = ay;
+    accel.z = az;
+    
+    vel.x += (accel.x/dataRate);
+    vel.y += (accel.y/dataRate);
+    vel.z += (accel.z/dataRate);
+    
+    pos.x += vel.x/dataRate;
+    pos.y += vel.y/dataRate;
+    pos.z += vel.z/dataRate;
+}
+
+void Kinematics::resetAll(){
+    accel.reset();
+    vel.reset();
+    pos.reset();    
+    timestamp = 0;
+}
+
+int Kinematics::getTime(){
+    return timestamp;
+}
+
+double Kinematics::getTimeSec(){
+    return timestamp/dataRate;
+}
+
+//////////////////////
+//Acceleration class//
+//////////////////////
+
+LinAccel::LinAccel(){
+    reset();
+}
+
+double LinAccel::getabs(){
+    return sqrt(x*x+z*z+y*y);
+    
+}
+
+void LinAccel::reset(){
+    set(0.0, 0.0, 0.0);
+}
+    
+void LinAccel::set(double ax, double ay, double az){
+    x = ax; y = ay; z = az;  
+}
+
+//////////////////
+//Velocity class//
+//////////////////
+
+Velocity::Velocity(){
+    reset();
+}
+
+double Velocity::getabs(){
+    return sqrt(x*x+z*z+y*y);
+}
+
+void Velocity::reset(){
+    set(0.0, 0.0, 0.0);
+}
+    
+void Velocity::set(double vx, double vy, double vz){
+    x = vx; y = vy; z = vz;  
+}
+
+//////////////////
+//Position class//
+//////////////////
+Position::Position(){
+    reset();
+}
+
+double Position::getabs(){
+    return sqrt(x*x+z*z+y*y);
+}
+
+double Position::getDistTo(double px, double py, double pz){
+    double dx = px-x;
+    double dy = py-y;
+    double dz = pz-z;
+    return sqrt(dx*dx+dy*dy+dz*dz);
+}
+
+void Position::reset(){
+    set(0.0, 0.0, 0.0);
+}
+    
+void Position::set(double px, double py, double pz){
+    x = px; y = py; z = pz;  
+}
+
+
+
+
+
+
+
+
+
+