Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: MA700Sensor/MA700Sensor.cpp
- Revision:
- 48:1b51771c3647
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MA700Sensor/MA700Sensor.cpp Fri Feb 07 11:31:37 2020 +0000
@@ -0,0 +1,342 @@
+
+#include "mbed.h"
+#include "MA700Sensor.h"
+#include "math_ops.h"
+#include "CAN_com.h"
+//#include "offset_lut.h"
+//#include <math.h>
+
+PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
+ //_CPR = CPR;
+ _CPR = CPR;
+ _ppairs = ppairs;
+ JointOffset = offset;
+ rotations = 0;
+ spi = new SPI(PA_7, PA_6, PA_5);
+ spi->format(16, 1);
+ // spi->format(16, 3); //shaorui modify // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+ spi->frequency(25000000);
+
+ cs = new DigitalOut(PA_4);
+ cs->write(1);
+ }
+
+void PositionSensorMA700::Sample(float dt){
+ GPIOA->ODR &= ~(1 << 4);
+ //raw = spi->write(readAngleCmd);//shaorui modify
+ //raw &= 0x3FFF;
+ raw = spi->write(0);
+ raw = raw>>1; //Extract last 14 bits
+ //raw = raw>>2; //Extract last 14 bits
+ GPIOA->ODR |= (1 << 4);
+ int off_1 = offset_lut[raw>>7];
+ int off_2 = offset_lut[((raw>>7)+1)%128];
+ int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7); // Interpolate between lookup table entries
+ int angle = raw + off_interp;
+ //angle = 0.9*old_counts+0.1*angle; // Correct for nonlinearity with lookup table from calibration
+ if(angle - old_counts > _CPR/2){
+ rotations -= 1;
+ // printf("old%d new%d\n\r",old_counts,angle);
+ }
+ else if (angle - old_counts < -_CPR/2){
+ rotations += 1;
+ // printf("old%d new%d\n\r",old_counts,angle);
+ }
+
+ old_counts = angle;
+ oldModPosition = modPosition;
+ modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+ position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+ MechPosition = position - MechOffset;
+ 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){
+
+ if((modPosition-oldModPosition) < -3.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)/dt;
+ }
+ else{
+ vel = (modPosition-oldModPosition)/dt;
+ }
+
+ int n = 40;
+ 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;
+ ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+ }
+
+int PositionSensorMA700::GetRawPosition(){
+ return raw;
+ }
+
+float PositionSensorMA700::GetMechPositionFixed(){
+ return MechPosition+MechOffset;
+ }
+
+float PositionSensorMA700::GetMechPosition(){
+ return MechPosition;
+ }
+
+float PositionSensorMA700::GetElecPosition(){
+ return ElecPosition;
+ }
+
+float PositionSensorMA700::GetElecVelocity(){
+ return ElecVelocity;
+ }
+
+float PositionSensorMA700::GetMechVelocity(){
+ return MechVelocity;
+ }
+
+void PositionSensorMA700::ZeroPosition(){
+ rotations = 0;
+ MechOffset = 0;
+ Sample(.00025f);
+ MechOffset = GetMechPosition();
+ }
+
+void PositionSensorMA700:: SetElecOffset(float offset){
+ ElecOffset = offset;
+ }
+void PositionSensorMA700::SetMechOffset(float offset){
+ MechOffset = offset;
+ }
+
+void PositionSensorMA700::WriteRegister( ControllerStruct * controller){
+ BCT=0x2300|(controller->sidebct&0xff);
+ BCTREAD=0x1300;
+ int judge=(controller->sidebct&0xf00)>>8;
+ ETXY=0x2500|(judge<<4);
+ ETXYREAD=0x1500;
+ int ez;
+ // readAngleCmd = 0x1400; //shaorui modify
+ GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+ spi->write( BCT); //shaorui ADD
+ GPIOA->ODR |= (1 << 4); //shaorui ADD
+ GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+ _test=spi->write( BCTREAD); //shaorui ADD
+ GPIOA->ODR |= (1 << 4); //shaorui ADD
+ GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+ _test1=ez=spi->write( ETXYREAD); //shaorui ADD
+ GPIOA->ODR |= (1 << 4); //shaorui ADD
+ ez&=0x000F;
+ GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+ spi->write( ETXY|ez); //shaorui ADD //Extract last 14 bits
+ GPIOA->ODR |= (1 << 4); //shaorui ADD
+
+ GPIOA->ODR &= ~(1 << 4); //shaorui ADD
+ _test2=spi->write( ETXYREAD); //shaorui ADD //Extract last 14 bits
+ GPIOA->ODR |= (1 << 4); //shaorui ADD
+
+
+
+
+ }
+
+int PositionSensorMA700::GetCPR(){
+ return _CPR;
+ }
+
+void PositionSensorMA700::WriteLUT(int new_lut[128]){
+ memcpy(offset_lut, new_lut, sizeof(offset_lut));
+ }
+
+void PositionSensorMA700::ReadLUT(void){
+ for(int z=0;z<128;z++)
+ {
+ printf("lut data:%d\n\r",offset_lut[z]);}
+ }
+
+int PositionSensorMA700::Gettest() {
+ return _test;}
+int PositionSensorMA700::Gettest1() {
+ return _test1;}
+int PositionSensorMA700::Gettest2() {
+ return _test2;}
+
+
+/*
+
+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
+
+ GPIOA->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ; //PA6 & PA7 as Alternate Function !< GPIO port mode register, Address offset: 0x00
+ GPIOA->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ; //PA6 & PA7 as Inputs !< GPIO port output type register, Address offset: 0x04
+ GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ; //Low speed !< GPIO port output speed register, Address offset: 0x08
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ; //Pull Down !< GPIO port pull-up/pull-down register, Address offset: 0x0C
+ GPIOA->AFR[0] |= 0x22000000 ; //AF02 for PA6 & PA7 !< GPIO alternate function registers, Address offset: 0x20-0x24
+ GPIOA->AFR[1] |= 0x00000000 ; //nibbles here refer to gpio8..15 !< GPIO alternate function registers, Address offset: 0x20-0x24
+
+ // configure TIM3 as Encoder input
+ // 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 = 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
+
+ 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->CR2 |= TIM_CR2_TI1S;
+ TIM2->CCER |= TIM_CCER_CC1P;
+ //TIM2->CCER |= TIM_CCER_CC1NP;
+ TIM2->CCER |= TIM_CCER_CC1E;
+
+
+ TIM2->CR1 = 0x01; //CEN, enable timer
+
+ TIM3->CR1 = 0x01; // CEN
+ ZPulse = new InterruptIn(PC_4);
+ ZSense = new DigitalIn(PC_4);
+ //ZPulse = new InterruptIn(PB_0);
+ //ZSense = new DigitalIn(PB_0);
+ ZPulse->enable_irq();
+ ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
+ //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
+ ZPulse->mode(PullDown);
+ flag = 0;
+
+
+ //ZTest = new DigitalOut(PC_2);
+ //ZTest->write(1);
+ }
+
+void PositionSensorEncoder::Sample(float dt){
+
+ }
+
+
+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.
+ int raw = TIM3->CNT;
+ float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
+ if(elec < 0) elec += 6.28318530718f;
+ return elec;
+}
+
+
+
+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*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
+ if(isinf(meas)){ meas = 1;}
+ out = meas;
+ //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];
+ }
+ velVec[0] = out;
+ return sum/(float)n;
+ }
+
+float PositionSensorEncoder::GetElecVelocity(){
+ return _ppairs*GetMechVelocity();
+ }
+
+void PositionSensorEncoder::ZeroEncoderCount(void){
+ if (ZSense->read() == 1 & flag == 0){
+ if (ZSense->read() == 1){
+ GPIOC->ODR ^= (1 << 4);
+ TIM3->CNT = 0x000;
+ //state = !state;
+ //ZTest->write(state);
+ GPIOC->ODR ^= (1 << 4);
+ //flag = 1;
+ }
+ }
+ }
+
+void PositionSensorEncoder::ZeroPosition(void){
+
+ }
+
+void PositionSensorEncoder::ZeroEncoderCountDown(void){
+ if (ZSense->read() == 0){
+ if (ZSense->read() == 0){
+ GPIOC->ODR ^= (1 << 4);
+ flag = 0;
+ float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
+ if(dir != dir){
+ dir = dir;
+ rotations += dir;
+ }
+
+ GPIOC->ODR ^= (1 << 4);
+
+ }
+ }
+ }
+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));
+ }
+*/
\ No newline at end of file