1
Dependencies: mbed-dev-f303 FastPWM3
Revision 56:4f73fa55baa0, committed 2020-05-14
- Comitter:
- shaorui
- Date:
- Thu May 14 10:44:41 2020 +0000
- Parent:
- 55:14ac00a5f43d
- Commit message:
- 1
Changed in this revision
diff -r 14ac00a5f43d -r 4f73fa55baa0 Config/hw_config.h --- a/Config/hw_config.h Tue Nov 19 08:54:35 2019 +0000 +++ b/Config/hw_config.h Thu May 14 10:44:41 2020 +0000 @@ -1,11 +1,11 @@ #ifndef HW_CONFIG_H #define HW_CONFIG_H - -#define PIN_U PA_10 +//Be used in hw_setup.cpp +#define PIN_U PA_8 #define PIN_V PA_9 -#define PIN_W PA_8 -#define ENABLE_PIN PA_11 // Enable gate drive pin -#define LED PC_5 // LED Pin +#define PIN_W PA_10 +#define ENABLE_PIN PC_9 // Enable gate drive pin +#define LED PC_7 // LED Pin #define I_SCALE 0.02014160156f // Amps per A/D Count #define V_SCALE 0.012890625f // Bus volts per A/D Count #define DTC_MAX 0.94f // Max phase duty cycle
diff -r 14ac00a5f43d -r 4f73fa55baa0 PositionSensor/PositionSensor.cpp --- a/PositionSensor/PositionSensor.cpp Tue Nov 19 08:54:35 2019 +0000 +++ b/PositionSensor/PositionSensor.cpp Thu May 14 10:44:41 2020 +0000 @@ -11,11 +11,11 @@ _ppairs = ppairs; ElecOffset = offset; rotations = 0; - spi = new SPI(PC_12, PC_11, PC_10); + spi = new SPI(PB_5, PB_4, PB_3); //MOSI MISO SCK (Electrical Board spi1) spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words - spi->frequency(25000000); + spi->frequency(25000000); - cs = new DigitalOut(PA_15); + cs = new DigitalOut(PD_2); //spi1 CS cs->write(1); readAngleCmd = 0xffff; MechOffset = offset; @@ -26,12 +26,12 @@ } void PositionSensorAM5147::Sample(float dt){ - GPIOA->ODR &= ~(1 << 15); + GPIOD->ODR &= ~(1 << 2); raw = spi->write(readAngleCmd); raw &= 0x3FFF; //raw = spi->write(0); //raw = raw>>2; //Extract last 14 bits - GPIOA->ODR |= (1 << 15); + GPIOD->ODR |= (1 << 2); 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
diff -r 14ac00a5f43d -r 4f73fa55baa0 Position_Torque_Sensor/Position_Torque_Sensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Position_Torque_Sensor/Position_Torque_Sensor.cpp Thu May 14 10:44:41 2020 +0000 @@ -0,0 +1,300 @@ + +#include "mbed.h" +#include "Position_Torque_Sensor.h" +#include "../math_ops.h" +//#include "offset_lut.h" +//#include <math.h> + +SensorAM5147::SensorAM5147(int CPR, float offset, int ppairs){ + //_CPR = CPR; + _CPR = CPR; + _ppairs = ppairs; + ElecOffset = offset; + rotations = 0; + spi = new SPI(PB_15, PB_14, PB_13); //MOSI MISO SCK (Electrical Board spi2) + spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words + spi->frequency(25000000); + //spi->frequency(10000000); + + cs2 = new DigitalOut(PB_12); //spi2 CS2 + cs1 = new DigitalOut(PB_10); //spi2 CS1 + cs1->write(1); + readAngleCmd = 0xffff; + MechOffset = offset; + modPosition = 0; + oldModPosition = 0; + oldVel = 0; + raw = 0; + } + +void SensorAM5147::Sample(float dt){ + GPIOB->ODR &= ~(1 << 10); + + raw = spi->write(readAngleCmd); + raw &= 0x3FFF; + //raw = spi->write(0); + //raw = raw>>1; //Extract last 14 bits + GPIOB->ODR |= (1 << 10); + 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; // Correct for nonlinearity with lookup table from calibration + if(angle - old_counts > _CPR/2){ + rotations -= 1; + } + else if (angle - old_counts < -_CPR/2){ + rotations += 1; + } + + 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 SensorAM5147::GetRawPosition(){ + return raw; + } + +float SensorAM5147::GetMechPositionFixed(){ + return MechPosition+MechOffset; + } + +float SensorAM5147::GetMechPosition(){ + return MechPosition; + } + +float SensorAM5147::GetElecPosition(){ + return ElecPosition; + } + +float SensorAM5147::GetElecVelocity(){ + return ElecVelocity; + } + +float SensorAM5147::GetMechVelocity(){ + return MechVelocity; + } + +void SensorAM5147::ZeroPosition(){ + rotations = 0; + MechOffset = 0; + Sample(.00025f); + MechOffset = GetMechPosition(); + } + +void SensorAM5147::SetElecOffset(float offset){ + ElecOffset = offset; + } +void SensorAM5147::SetMechOffset(float offset){ + MechOffset = offset; + } + +int SensorAM5147::GetCPR(){ + return _CPR; + } + + +void SensorAM5147::WriteLUT(int new_lut[128]){ + memcpy(offset_lut, new_lut, sizeof(offset_lut)); + } + + + +SensorEncoder::SensorEncoder(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, &SensorEncoder::ZeroEncoderCount); + //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown); + ZPulse->mode(PullDown); + flag = 0; + + + //ZTest = new DigitalOut(PC_2); + //ZTest->write(1); + } + +void SensorEncoder::Sample(float dt){ + + } + + +float SensorEncoder::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 SensorEncoder::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 SensorEncoder::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 SensorEncoder::GetElecVelocity(){ + return _ppairs*GetMechVelocity(); + } + +void SensorEncoder::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 SensorEncoder::ZeroPosition(void){ + + } + +void SensorEncoder::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 SensorEncoder::SetElecOffset(float offset){ + + } + +int SensorEncoder::GetRawPosition(void){ + return 0; + } + +int SensorEncoder::GetCPR(){ + return _CPR; + } + + +void SensorEncoder::WriteLUT(int new_lut[128]){ + memcpy(offset_lut, new_lut, sizeof(offset_lut)); + }
diff -r 14ac00a5f43d -r 4f73fa55baa0 Position_Torque_Sensor/Position_Torque_Sensor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Position_Torque_Sensor/Position_Torque_Sensor.h Thu May 14 10:44:41 2020 +0000 @@ -0,0 +1,69 @@ +#ifndef POSITION_TORQUE_SENSOR_H +#define POSITION_TORQUE_SENSOR_H +class PositionSensor1 { +public: + virtual void Sample(float dt) = 0; + virtual float GetMechPosition() {return 0.0f;} + virtual float GetMechPositionFixed() {return 0.0f;} + virtual float GetElecPosition() {return 0.0f;} + virtual float GetMechVelocity() {return 0.0f;} + virtual float GetElecVelocity() {return 0.0f;} + virtual void ZeroPosition(void) = 0; + virtual int GetRawPosition(void) = 0; + virtual void SetElecOffset(float offset) = 0; + virtual int GetCPR(void) = 0; + virtual void WriteLUT(int new_lut[128]) = 0; +}; + + +class SensorEncoder: public PositionSensor1 { +public: + SensorEncoder(int CPR, float offset, int ppairs); + virtual void Sample(float dt); + virtual float GetMechPosition(); + virtual float GetElecPosition(); + virtual float GetMechVelocity(); + virtual float GetElecVelocity(); + virtual void ZeroPosition(void); + virtual void SetElecOffset(float offset); + virtual int GetRawPosition(void); + virtual int GetCPR(void); + virtual void WriteLUT(int new_lut[128]); +private: + InterruptIn *ZPulse; + DigitalIn *ZSense; + //DigitalOut *ZTest; + virtual void ZeroEncoderCount(void); + virtual void ZeroEncoderCountDown(void); + int _CPR, flag, rotations, _ppairs, raw; + //int state; + float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40]; + int offset_lut[128]; +}; + +class SensorAM5147: public PositionSensor1{ +public: + SensorAM5147(int CPR, float offset, int ppairs); + virtual void Sample(float dt); + virtual float GetMechPosition(); + virtual float GetMechPositionFixed(); + virtual float GetElecPosition(); + virtual float GetMechVelocity(); + virtual float GetElecVelocity(); + virtual int GetRawPosition(); + virtual void ZeroPosition(); + virtual void SetElecOffset(float offset); + virtual void SetMechOffset(float offset); + virtual int GetCPR(void); + virtual void WriteLUT(int new_lut[128]); +private: + float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt; + int raw, _CPR, rotations, old_counts, _ppairs; + SPI *spi; + DigitalOut *cs1,*cs2; + int readAngleCmd; + int offset_lut[128]; + +}; + +#endif \ No newline at end of file
diff -r 14ac00a5f43d -r 4f73fa55baa0 hw_setup.cpp --- a/hw_setup.cpp Tue Nov 19 08:54:35 2019 +0000 +++ b/hw_setup.cpp Thu May 14 10:44:41 2020 +0000 @@ -50,9 +50,9 @@ ADC->CCR = 0x00000016; // Regular simultaneous mode only ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON - ADC1->SQR3 = 0x000000A; // use PC_0 as input- ADC1_IN0 + ADC1->SQR3 = 0x000000D; // use PC_3 as input- ADC1_IN0 ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC2 ON - ADC2->SQR3 = 0x0000000B; // use PC_1 as input - ADC2_IN11 + ADC2->SQR3 = 0x0000000C; // use PC_2 as input - ADC2_IN11 ADC3->CR2 |= ADC_CR2_ADON; // ADC3 ON ADC3->SQR3 = 0x00000000; // use PA_0, - ADC3_IN0 GPIOC->MODER |= 0x0000000f; // Alternate function, PC_0, PC_1 are analog inputs
diff -r 14ac00a5f43d -r 4f73fa55baa0 main.cpp --- a/main.cpp Tue Nov 19 08:54:35 2019 +0000 +++ b/main.cpp Thu May 14 10:44:41 2020 +0000 @@ -32,11 +32,13 @@ #include "PreferenceWriter.h" #include "CAN_com.h" #include "DRV.h" +#include "Position_Torque_Sensor.h" PreferenceWriter prefs(6); GPIOStruct gpio; ControllerStruct controller; +ControllerStruct waijie_controller; ObserverStruct observer; COMStruct com; Serial pc(PA_2, PA_3); @@ -47,18 +49,19 @@ CANMessage txMsg; -SPI drv_spi(PA_7, PA_6, PA_5); -DigitalOut drv_cs(PA_4); -//DigitalOut drv_en_gate(PA_11); +SPI drv_spi(PC_12, PC_11, PC_10); //MOSI MISO CLK (Electrical Board spi3) +DigitalOut drv_cs(PA_15); +//DigitalOut drv_en_gate(PC_9); DRV832x drv(&drv_spi, &drv_cs); PositionSensorAM5147 spi(16384, 0.0, NPP); +SensorAM5147 spi1(16384,0.0,NPP); volatile int count = 0; volatile int state = REST_MODE; volatile int state_change; volatile int counthjb = 0; - +volatile int testcount = 0; void onMsgReceived() { //msgAvailable = true; counthjb++; @@ -83,6 +86,12 @@ } else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); + //Test for Can Communication + printf("p_des :%.3f\n\r",controller.p_des); + printf("v_des :%.3f\n\r",controller.v_des); + printf("kp :%.3f\n\r",controller.kp); + printf("kd :%.3f\n\r",controller.kd); + printf("t_ff :%.3f\n\r",controller.t_ff); } pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); can.write(txMsg); @@ -177,7 +186,8 @@ //volatile int delay; //for (delay = 0; delay < 55; delay++); - spi.Sample(DT); // sample position sensor + spi.Sample(DT); // sample position sensor + spi1.Sample(DT); // sample position sensor controller.adc2_raw = ADC2->DR; // Read ADC Data Registers controller.adc1_raw = ADC1->DR; controller.adc3_raw = ADC3->DR; @@ -187,7 +197,26 @@ controller.dtheta_elec = spi.GetElecVelocity(); controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement /// + /************Test for AM5147P Position Sensor*********/ + + //controller.adc2_raw = ADC2->DR; // Read ADC Data Registers + //controller.adc1_raw = ADC1->DR; + //controller.adc3_raw = ADC3->DR; + waijie_controller.theta_elec = spi1.GetElecPosition(); + waijie_controller.theta_mech = spi1.GetMechPosition(); + waijie_controller.dtheta_mech =spi1.GetMechVelocity(); + waijie_controller.dtheta_elec = spi1.GetElecVelocity(); + //controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement + testcount++; + if(testcount==40000) + { + printf("waijie:%.3f Raw:%.3f\n\r", waijie_controller.theta_mech,spi1.GetRawPosition()); + printf("neizhi:%.3f Raw:%.3f\n\r", GR*controller.theta_mech,spi.GetRawPosition()); + testcount=0; + } + + /************Test for AM5147P Position Sensor*********/ /// Check state machine state, and run the appropriate function /// switch(state){ case REST_MODE: // Do nothing @@ -431,7 +460,8 @@ spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table init_controller_params(&controller); - pc.baud(460800); // set serial baud rate + //pc.baud(460800); + pc.baud(115200); // set serial baud rate wait(.01); pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); wait(.01); @@ -473,9 +503,14 @@ //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec); //printf("%.3f\n\r", controller.dtheta_mech); + /********Test for AM5147P Position Sensor********/ + // printf("%.3f\n\r", GR*waijie_controller.theta_mech); + /********Test for AM5147P Position Sensor********/ + printf("enter enter enter\n\r"); wait(.002); } - + //pc.printf("%.3f\n\r", GR*waijie_controller.theta_mech); + //wait(.1); } }