Diff: AnglePosition.cpp

Revision:

2:20afba507e9e

Parent:

1:5c789825341d

--- a/AnglePosition.cpp	Tue Oct 31 14:34:23 2017 +0000
+++ b/AnglePosition.cpp	Thu Nov 02 15:08:04 2017 +0000
@@ -1,6 +1,7 @@
 #include "AnglePosition.h"
 #include "mbed.h"
 
+//Declare pi
 const double PI = 3.14159265358979323846;
 
 AnglePosition::AnglePosition(void)
@@ -8,11 +9,7 @@
     
 }
 
-float AnglePosition::gettargetpositionpot(double input, int max_range){
-    float target = input * max_range;
-    return target;
-}
-
+//------------------------------------------------------------------------------
 float AnglePosition::gettargetposition(float input, int max_range){
     float target;
     if(input<=max_range && input>=0){
@@ -24,13 +21,13 @@
     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);
@@ -41,7 +38,7 @@
     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);
@@ -50,5 +47,4 @@
     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;
-}
-
+}
\ No newline at end of file