Hobbyking_Cheetah_F334

Users » benkatz » Code » Hobbyking_Cheetah_F334 » Documentation
Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_F334
Dependencies: CANnucleo FastPWM3 mbed
Fork of Hobbyking_Cheetah_Compact by Ben Katz
Embed: (wiki syntax)
Show/hide line numbers PositionSensor.cpp Source File
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     
00020 void PositionSensorMA700::Sample(){
00021     
00022     
00023     }
00024 
00025 int PositionSensorMA700::GetRawPosition(){
00026         cs->write(0);
00027     int response = spi->write(0)>>4;
00028     cs->write(1);
00029     return response;
00030     }
00031     
00032 
00033 float PositionSensorMA700::GetMechPosition(){
00034     cs->write(0);
00035     int response = spi->write(0)>>4;
00036     cs->write(1);
00037     if(response - old_counts > _CPR/4){
00038         rotations -= 1;
00039         }
00040     else if (response - old_counts < -_CPR/4){
00041         rotations += 1;
00042         }
00043     old_counts = response;
00044     MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
00045     //return MechPosition - MechOffset;
00046     return MechPosition;
00047     }
00048 
00049 float PositionSensorMA700::GetElecPosition(){
00050     cs->write(0);
00051     int response = spi->write(0)>>4;
00052     cs->write(1);
00053     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*response)%_CPR)) - _offset;
00054     if(elec < 0) elec += 6.28318530718f;
00055     return elec;
00056     }
00057 
00058 float PositionSensorMA700::GetMechVelocity(){
00059     return 0;
00060     }
00061 
00062 void PositionSensorMA700::ZeroPosition(){
00063     rotations = 0;
00064     MechOffset = GetMechPosition();
00065     }
00066     
00067 PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
00068     //_CPR = CPR;
00069     _CPR = CPR;
00070     _ppairs = ppairs;
00071     ElecOffset = offset;
00072     rotations = 0;
00073     spi = new SPI(PC_12, PC_11, PC_10);
00074     spi->format(16, 1);
00075     spi->frequency(5000000);
00076     cs = new DigitalOut(PA_15);
00077     cs->write(1);
00078     readAngleCmd = 0xffff;
00079     MechOffset = 0;
00080     modPosition = 0;
00081     oldModPosition = 0;
00082     oldVel = 0;
00083     }
00084     
00085 void PositionSensorAM5147::Sample(){
00086     cs->write(0);
00087     int angle = spi->write(readAngleCmd);
00088     angle &= 0x3FFF;    //Extract last 14 bits
00089     cs->write(1);
00090     raw = angle;
00091     if(angle - old_counts > _CPR/4){
00092         rotations -= 1;
00093         }
00094     else if (angle - old_counts < -_CPR/4){
00095         rotations += 1;
00096         }
00097     
00098     old_counts = angle;
00099     oldModPosition = modPosition;
00100     modPosition = ((6.28318530718f * ((float) angle))/ (float)_CPR);
00101     position = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
00102     MechPosition = position - MechOffset;
00103     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - ElecOffset;
00104     if(elec < 0) elec += 6.28318530718f;
00105     ElecPosition = elec;
00106     
00107     float vel;
00108     if(modPosition<.1f && oldModPosition>6.1f){
00109         vel = (modPosition - oldModPosition + 6.28318530718f)*40000.0f;
00110         }
00111     else if(modPosition>6.1f && oldModPosition<0.1f){
00112         vel = (modPosition - oldModPosition - 6.28318530718f)*40000.0f;
00113         }
00114     else{
00115         vel = (modPosition-oldModPosition)*40000.0f;
00116     }    
00117     
00118     int n = 16;
00119     float sum = vel;
00120     for (int i = 1; i < (n); i++){
00121         velVec[n - i] = velVec[n-i-1];
00122         sum += velVec[n-i];
00123         }
00124     velVec[0] = vel;
00125     MechVelocity =  sum/(float)n;
00126     ElecVelocity = MechVelocity*_ppairs;
00127     }
00128 
00129 int PositionSensorAM5147::GetRawPosition(){
00130     return raw;
00131     }
00132     
00133 float PositionSensorAM5147::GetMechPosition(){
00134     return MechPosition;
00135     }
00136 
00137 float PositionSensorAM5147::GetElecPosition(){
00138     return ElecPosition;
00139     }
00140 
00141 float PositionSensorAM5147::GetMechVelocity(){
00142     return MechVelocity;
00143     }
00144 
00145 void PositionSensorAM5147::ZeroPosition(){
00146     rotations = 0;
00147     MechOffset = GetMechPosition();
00148     }
00149     
00150     
00151 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
00152     _ppairs = ppairs;
00153     _CPR = CPR;
00154     _offset = offset;
00155     MechPosition = 0;
00156     out_old = 0;
00157     oldVel = 0;
00158     
00159     
00160     // Enable clock for GPIOA
00161     __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
00162  
00163     GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
00164     GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
00165     GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
00166     GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
00167     GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00168     GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00169    
00170     // configure TIM3 as Encoder input
00171     // Enable clock for TIM3
00172     __TIM3_CLK_ENABLE();
00173  
00174     TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1
00175     TIM3->SMCR  = TIM_ENCODERMODE_TI12;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
00176     TIM3->CCMR1 = 0x1111;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
00177     TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
00178     TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
00179     TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
00180     TIM3->ARR   = CPR-1; // reload at 0xfffffff         < TIM auto-reload register
00181   
00182     TIM3->CNT = 0x000;  //reset the counter before we use it  
00183     
00184     // Extra Timer for velocity measurement
00185     
00186     __TIM2_CLK_ENABLE();
00187     TIM3->CR2 = 0x030;  //MMS = 101
00188     
00189     TIM2->PSC = 0x03;
00190     //TIM2->CR2 |= TIM_CR2_TI1S;
00191     TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
00192     TIM2->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
00193     
00194     //TIM2->CR2 |= TIM_CR2_TI1S;
00195     TIM2->CCER |= TIM_CCER_CC1P;
00196     //TIM2->CCER |= TIM_CCER_CC1NP;
00197     TIM2->CCER |= TIM_CCER_CC1E;
00198     
00199     
00200     TIM2->CR1 = 0x01;       //CEN
00201     
00202     TIM3->CR1   = 0x01;     // CEN
00203     ZPulse = new InterruptIn(PC_4);
00204     ZSense = new DigitalIn(PC_4);
00205     //ZPulse = new InterruptIn(PB_0);
00206     //ZSense = new DigitalIn(PB_0);
00207     ZPulse->enable_irq();
00208     ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
00209     //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
00210     ZPulse->mode(PullDown);
00211     flag = 0;
00212 
00213     
00214     //ZTest = new DigitalOut(PC_2);
00215     //ZTest->write(1);
00216     }
00217     
00218 void PositionSensorEncoder::Sample(){
00219     
00220     }
00221 
00222  
00223 float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
00224     int raw = TIM3->CNT;
00225     float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
00226     return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
00227 }
00228 
00229 float PositionSensorEncoder::GetElecPosition() {        //returns rotor electrical angle in radians.
00230     int raw = TIM3->CNT;
00231     float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
00232     if(elec < 0) elec += 6.28318530718f;
00233     return elec;
00234 }
00235 
00236 
00237     
00238 float PositionSensorEncoder::GetMechVelocity(){
00239 
00240     float out = 0;
00241     float rawPeriod = TIM2->CCR1; //Clock Ticks
00242     int currentTime = TIM2->CNT;
00243     if(currentTime > 2000000){rawPeriod = currentTime;}
00244     float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
00245     float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
00246     if(isinf(meas)){ meas = 1;}
00247     out = meas;
00248     //if(meas == oldVel){
00249      //   out = .9f*out_old;
00250      //   }
00251     
00252  
00253     oldVel = meas;
00254     out_old = out;
00255     int n = 16;
00256     float sum = out;
00257     for (int i = 1; i < (n); i++){
00258         velVec[n - i] = velVec[n-i-1];
00259         sum += velVec[n-i];
00260         }
00261     velVec[0] = out;
00262     return sum/(float)n;
00263     }
00264     
00265 float PositionSensorEncoder::GetElecVelocity(){
00266     return _ppairs*GetMechVelocity();
00267     }
00268     
00269 void PositionSensorEncoder::ZeroEncoderCount(void){
00270     if (ZSense->read() == 1 & flag == 0){
00271         if (ZSense->read() == 1){
00272             GPIOC->ODR ^= (1 << 4);   
00273             TIM3->CNT = 0x000;
00274             //state = !state;
00275             //ZTest->write(state);
00276             GPIOC->ODR ^= (1 << 4);
00277             //flag = 1;
00278         }
00279         }
00280     }
00281     
00282 void PositionSensorEncoder::ZeroEncoderCountDown(void){
00283     if (ZSense->read() == 0){
00284         if (ZSense->read() == 0){
00285             GPIOC->ODR ^= (1 << 4);
00286             flag = 0;
00287             float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
00288             if(dir != dir){
00289                 dir = dir;
00290                 rotations +=  dir;
00291                 }
00292 
00293             GPIOC->ODR ^= (1 << 4);
00294 
00295         }
00296         }
00297     }
Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	02 Nov 2017
Imports:	21
Forks:	0
Commits:	24
Dependents:	0
Dependencies:	3
Followers:	4
This repository is Public (Unlisted).
Important changes to repositories hosted on mbed.com

PositionSensor.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

PositionSensor.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
