motor control code by JYB
Dependencies: mbed-dev-f303 FastPWM3
Diff: PositionSensor/PositionSensor.cpp
- Revision:
- 45:aadebe074af6
- Parent:
- 38:67e4e1453a4b
- Child:
- 47:f4ecf3e0576a
--- a/PositionSensor/PositionSensor.cpp Sat Jul 14 22:03:52 2018 +0000 +++ b/PositionSensor/PositionSensor.cpp Mon Jul 30 20:25:24 2018 +0000 @@ -1,6 +1,7 @@ #include "mbed.h" #include "PositionSensor.h" +#include "math_ops.h" //#include "offset_lut.h" //#include <math.h> @@ -24,7 +25,7 @@ raw = 0; } -void PositionSensorAM5147::Sample(){ +void PositionSensorAM5147::Sample(float dt){ GPIOA->ODR &= ~(1 << 15); raw = spi->write(readAngleCmd); raw &= 0x3FFF; //Extract last 14 bits @@ -42,23 +43,26 @@ old_counts = angle; oldModPosition = modPosition; - modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR); - position = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR; + modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR); + position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR; MechPosition = position - MechOffset; - float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset; - if(elec < 0) elec += 6.28318530718f; - else if(elec > 6.28318530718f) elec -= 6.28318530718f ; + float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset; + if(elec < 0) elec += 2.0f*PI; + else if(elec > 2.0f*PI) elec -= 2.0f*PI ; ElecPosition = elec; float vel; - if(modPosition<.1f && oldModPosition>6.1f){ - vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f; + //if(modPosition<.1f && oldModPosition>6.1f){ + + if((modPosition-oldModPosition) < -3.0f){ + vel = (modPosition - oldModPosition + 2.0f*PI)/dt; } - else if(modPosition>6.1f && oldModPosition<0.1f){ - vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f; + //else if(modPosition>6.1f && oldModPosition<0.1f){ + else if((modPosition - oldModPosition) > 3.0f){ + vel = (modPosition - oldModPosition - 2.0f*PI)/dt; } else{ - vel = (modPosition-oldModPosition)*40000.0f; + vel = (modPosition-oldModPosition)/dt; } int n = 40; @@ -68,7 +72,7 @@ sum += velVec[n-i]; } velVec[0] = vel; - MechVelocity = sum/(float)n; + MechVelocity = sum/((float)n); ElecVelocity = MechVelocity*_ppairs; ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity; } @@ -100,7 +104,7 @@ void PositionSensorAM5147::ZeroPosition(){ rotations = 0; MechOffset = 0; - Sample(); + Sample(.00025f); MechOffset = GetMechPosition(); } @@ -189,7 +193,7 @@ //ZTest->write(1); } -void PositionSensorEncoder::Sample(){ +void PositionSensorEncoder::Sample(float dt){ }