PositionSensor.cpp

00001 
00002 #include "mbed.h"
00003 #include "PositionSensor.h"
00004 //#include <math.h>
00005 
00006 PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
00007     //_CPR = CPR;
00008     _CPR = CPR;
00009     _ppairs = ppairs;
00010     _offset = offset;
00011     rotations = 0;
00012     spi = new SPI(PC_12, PC_11, PC_10);
00013     spi->format(16, 0);
00014     cs = new DigitalOut(PA_15);
00015     cs->write(1);
00016     MechOffset = 0;
00017     }
00018 
00019 int PositionSensorMA700::GetRawPosition(){
00020         cs->write(0);
00021     int response = spi->write(0)>>4;
00022     cs->write(1);
00023     return response;
00024     }
00025     
00026 
00027 float PositionSensorMA700::GetMechPosition(){
00028     cs->write(0);
00029     int response = spi->write(0)>>4;
00030     cs->write(1);
00031     if(response - old_counts > _CPR/4){
00032         rotations -= 1;
00033         }
00034     else if (response - old_counts < -_CPR/4){
00035         rotations += 1;
00036         }
00037     old_counts = response;
00038     MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
00039     //return MechPosition - MechOffset;
00040     return MechPosition;
00041     }
00042 
00043 float PositionSensorMA700::GetElecPosition(){
00044     cs->write(0);
00045     int response = spi->write(0)>>4;
00046     cs->write(1);
00047     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*response)%_CPR)) - _offset;
00048     if(elec < 0) elec += 6.28318530718f;
00049     return elec;
00050     }
00051 
00052 float PositionSensorMA700::GetMechVelocity(){
00053     return 0;
00054     }
00055 
00056 void PositionSensorMA700::ZeroPosition(){
00057     rotations = 0;
00058     MechOffset = GetMechPosition();
00059     }
00060     
00061 PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
00062     //_CPR = CPR;
00063     _CPR = CPR;
00064     _ppairs = ppairs;
00065     _offset = offset;
00066     rotations = 0;
00067     spi = new SPI(PC_12, PC_11, PC_10);
00068     spi->format(16, 1);
00069     spi->frequency(10000000);
00070     cs = new DigitalOut(PA_15);
00071     cs->write(1);
00072     readAngleCmd = 0xffff;
00073     MechOffset = 0;
00074     }
00075 
00076 int PositionSensorAM5147::GetRawPosition(){
00077     cs->write(0);
00078     int angle = spi->write(0xffff);
00079     angle &= 0x3FFF;    //Extract last 14 bits
00080     cs->write(1);    
00081     return angle;
00082     }
00083     
00084 
00085 float PositionSensorAM5147::GetMechPosition(){
00086     cs->write(0);
00087     int angle = spi->write(readAngleCmd);
00088     angle &= 0x3FFF;    //Extract last 14 bits
00089     cs->write(1);
00090     if(angle - old_counts > _CPR/4){
00091         rotations -= 1;
00092         }
00093     else if (angle - old_counts < -_CPR/4){
00094         rotations += 1;
00095         }
00096     old_counts = angle;
00097     MechPosition = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
00098     //return MechPosition - MechOffset;
00099     return MechPosition;
00100     }
00101 
00102 float PositionSensorAM5147::GetElecPosition(){
00103     cs->write(0);
00104     int angle = spi->write(readAngleCmd);
00105     angle &= 0x3FFF;    //Extract last 14 bits
00106     cs->write(1);
00107     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - _offset;
00108     if(elec < 0) elec += 6.28318530718f;
00109     return elec;
00110     }
00111 
00112 float PositionSensorAM5147::GetMechVelocity(){
00113     return 0;
00114     }
00115 
00116 void PositionSensorAM5147::ZeroPosition(){
00117     rotations = 0;
00118     MechOffset = GetMechPosition();
00119     }
00120     
00121 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
00122     _ppairs = ppairs;
00123     _CPR = CPR;
00124     _offset = offset;
00125     MechPosition = 0;
00126     
00127     // Enable clock for GPIOA
00128     __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
00129  
00130     GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
00131     GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
00132     GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
00133     GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
00134     GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00135     GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00136    
00137     // configure TIM3 as Encoder input
00138     // Enable clock for TIM3
00139     __TIM3_CLK_ENABLE();
00140  
00141     TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1
00142     TIM3->SMCR  = TIM_ENCODERMODE_TI12;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
00143     TIM3->CCMR1 = 0xf1f1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
00144     TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
00145     TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
00146     TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
00147     TIM3->ARR   = CPR-1; // reload at 0xfffffff         < TIM auto-reload register
00148   
00149     TIM3->CNT = 0x000;  //reset the counter before we use it  
00150     
00151     // Extra Timer for velocity measurement
00152     
00153     __TIM2_CLK_ENABLE();
00154     TIM3->CR2 = 0x030;  //MMS = 101
00155     
00156     TIM2->PSC = 0x03;
00157     //TIM2->CR2 |= TIM_CR2_TI1S;
00158     TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
00159     TIM2->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
00160     
00161     //TIM2->CR2 |= TIM_CR2_TI1S;
00162     TIM2->CCER |= TIM_CCER_CC1P;
00163     //TIM2->CCER |= TIM_CCER_CC1NP;
00164     TIM2->CCER |= TIM_CCER_CC1E;
00165     
00166     
00167     TIM2->CR1 = 0x01;       //CEN
00168     
00169     TIM3->CR1   = 0x01;     // CEN
00170     ZPulse = new InterruptIn(PC_4);
00171     ZSense = new DigitalIn(PC_4);
00172     //ZPulse = new InterruptIn(PB_0);
00173     //ZSense = new DigitalIn(PB_0);
00174     ZPulse->enable_irq();
00175     ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
00176     //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
00177     ZPulse->mode(PullDown);
00178     flag = 0;
00179 
00180     
00181     //ZTest = new DigitalOut(PC_2);
00182     //ZTest->write(1);
00183     
00184     
00185 }
00186  
00187 float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
00188     int raw = TIM3->CNT;
00189     float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
00190     return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
00191 }
00192 
00193 float PositionSensorEncoder::GetElecPosition() {        //returns rotor electrical angle in radians.
00194     int raw = TIM3->CNT;
00195     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
00196     if(elec < 0) elec += 6.28318530718f;
00197     return elec;
00198 }
00199 
00200 
00201     
00202 float PositionSensorEncoder::GetMechVelocity(){
00203     float out = 0;
00204     float rawPeriod = TIM2->CCR1; //Clock Ticks
00205     
00206     float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
00207     float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
00208     out = meas;
00209     if(meas == vel_old){
00210         out = .99f*out_old;
00211         }
00212     else{
00213         out = meas;
00214         }
00215     
00216     vel_old = meas;
00217     out_old = out;
00218     return out;
00219     }
00220     
00221 float PositionSensorEncoder::GetElecVelocity(){
00222     return _ppairs*GetMechVelocity();
00223     }
00224     
00225 void PositionSensorEncoder::ZeroEncoderCount(void){
00226     if (ZSense->read() == 1 & flag == 0){
00227         if (ZSense->read() == 1){
00228             GPIOC->ODR ^= (1 << 4);   
00229             TIM3->CNT = 0x000;
00230             //state = !state;
00231             //ZTest->write(state);
00232             GPIOC->ODR ^= (1 << 4);
00233             //flag = 1;
00234         }
00235         }
00236     }
00237     
00238 void PositionSensorEncoder::ZeroEncoderCountDown(void){
00239     if (ZSense->read() == 0){
00240         if (ZSense->read() == 0){
00241             GPIOC->ODR ^= (1 << 4);
00242             flag = 0;
00243             float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
00244             if(dir != dir){
00245                 dir = dir;
00246                 rotations +=  dir;
00247                 }
00248 
00249             GPIOC->ODR ^= (1 << 4);
00250 
00251         }
00252         }
00253     }