Diff: PositionSensor/PositionSensor.cpp

Revision:

47:e1196a851f76

Parent:

46:2d4b1dafcfe3

Child:

48:74a40481740c

--- a/PositionSensor/PositionSensor.cpp	Thu Jul 12 02:50:34 2018 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Dec 05 04:07:46 2018 +0000
@@ -1,7 +1,7 @@
 
 #include "mbed.h"
 #include "PositionSensor.h"
-#include "math_ops.h"
+#include "../math_ops.h"
 //#include "offset_lut.h"
 //#include <math.h>
 
@@ -25,10 +25,12 @@
     raw = 0;
     }
     
-void PositionSensorAM5147::Sample(){
+void PositionSensorAM5147::Sample(float dt){
     GPIOA->ODR &= ~(1 << 15);
-    raw = spi->write(readAngleCmd);
-    raw &= 0x3FFF;                                                              //Extract last 14 bits
+    //raw = spi->write(readAngleCmd);
+    //raw &= 0x3FFF;   
+    raw = spi->write(0);
+    raw = raw>>1;                                                             //Extract last 14 bits
     GPIOA->ODR |= (1 << 15);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
@@ -53,15 +55,16 @@
     
     float vel;
     //if(modPosition<.1f && oldModPosition>6.1f){
+
     if((modPosition-oldModPosition) < -3.0f){
-        vel = (modPosition - oldModPosition + 2.0f*PI)*40000.0f;
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
         }
     //else if(modPosition>6.1f && oldModPosition<0.1f){
     else if((modPosition - oldModPosition) > 3.0f){
-        vel = (modPosition - oldModPosition - 2.0f*PI)*40000.0f;
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
         }
     else{
-        vel = (modPosition-oldModPosition)*40000.0f;
+        vel = (modPosition-oldModPosition)/dt;
     }    
     
     int n = 40;
@@ -103,7 +106,7 @@
 void PositionSensorAM5147::ZeroPosition(){
     rotations = 0;
     MechOffset = 0;
-    Sample();
+    Sample(.00025f);
     MechOffset = GetMechPosition();
     }
     
@@ -192,7 +195,7 @@
     //ZTest->write(1);
     }
     
-void PositionSensorEncoder::Sample(){
+void PositionSensorEncoder::Sample(float dt){
     
     }