Shao Rui
123487
Dependencies: mbed-dev-f303 FastPWM3
Diff: PositionSensor/PositionSensor.cpp
- Revision:
- 22:60276ba87ac6
- Parent:
- 20:bf9ea5125d52
- Child:
- 23:2adf23ee0305
--- a/PositionSensor/PositionSensor.cpp Thu Mar 02 15:31:45 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp Fri Mar 31 18:24:46 2017 +0000
@@ -1,116 +1,86 @@
#include "mbed.h"
#include "PositionSensor.h"
+//#include "offset_lut.h"
//#include <math.h>
-PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
- //_CPR = CPR;
- _CPR = CPR;
- _ppairs = ppairs;
- _offset = offset;
- rotations = 0;
- spi = new SPI(PC_12, PC_11, PC_10);
- spi->format(16, 0);
- cs = new DigitalOut(PA_15);
- cs->write(1);
- MechOffset = 0;
- }
-
-int PositionSensorMA700::GetRawPosition(){
- cs->write(0);
- int response = spi->write(0)>>4;
- cs->write(1);
- return response;
- }
-
-
-float PositionSensorMA700::GetMechPosition(){
- cs->write(0);
- int response = spi->write(0)>>4;
- cs->write(1);
- if(response - old_counts > _CPR/4){
- rotations -= 1;
- }
- else if (response - old_counts < -_CPR/4){
- rotations += 1;
- }
- old_counts = response;
- MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
- //return MechPosition - MechOffset;
- return MechPosition;
- }
-
-float PositionSensorMA700::GetElecPosition(){
- cs->write(0);
- int response = spi->write(0)>>4;
- cs->write(1);
- float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*response)%_CPR)) - _offset;
- if(elec < 0) elec += 6.28318530718f;
- return elec;
- }
-
-float PositionSensorMA700::GetMechVelocity(){
- return 0;
- }
-
-void PositionSensorMA700::ZeroPosition(){
- rotations = 0;
- MechOffset = GetMechPosition();
- }
-
PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
//_CPR = CPR;
_CPR = CPR;
_ppairs = ppairs;
- _offset = offset;
+ ElecOffset = offset;
rotations = 0;
spi = new SPI(PC_12, PC_11, PC_10);
spi->format(16, 1);
spi->frequency(5000000);
cs = new DigitalOut(PA_15);
cs->write(1);
- readAngleCmd = 0xffff;
+ readAngleCmd = 0xffff;
MechOffset = 0;
+ modPosition = 0;
+ oldModPosition = 0;
+ oldVel = 0;
+ raw = 0;
+ }
+
+void PositionSensorAM5147::Sample(){
+ cs->write(0);
+ raw = spi->write(readAngleCmd);
+ raw &= 0x3FFF; //Extract last 14 bits
+ cs->write(1);
+ int angle = raw + offset_lut[raw>>7];
+ if(angle - old_counts > _CPR/2){
+ rotations -= 1;
+ }
+ else if (angle - old_counts < -_CPR/2){
+ rotations += 1;
+ }
+
+ old_counts = angle;
+ oldModPosition = modPosition;
+ modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR);
+ position = (6.28318530718f * ((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;
+ ElecPosition = elec;
+
+ float vel;
+ if(modPosition<.1f && oldModPosition>6.1f){
+ vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f;
+ }
+ else if(modPosition>6.1f && oldModPosition<0.1f){
+ vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f;
+ }
+ else{
+ vel = (modPosition-oldModPosition)*40000.0f;
+ }
+
+ int n = 16;
+ float sum = vel;
+ for (int i = 1; i < (n); i++){
+ velVec[n - i] = velVec[n-i-1];
+ sum += velVec[n-i];
+ }
+ velVec[0] = vel;
+ MechVelocity = sum/(float)n;
+ ElecVelocity = MechVelocity*_ppairs;
}
int PositionSensorAM5147::GetRawPosition(){
- cs->write(0);
- int angle = spi->write(0xffff);
- angle &= 0x3FFF; //Extract last 14 bits
- cs->write(1);
- return angle;
+ return raw;
}
-
float PositionSensorAM5147::GetMechPosition(){
- cs->write(0);
- int angle = spi->write(readAngleCmd);
- angle &= 0x3FFF; //Extract last 14 bits
- cs->write(1);
- if(angle - old_counts > _CPR/4){
- rotations -= 1;
- }
- else if (angle - old_counts < -_CPR/4){
- rotations += 1;
- }
- old_counts = angle;
- MechPosition = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
- //return MechPosition - MechOffset;
return MechPosition;
}
float PositionSensorAM5147::GetElecPosition(){
- cs->write(0);
- int angle = spi->write(readAngleCmd);
- angle &= 0x3FFF; //Extract last 14 bits
- cs->write(1);
- float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - _offset;
- if(elec < 0) elec += 6.28318530718f;
- return elec;
+ return ElecPosition;
}
float PositionSensorAM5147::GetMechVelocity(){
- return 0;
+ return MechVelocity;
}
void PositionSensorAM5147::ZeroPosition(){
@@ -118,13 +88,28 @@
MechOffset = GetMechPosition();
}
+void PositionSensorAM5147::SetElecOffset(float offset){
+ ElecOffset = offset;
+ }
+
+int PositionSensorAM5147::GetCPR(){
+ return _CPR;
+ }
+
+
+void PositionSensorAM5147::WriteLUT(int new_lut[128]){
+ memcpy(offset_lut, new_lut, sizeof(offset_lut));
+ }
-
+
PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
_ppairs = ppairs;
_CPR = CPR;
_offset = offset;
MechPosition = 0;
+ out_old = 0;
+ oldVel = 0;
+ raw = 0;
// Enable clock for GPIOA
__GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
@@ -140,25 +125,25 @@
// Enable clock for TIM3
__TIM3_CLK_ENABLE();
- TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1
- TIM3->SMCR = TIM_ENCODERMODE_TI12; // SMS='011' (Encoder mode 3) < TIM slave mode control register
- TIM3->CCMR1 = 0xf1f1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1, maximum digital filtering
- TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
- TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
- TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
- TIM3->ARR = CPR-1; // reload at 0xfffffff < TIM auto-reload register
+ TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1
+ TIM3->SMCR = TIM_ENCODERMODE_TI12; // SMS='011' (Encoder mode 3) < TIM slave mode control register
+ TIM3->CCMR1 = 0x1111; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1, maximum digital filtering
+ TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
+ TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
+ TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
+ TIM3->ARR = CPR; // IM auto-reload register
TIM3->CNT = 0x000; //reset the counter before we use it
// Extra Timer for velocity measurement
__TIM2_CLK_ENABLE();
- TIM3->CR2 = 0x030; //MMS = 101
+ TIM3->CR2 = 0x030; //MMS = 101
TIM2->PSC = 0x03;
//TIM2->CR2 |= TIM_CR2_TI1S;
- TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
- TIM2->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
+ TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
+ TIM2->CCMR1 = 0x3; // CC1S = 11, IC1 mapped on TRC
//TIM2->CR2 |= TIM_CR2_TI1S;
TIM2->CCER |= TIM_CCER_CC1P;
@@ -166,9 +151,9 @@
TIM2->CCER |= TIM_CCER_CC1E;
- TIM2->CR1 = 0x01; //CEN
+ TIM2->CR1 = 0x01; //CEN, enable timer
- TIM3->CR1 = 0x01; // CEN
+ TIM3->CR1 = 0x01; // CEN
ZPulse = new InterruptIn(PC_4);
ZSense = new DigitalIn(PC_4);
//ZPulse = new InterruptIn(PB_0);
@@ -182,17 +167,20 @@
//ZTest = new DigitalOut(PC_2);
//ZTest->write(1);
+ }
+void PositionSensorEncoder::Sample(){
-}
+ }
+
-float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians.
+float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians.
int raw = TIM3->CNT;
float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
}
-float PositionSensorEncoder::GetElecPosition() { //returns rotor electrical angle in radians.
+float PositionSensorEncoder::GetElecPosition() { //returns rotor electrical angle in radians.
int raw = TIM3->CNT;
float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
if(elec < 0) elec += 6.28318530718f;
@@ -202,22 +190,30 @@
float PositionSensorEncoder::GetMechVelocity(){
+
float out = 0;
float rawPeriod = TIM2->CCR1; //Clock Ticks
-
+ int currentTime = TIM2->CNT;
+ if(currentTime > 2000000){rawPeriod = currentTime;}
float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; // +/- 1
- float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
+ float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
+ if(isinf(meas)){ meas = 1;}
out = meas;
- if(meas == vel_old){
- out = .99f*out_old;
+ //if(meas == oldVel){
+ // out = .9f*out_old;
+ // }
+
+
+ oldVel = meas;
+ out_old = out;
+ int n = 16;
+ float sum = out;
+ for (int i = 1; i < (n); i++){
+ velVec[n - i] = velVec[n-i-1];
+ sum += velVec[n-i];
}
- else{
- out = meas;
- }
-
- vel_old = meas;
- out_old = out;
- return out;
+ velVec[0] = out;
+ return sum/(float)n;
}
float PositionSensorEncoder::GetElecVelocity(){
@@ -236,6 +232,10 @@
}
}
}
+
+void PositionSensorEncoder::ZeroPosition(void){
+
+ }
void PositionSensorEncoder::ZeroEncoderCountDown(void){
if (ZSense->read() == 0){
@@ -252,4 +252,20 @@
}
}
- }
\ No newline at end of file
+ }
+void PositionSensorEncoder::SetElecOffset(float offset){
+
+ }
+
+int PositionSensorEncoder::GetRawPosition(void){
+ return 0;
+ }
+
+int PositionSensorEncoder::GetCPR(){
+ return _CPR;
+ }
+
+
+void PositionSensorEncoder::WriteLUT(int new_lut[128]){
+ memcpy(offset_lut, new_lut, sizeof(offset_lut));
+ }