From a46261cb6a1a3e8232088a14ba975671d2dda3a3 Mon Sep 17 00:00:00 2001
From: Noah Gleason <noah.gleason1@gmail.com>
Date: Tue, 17 Oct 2017 21:44:22 -0400
Subject: [PATCH] Get retroactive pose script working

---
 R scripts/poseEstimationChecker.R | 40 ++++++++++++++++++++++++-------
 1 file changed, 32 insertions(+), 8 deletions(-)

diff --git a/R scripts/poseEstimationChecker.R b/R scripts/poseEstimationChecker.R
index 9fc7cfc0..4cd70254 100644
--- a/R scripts/poseEstimationChecker.R	
+++ b/R scripts/poseEstimationChecker.R	
@@ -5,16 +5,18 @@ calcWheelbase <- function(left, right, angle){
   return((left-right)/angle);
 }
 
-smoothValue <- function(value, timeMillis, n){
-  smoothed <- (value[(n+1):length(value)] - value[1:(length(value)-n)])/(timeMillis[(n+1):length(timeMillis)] - timeMillis[1:(length(timeMillis)-n)])*1000;
-  return(c(rep(0, floor(n/2)), smoothed, rep(0, ceiling(n/2))));
+smoothDerivative <- function(value, timeMillis, n){
+  smoothed <- (value[(n+1):length(value)] - value[1:(length(value)-n)])/((timeMillis[(n+1):length(timeMillis)] - timeMillis[1:(length(timeMillis)-n)])/1000);
+  return(c(rep(0, ceiling(n/2)), smoothed, rep(0, floor(n/2))));
 }
 
 plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst){
+  rawAngle <- deg2rad(rawAngleDegrees)
+  
   #Smooth values
-  angular <- deg2rad(smoothValue(rawAngleDegrees, timeMillis, smoothingConst))
-  left <- smoothValue(leftPos, timeMillis, smoothingConst)
-  right <- smoothValue(rightPos, timeMillis, smoothingConst)
+  angular <- smoothDerivative(rawAngle, timeMillis, smoothingConst)
+  left <- smoothDerivative(leftPos, timeMillis, smoothingConst)
+  right <- smoothDerivative(rightPos, timeMillis, smoothingConst)
   
   #find effective wheelbase
   wheelbase <- calcWheelbase(left, right, angular)
@@ -37,7 +39,29 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
   #Plot turn radius
   plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2]-2.0833, xlab="Turn Radius (feet)", ylab="Error in wheelbase diameter (feet)", main="Error in Wheelbase Diameter vs. Turn Radius")
   abline(a=avgWheelbase-2.0833, b=0, col='green')
-
   
-  return(angular)
+  sumLeft <- 0
+  out <- array(dim=c(length(angular),7))
+  colnames(out)<-c("X","Y","leftX","leftY","rightX","rightY","time")
+  out[1,] <- c(leftPos[1],rightPos[1],NA,NA,NA,NA,timeMillis[1])
+  for(i in 2:length(angular)){
+    deltaTime <- (timeMillis[i] - out[i-1,7])/1000
+    deltaLeft <- leftPos[i]-leftPos[i-1]
+    deltaRight <- rightPos[i]-rightPos[i-1]
+    deltaTheta <- rawAngle[i]-rawAngle[i-1]
+    avgMoved <- (deltaLeft+deltaRight)/2
+    
+    sumLeft <- sumLeft + deltaLeft
+    if (deltaTheta == 0){
+      out[i,] <- c(out[i-1,1]+avgMoved*cos(rawAngle[i]),out[i-1,2]+avgMoved*sin(rawAngle[i]), NA, NA, NA, NA, timeMillis[i])
+    } else {
+      angle <- rawAngle[i-1]-(deltaTheta/2)
+      mag <- 2*(avgMoved/deltaTheta)*sin(deltaTheta/2)
+      out[i,] <- c(out[i-1,1]+mag*cos(angle), out[i-1,2]+mag*sin(angle), NA, NA, NA, NA, timeMillis[i])
+    }
+  }
+  
+  plot(out[,1], out[,2], t="l")
+  
+  return(sumLeft)
 }
\ No newline at end of file