Diff: AnglePosition.cpp

Revision:

0:d7e19af20f93

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AnglePosition.cpp	Thu Oct 26 10:53:29 2017 +0000
@@ -0,0 +1,43 @@
+#include "AnglePosition.h"
+#include "mbed.h"
+
+const double PI = 3.14159265358979323846;
+
+AnglePosition::AnglePosition(void)
+{
+    
+}
+
+float AnglePosition::gettargetposition(double input, int max_range){
+    float target = input * max_range;
+    return target;
+}
+
+
+float AnglePosition::getreferenceposition(float target, float offset) {
+    float  referenceposition = target + offset;
+    return referenceposition;
+}
+
+
+float AnglePosition::getalpha(float max_rangex, float max_rangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2, double inputx, double inputy) {
+    float x_target = gettargetposition(inputx, max_rangex);
+    float y_target = gettargetposition(inputy, max_rangey);
+    float x = getreferenceposition(x_target, x_offset);
+    float y = getreferenceposition(y_target, y_offset);        
+    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
+    float alpha = alpha_ + alpha_offset;
+    return alpha;
+}
+ 
+    
+float AnglePosition::getbeta(float max_rangex, float max_rangey, float x_offset, float y_offset, float beta_offset, float L1, float L2, double inputx, double inputy) {
+    float x_target = gettargetposition(inputx, max_rangex);
+    float y_target = gettargetposition(inputy, max_rangey);
+    float x = getreferenceposition(x_target, x_offset);
+    float y = getreferenceposition(y_target, y_offset);    
+    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
+    float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
+    return beta;
+}
+