Diff: geometry.cpp

Revision:

0:494acf21d3bc

Child:

2:fc869e45e672

diff -r 000000000000 -r 494acf21d3bc geometry.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometry.cpp	Mon Oct 31 13:05:53 2016 +0000
@@ -0,0 +1,60 @@
+#include "math.h"
+
+/*
+    Constants
+*/
+
+const float L_max = 50.0;   // Max arm length
+const float L_min = 30.0;   // Min arm length
+
+const float d = 30.0;       // Distance between arm centres of rotation 
+const float h = 30.0;       // Height of lower arm centre of rotation                       TODO -- Determine
+
+const float reach = 2*sqrt(pow(L_max,2)-pow(L_min,2))-d;    // Total length of wall that can be covered
+
+const float y_max = (2*h + d)/2 + reach/2;
+const float y_min = (2*h + d)/2 - reach/2;
+
+const float x_min = L_min;
+const float x_max = 0;                                                                          // TODO
+
+/*
+    Methods
+*/
+
+// Sets the required arm lengths in the 'upper' and 'lower' parameters
+void getArmLengthsForRollerPosition(float x, float y, float &upper, float &lower) {
+    lower = sqrt(pow(y-h,2)+pow(x,2));
+    upper = sqrt(pow(lower,2)+pow(d,2)-2*d*sqrt(pow(lower,2)-pow(x,2)));
+}
+
+// Sets the roller position corresponding to the current arm lengths in the x and y parameters
+void getRollerPositionForArmLengths(float upper, float lower, float &x, float &y) {
+    y=(pow(lower,2)-pow(upper,2)+2*h*d+pow(d,2))/(2*d);
+    x=sqrt(pow(upper,2)-pow(y-h,2)); //TODO
+}
+
+/*
+    Getters
+*/
+
+float getMaximalArmLength(){
+    return L_max;
+}
+
+float getMinimalArmLength(){
+    return L_min;
+}
+
+float getReach(){
+    return reach;
+}
+
+float getMaximalRollerHeight(){
+    return y_max;
+}
+
+float getMinimalRollerHeight(){
+    return y_min;
+}
+