Test the set param mode
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){ }