Austin Brown
Slurp
Revision 0:9edd6ec0f56a, committed 2017-05-20
- Comitter:
- austinbrown124
- Date:
- Sat May 20 21:42:20 2017 +0000
- Commit message:
- First Commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/FOC/foc.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,161 @@
+
+#include "foc.h"
+
+//#include "FastMath.h"
+//using namespace FastMath;
+
+
+void abc( float theta, float d, float q, float *a, float *b, float *c){
+ ///Phase current amplitude = lengh of dq vector///
+ ///i.e. iq = 1, id = 0, peak phase current of 1///
+
+ *a = d*cosf(theta) + q*sinf(theta);
+ *b = d*cosf((2.0f*PI/3.0f)+theta) + q*sinf((2.0f*PI/3.0f)+theta);
+ *c = d*cosf((-2.0f*PI/3.0f)+theta) + q*sinf((-2.0f*PI/3.0f)+theta);
+ }
+
+void dq0(float theta, float a, float b, float c, float *d, float *q){
+ ///Phase current amplitude = lengh of dq vector///
+ ///i.e. iq = 1, id = 0, peak phase current of 1///
+
+ *d = (2.0f/3.0f)*(a*cosf(theta) + b*cosf((2.0f*PI/3.0f)+theta) + c*cosf((-2.0f*PI/3.0f)+theta));
+ *q = (2.0f/3.0f)*(a*sinf(theta) + b*sinf((2.0f*PI/3.0f)+theta) + c*sinf((-2.0f*PI/3.0f)+theta));
+ }
+
+void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
+ ///u,v,w amplitude = v_bus for full modulation depth///
+
+ float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
+ *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+ *dtc_v = fminf(fmaxf(((v - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+ *dtc_w = fminf(fmaxf(((w - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
+
+ }
+
+void zero_current(int *offset_1, int *offset_2){
+ int adc1_offset = 0;
+ int adc2_offset = 0;
+ int n = 1024;
+ for (int i = 0; i<n; i++){
+ ADC1->CR2 |= 0x40000000;
+ wait(.001);
+ adc2_offset += ADC2->DR;
+ adc1_offset += ADC1->DR;
+ }
+ *offset_1 = adc1_offset/n;
+ *offset_2 = adc2_offset/n;
+ }
+
+void reset_foc(ControllerStruct *controller){
+ controller->q_int = 0;
+ controller->d_int = 0;
+ }
+
+
+void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
+
+ controller->loop_count ++;
+ if(gpio->phasing){
+ controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset); //Calculate phase currents from ADC readings
+ controller->i_a = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
+ }
+ else{
+ controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset); //Calculate phase currents from ADC readings
+ controller->i_a = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
+ }
+ controller->i_c = -controller->i_b - controller->i_a;
+
+
+ dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q); //dq0 transform on currents
+
+ ///Cogging Compensation Lookup///
+ //int ind = theta * (128.0f/(2.0f*PI));
+ //float cogging_current = controller->cogging[ind];
+ //float cogging_current = 1.0f*cos(6*theta);
+ ///Controller///
+ float i_d_error = controller->i_d_ref - controller->i_d;
+ float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
+ //float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE); //feed-forward voltage
+ //float v_q_ff = 2.0f*(2*controller->i_q_ref*R_PHASE + controller->dtheta_elec*WB*0.8165f);
+ controller->d_int += i_d_error;
+ controller->q_int += i_q_error;
+
+ //v_d_ff = 0;
+ //v_q_ff = 0;
+
+ limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));
+ //controller->d_int = fminf(fmaxf(controller->d_int, -D_INT_LIM), D_INT_LIM);
+ //controller->q_int = fminf(fmaxf(controller->q_int, -Q_INT_LIM), Q_INT_LIM);
+
+
+ controller->v_d = K_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;
+ controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff;
+
+ //controller->v_d = v_d_ff;
+ //controller->v_q = v_q_ff;
+
+ limit_norm(&controller->v_d, &controller->v_q, controller->v_bus);
+
+ abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
+ svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
+
+ //gpio->pwm_u->write(1.0f-controller->dtc_u); //write duty cycles
+ //gpio->pwm_v->write(1.0f-controller->dtc_v);
+ //gpio->pwm_w->write(1.0f-controller->dtc_w);
+
+ //bing1 = (controller->dtc_u);
+ //bing2 = (controller->dtc_v);
+ //bing3 = (controller->dtc_w);
+ /*
+ //if(gpio->phasing){
+ TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_u);
+ TIM1->CCR2 = 0x1194*(1.0f-controller->dtc_v);
+ TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_w);
+ }
+ else{
+ TIM1->CCR3 = 0x1194*(1.0f-controller->dtc_u);
+ TIM1->CCR1 = 0x1194*(1.0f-controller->dtc_v);
+ TIM1->CCR2 = 0x1194*(1.0f-controller->dtc_w);
+ }*/
+ //gpio->pwm_u->write(1.0f - .05f); //write duty cycles
+ //gpio->pwm_v->write(1.0f - .05f);
+ //gpio->pwm_w->write(1.0f - .1f);
+ //TIM1->CCR1 = 0x708*(1.0f-controller->dtc_u);
+ //TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
+ //TIM1->CCR3 = 0x708*(1.0f-controller->dtc_w);
+ controller->theta_elec = theta; //For some reason putting this at the front breaks thins
+
+
+ if(controller->loop_count >400){
+ //controller->i_q_ref = -controller->i_q_ref;
+ controller->loop_count = 0;
+
+ //printf("%d %f\n\r", ind, cogging_current);
+ //printf("%f\n\r", controller->theta_elec);
+ //pc.printf("%f %f %f\n\r", controller->i_a, controller->i_b, controller->i_c);
+ //pc.printf("%f %f\n\r", controller->i_d, controller->i_q);
+ //pc.printf("%d %d\n\r", controller->adc1_raw, controller->adc2_raw);
+ }
+ }
+
+/*
+void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
+
+ }
+*/
+
+void voltageabc( float theta, float d, float q, float *a, float *b, float *c){
+ ///Phase current amplitude = lengh of dq vector///
+ ///i.e. iq = 1, id = 0, peak phase current of 1///
+
+ *a = sinf(theta);
+ *b = sinf((2.0f*PI/3.0f)+theta);
+ *c = sinf((-2.0f*PI/3.0f)+theta);
+ }
+
+
+
+
+
+
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FOC/foc.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,19 @@ +#ifndef FOC_H +#define FOC_H + +#include "structs.h" +#include "PositionSensor.h" +#include "mbed.h" +#include "math.h" +#include "math_ops.h" +#include "motor_config.h" +#include "current_controller_config.h" + +void abc(float theta, float d, float q, float *a, float *b, float *c); +void dq0(float theta, float a, float b, float c, float *d, float *q); +void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w); +void zero_current(int *offset_1, int *offset_2); +void reset_foc(ControllerStruct *controller); +void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta); +void voltageabc(float theta, float d, float q, float *a, float *b, float *c); +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FastPWM.lib Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/benkatz/code/FastPWM3/#51c979bca21e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Inverter/Inverter.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,96 @@
+
+#include "mbed.h"
+#include "hw_pins.h"
+//#include "hw_config.h"
+#include "structs.h"
+#include "FastPWM.h"
+
+void Init_PWM(GPIOStruct *gpio){
+
+ printf("\nStarting Hardware Function\n\r");
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOC
+ RCC->APB1ENR |= 0x00000001; // enable TIM2 clock
+ RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
+
+ GPIOC->MODER |= (1 << 10); // set pin 5 to be general purpose output for LED
+
+ //gpio->enable = new DigitalOut(ENABLE_PIN);
+ gpio->pwm_ul = new FastPWM(PIN_AL);
+ gpio->pwm_vl = new FastPWM(PIN_BL);
+ gpio->pwm_wl = new FastPWM(PIN_CL);
+ gpio->pwm_uh = new FastPWM(PIN_AH);
+ gpio->pwm_vh = new FastPWM(PIN_BH);
+ gpio->pwm_wh = new FastPWM(PIN_CH);
+
+ gpio->phasing = 1;
+
+
+ //ISR Setup
+
+ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
+
+ TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
+ TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up
+ TIM1->CR1 |= TIM_CR1_UDIS;
+ TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
+ TIM1->RCR |= 0x001; // update event once per up/down count of tim1
+ TIM1->EGR |= TIM_EGR_UG;
+
+
+ //PWM Setup
+ TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
+ TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
+
+ TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
+ TIM1->RCR |= 0x001; // update event once per up/down count of tim1
+ TIM1->EGR |= TIM_EGR_UG;
+
+
+ //PWM Setup
+ TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
+ TIM1->ARR = 0x1194; // 20 Khz
+ //TIM1->BDTR |= TIM_BDTR_MOE;
+ //TIM1->BDTR |= TIM_BDTR_OSSI;
+ //TIM1->BDTR |= TIM_BDTR_OSSR;
+ TIM1->BDTR |= 0xBF;
+ TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2E | TIM_CCER_CC2NE | TIM_CCER_CC3E | TIM_CCER_CC3NE;
+ //TIM1->CCER |= ~TIM_CCER_CC1NP; //Interupt when low side is on.
+ //TIM1->CCER |= TIM_CCER_CC1NP;
+ TIM1->CR1 |= TIM_CR1_CEN;
+
+
+ }
+
+void Init_ADC(void){
+ // ADC Setup
+ RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
+ RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOC
+
+ ADC->CCR = 0x00000006; // Regular simultaneous mode only
+ ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
+ ADC1->SQR3 = 0x000000A; // use PC_0 as input this is the V phase
+ ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
+ ADC2->SQR3 = 0x0000000B; // use PC_1 as input. This is the U phase
+ GPIOC->MODER |= 0x0000000f; // Alternate function, PC_0, PC_1 are analog inputs
+
+ }
+
+void Init_DAC(void){
+ RCC->APB1ENR |= 0x20000000; // Enable clock for DAC
+ DAC->CR |= 0x00000001; // DAC control reg, both channels ON
+ GPIOA->MODER |= 0x00000300; // PA04 as analog output
+ }
+
+void Init_All_HW(GPIOStruct *gpio){
+ wait_ms(100);
+ Init_ADC();
+ wait(0.1);
+
+ //Init_DAC();
+ wait(0.1);
+
+ Init_PWM(gpio);
+
+ }
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Inverter/Inverter.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,12 @@ +#ifndef HW_PINS_H +#define HW_PINS_H + +#include "mbed.h" +#include "structs.h" + +void Init_PWM(GPIOStruct *gpio); +void Init_ADC(void); +void Init_DAC(void); +void Init_All_HW(GPIOStruct *gpio); + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,272 @@
+
+#include "mbed.h"
+#include "PositionSensor.h"
+//#include "offset_lut.h"
+//#include <math.h>
+/*
+PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
+ //_CPR = CPR;
+ _CPR = CPR;
+ _ppairs = ppairs;
+ 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;
+ 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;
+ else if(elec > 6.28318530718f) 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(){
+ return raw;
+ }
+
+float PositionSensorAM5147::GetMechPosition(){
+ return MechPosition;
+ }
+
+float PositionSensorAM5147::GetElecPosition(){
+ return ElecPosition;
+ }
+
+float PositionSensorAM5147::GetMechVelocity(){
+ return MechVelocity;
+ }
+
+void PositionSensorAM5147::ZeroPosition(){
+ rotations = 0;
+ 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
+
+ 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 = 0x0101; // 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(PA_5);
+ ZSense = new DigitalIn(PA_5);
+ //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 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));
+ }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.h Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,65 @@
+#ifndef POSITIONSENSOR_H
+#define POSITIONSENSOR_H
+class PositionSensor {
+public:
+ virtual void Sample(void) = 0;
+ virtual float GetMechPosition() {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 PositionSensorEncoder: public PositionSensor {
+public:
+ PositionSensorEncoder(int CPR, float offset, int ppairs);
+ virtual void Sample();
+ 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[16];
+ int offset_lut[128];
+};
+/*
+class PositionSensorAM5147: public PositionSensor{
+public:
+ PositionSensorAM5147(int CPR, float offset, int ppairs);
+ virtual void Sample();
+ virtual float GetMechPosition();
+ virtual float GetElecPosition();
+ virtual float GetMechVelocity();
+ virtual int GetRawPosition();
+ virtual void ZeroPosition();
+ virtual void SetElecOffset(float offset);
+ virtual int GetCPR(void);
+ virtual void WriteLUT(int new_lut[128]);
+private:
+ float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[16], MechVelocity, ElecVelocity;
+ int raw, _CPR, rotations, old_counts, _ppairs;
+ SPI *spi;
+ DigitalOut *cs;
+ int readAngleCmd;
+ int offset_lut[128];
+
+};*/
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hw_setup/current_controller_config.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,21 @@ +#ifndef CURRENT_CONTROLLER_CONFIG_H +#define CURRENT_CONTROLLER_CONFIG_H + +#define K_D .05f // Volts/Amp +#define K_Q .05f // Volts/Amp +#define KI_D 0.04f // 1/samples +#define KI_Q 0.04f // 1/samples +#define V_BUS 24.0f // Volts + +#define D_INT_LIM V_BUS/(K_D*KI_D) // Amps*samples +#define Q_INT_LIM V_BUS/(K_Q*KI_Q) // Amps*samples + +#define DTC_MAX 0.95f // Max phase duty cycle +#define DTC_MIN 0.05f // Min phase duty cycle + +#define I_SCALE 0.02014160156f // Amps per A/D Count +// 1/(.002*1*20*4096/3.3) + + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hw_setup/hw_pins.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,13 @@ +#ifndef HW_PINS_H +#define HW_PINS_H + +#define PIN_AL PA_8 +#define PIN_BL PA_9 +#define PIN_CL PA_10 +#define PIN_AH PB_13 //U, the top fet +#define PIN_BH PB_14 +#define PIN_CH PB_15 //W, the bottom fet with no shunt amp + + + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hw_setup/motor_config.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,16 @@ +#ifndef MOTOR_CONFIG_H +#define MOTOR_CONFIG_H + +//#define R_PHASE 0.105f //Ohms +//#define L_D 0.00003f //Henries +//#define L_Q 0.00003f //Henries +//#define KT .07f //N-m per peak phase amp (= RMS amps/1.5) +#define POLE_PAIRS 7 //Number of pole pairs + +//#define WB KT/NPP //Webers. + +#define ENC_TICKS_PER_REV 8192 + + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,241 @@
+#include "mbed.h"
+#include "PositionSensor.h"
+#include "structs.h"
+
+#include "Inverter.h"
+//#define PI 3.14159f
+
+#include "foc.h"
+
+
+
+
+DigitalOut clockpin(PC_12);
+//AnalogIn pot_in(PC_3);
+
+
+// controller modes
+#define REST_MODE 0
+#define OLSINE 1
+#define ONEUPTWODOWN 2
+#define VMODESINE 3
+//#define BEN_CALIBRATION_MODE 1
+#define TORQUE_MODE 4
+//#define PD_MODE 5
+//#define SETUP_MODE 6
+//#define ENCODER_MODE 7
+
+
+GPIOStruct gpio;
+ControllerStruct controller;
+
+
+//float fake_theta = 0.0;
+float theta1 = 0.0f;
+//float theta_offset = 4.708f;
+//float theta_offset = 3.38f;
+//float theta_offset = 4.97f;
+float theta_offset = 4.8f;
+
+int bing1 = 0;
+int bing2 = 0;
+int bing3 = 0;
+
+int var1 = 0;
+int var2 = 0;
+
+float a = 0.0f;
+float b = 0.0f;
+float c = 0.0f;
+
+volatile int count = 0;
+
+int controller_state = ONEUPTWODOWN;//VMODESINE;
+
+Serial pc(PA_2, PA_3);
+
+PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS);
+
+
+// Main 20khz loop Interrupt ///
+/// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers ///
+extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
+ //clockpin = 1;
+
+ if (TIM1->SR & TIM_SR_UIF ) {
+
+
+ ///Sample current always ///
+ ADC1->CR2 |= 0x40000000; //Begin sample and conversion
+ //volatile int delay;
+ //for (delay = 0; delay < 55; delay++);
+ controller.adc2_raw = ADC2->DR;
+ controller.adc1_raw = ADC1->DR;
+
+
+
+ /// Check state machine state, and run the appropriate function ///
+ //printf("%d\n\r", state);
+ switch(controller_state){
+ case REST_MODE: //nothing
+ if(count > 1000){
+ count = 0;
+ printf("Rest Mode");
+ printf("\n\r");
+ }
+ break;
+
+ case OLSINE: // open loop sines
+
+ //theta1+= 0.1f*pot_in;
+ theta1+= 0.001f;
+ if (theta1 > 2*PI) { theta1-=(2*PI); }
+
+ //trigger clock pin sychronous with electrical revolutions
+ if (theta1 > PI) { clockpin = 1; }
+ else {clockpin = 0;}
+
+ a = cosf(theta1)/2 + 0.5f;
+ b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+ c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+
+ TIM1->CCR1 = 0x1194*(a);
+ TIM1->CCR2 = 0x1194*(b);
+ TIM1->CCR3 = 0x1194*(c);
+ break;
+
+ case ONEUPTWODOWN: // one up, two down
+
+ a = 0.8f;
+ b = 0.2f;
+ c = 0.2f;
+
+ count++;
+ if(count > 500){
+ count = 0;
+ //printf("%f ",encoder.GetElecPosition());
+ }
+
+ TIM1->CCR1 = 0x1194*(a);
+ TIM1->CCR2 = 0x1194*(b);
+ TIM1->CCR3 = 0x1194*(c);
+
+ break;
+
+ case VMODESINE: // position mode sines
+ theta1 = encoder.GetElecPosition() + theta_offset;
+
+ if (theta1 > 2*PI) {
+ theta1-=(2*PI);
+ }
+
+ //trigger clock pin sychronous with electrical revolutions
+ if (encoder.GetElecPosition() > PI) { clockpin = 1; }
+ else {clockpin = 0;}
+
+ a = cosf(theta1)/2 + 0.5f;
+ b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+ c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
+
+ TIM1->CCR1 = 0x1194*(a);
+ TIM1->CCR2 = 0x1194*(b);
+ TIM1->CCR3 = 0x1194*(c);
+
+
+ //remove this code later
+ //print Q and D values
+ count++;
+ // Run current loop
+ //spi.Sample(); // Sample position sensor
+ if(count > 1000){
+ count = 0;
+ //printf("%f ",controller.i_q);
+ //printf("%f ",controller.i_d);
+ //printf("\n\r");
+ }
+
+ break;
+
+ case TORQUE_MODE:
+
+ // Run torque control
+ count++;
+ controller.theta_elec = encoder.GetElecPosition() + theta_offset;
+ commutate(&controller, &gpio, controller.theta_elec); // Run current loop
+ //spi.Sample(); // Sample position sensor
+ if(count > 1000){
+ count = 0;
+ //readCAN();
+ //controller.i_q_ref = ((float)(canCmd-1000))/100;
+ controller.i_q_ref = 2;
+ //pc.printf("%f\n\r ", controller.theta_elec);
+
+ //printf("%i ",controller.adc1_raw);
+ //printf("%i \n\r ",controller.adc2_raw);
+ }
+ }
+
+
+ ///*
+
+ controller.theta_elec = encoder.GetElecPosition() + theta_offset;
+ commutate(&controller, &gpio, controller.theta_elec);
+
+
+
+ //*/
+
+ }
+ TIM1->SR = 0x0;
+ //clockpin = 0; // reset the status register
+}
+
+int main() {
+ //float meas;
+ pc.baud(115200);
+ printf("\nStarting Hardware\n");
+ Init_All_HW(&gpio);
+
+ zero_current(&controller.adc1_offset, &controller.adc2_offset); // Setup PWM, ADC, GPIO
+ wait(0.1);
+
+
+ while(1) {
+ //printf("AnalogIn example\n");
+ //printf("%f ",a);
+ //printf("\n");
+ wait(0.1);
+
+ if (0==0) {
+ //printf("%i ", TIM3->CNT);
+
+ printf("%f ",encoder.GetElecPosition());
+ //printf("%f ",encoder.GetMechPosition());
+
+ printf("%f ",theta_offset);
+
+ printf("%i ",controller.adc1_raw);
+ printf("%i ",controller.adc2_raw);
+
+ printf("%i ",controller.adc1_offset);
+ printf("%i ",controller.adc2_offset);
+
+ printf("%f ",controller.i_q);
+ printf("%f ",controller.i_d);
+
+ printf("%f ",controller.i_a);
+ printf("%f ",controller.i_b);
+
+ //printf("%f ",controller.theta_elec);
+
+
+
+ printf("\n\r");
+
+ }
+
+
+
+
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,27 @@
+
+#include "math_ops.h"
+
+
+float fmaxf(float x, float y){
+ return (((x)>(y))?(x):(y));
+ }
+
+float fminf(float x, float y){
+ return (((x)<(y))?(x):(y));
+ }
+
+float fmaxf3(float x, float y, float z){
+ return (x > y ? (x > z ? x : z) : (y > z ? y : z));
+ }
+
+float fminf3(float x, float y, float z){
+ return (x < y ? (x < z ? x : z) : (y < z ? y : z));
+ }
+
+void limit_norm(float *x, float *y, float limit){
+ float norm = sqrt(*x * *x + *y * *y);
+ if(norm > limit){
+ *x = *x * limit/norm;
+ *y = *y * limit/norm;
+ }
+ }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/math_ops/math_ops.h Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,14 @@ +#ifndef MATH_OPS_H +#define MATH_OPS_H + +#define PI 3.14159265359f + +#include "math.h" + +float fmaxf(float x, float y); +float fminf(float x, float y); +float fmaxf3(float x, float y, float z); +float fminf3(float x, float y, float z); +void limit_norm(float *x, float *y, float limit); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/structs.h Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,48 @@
+#ifndef STRUCTS_H
+#define STRUCTS_H
+
+//#include "CANnucleo.h"
+#include "mbed.h"
+#include "FastPWM.h"
+
+
+typedef struct{
+ DigitalOut *enable;
+ FastPWM *pwm_ul, *pwm_vl, *pwm_wl, *pwm_uh, *pwm_vh, *pwm_wh;
+ int phasing;
+ } GPIOStruct;
+
+typedef struct{
+
+ }COMStruct;
+
+typedef struct{
+ int adc1_raw, adc2_raw;
+ float i_a, i_b, i_c;
+ float v_bus;
+ float theta_mech, theta_elec;
+ float dtheta_mech, dtheta_elec;
+ float i_d, i_q;
+ float v_d, v_q;
+ float dtc_u, dtc_v, dtc_w;
+ float v_u, v_v, v_w;
+ float d_int, q_int;
+ int adc1_offset, adc2_offset;
+ float i_d_ref, i_q_ref;
+ int loop_count;
+ int mode;
+ float cogging[128];
+ } ControllerStruct;
+
+typedef struct{
+ float vel_1;
+ float vel_1_old;
+ float vel_1_est;
+ float vel_2;
+ float vel_2_old;
+ float vel_2_est;
+ float ts;
+ float est;
+ } VelocityEstimatorStruct;
+
+#endif