[chronojump] FIXED getDisplacementInertial for ROTARYAXISINERTIALMOVPULLEY

[Xavier Padullés <xpadulles src gnome org>]

commit 6a3bf6fdfc2f19a5e8e495f1a20919aae1b5e5a1
Author: Xavier Padullés <x padulles gmail com>
Date:   Wed Mar 2 19:24:48 2016 +0100

    FIXED getDisplacementInertial for ROTARYAXISINERTIALMOVPULLEY

 encoder/util.R |   34 ++++++++++++++++++++++++++--------
 1 files changed, 26 insertions(+), 8 deletions(-)
---
diff --git a/encoder/util.R b/encoder/util.R
index bddd0c4..9484205 100644
--- a/encoder/util.R
+++ b/encoder/util.R
@@ -998,16 +998,34 @@ getDisplacementInertial <- function(displacement, encoderConfigurationName, diam
        } else if(encoderConfigurationName == "ROTARYFRICTIONSIDEINERTIALMOVPULLEY"){
          displacement = displacement * diameter /(gearedDown * diameterExt) #if gearedDown = 2 : Half the 
displacement of the axis
        } else if(encoderConfigurationName == "ROTARYAXISINERTIALMOVPULLEY"){
-         displacementMeters = displacement / 1000 #mm -> m
+         ticksRotaryEncoder = 200 #our rotary axis encoder send 200 ticks per revolution
+         
+         ####### Old code #########
          diameterMeters = diameter / 100 #cm -> m
-         ticksRotaryEncoder = 200 #our rotary axis encoder send 200 ticks per turn
-         #angle in radians
-         angle = cumsum(displacementMeters * 1000) * 2 * pi / ticksRotaryEncoder
-         position = angle * diameterMeters / 2
-         position = position * 1000 / gearedDown       #m -> mm        if gearedDown = 2 : Half the 
displacement of the axis
+         #displacementMeters = displacement / 1000 #mm -> m
+         #position = angle * diameterMeters / 2
+         #position = position * 1000 / gearedDown      #m -> mm        if gearedDown = 2 : Half the 
displacement of the axis
+         #angle = cumsum(displacementMeters * 1000) * 2 * pi / ticksRotaryEncoder
          #this is to make "inverted cumsum"
-         displacement = diff(position) #this displacement is going to be used now
-         displacement = c(displacement[1],displacement) #this is to recuperate the lost 1st value in the 
diff operation
+         #displacement = diff(position) #this displacement is going to be used now
+         #displacement = c(displacement[1],displacement) #this is to recuperate the lost 1st value in the 
diff operation
+         
+         print("Encoder signal")
+         print(displacement)
+         #Number of revolutions that the flywheel rotates every millisecond
+         revolutionsPerMs = displacement / ticksRotaryEncoder # One revolution every ticksRotaryEncoder ticks
+         print("revolutionPerMs")
+         print(revolutionsPerMs)
+         
+         #The person is gearedDown from the machine point of view 
+         #If force multiplier is 2 (gearedDown = 0.5) the displacement of the body is 
+         #half the the displacement at the perimeter of the axis
+         displacement = revolutionsPerMs * pi * diameter * 10 * gearedDown # Revolutions * perimeter * 
gearedDown  and converted cm -> mm
+         print("gearedDown:")
+         print(gearedDown)
+         print("Displacement")
+         print(displacement)
+         
        }