Revision 7:dc5f27756e02, committed 2016-03-29

Comitter:

benkatz

Date:

Tue Mar 29 01:05:46 2016 +0000

Parent:

6:4ee1cdc43aa8

Child:

8:10ae7bc88d6e

Commit message:

propper svm;

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Sat Mar 12 19:55:52 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Tue Mar 29 01:05:46 2016 +0000
@@ -9,9 +9,9 @@
 
 CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, float Kp, float Ki){
     _Inverter = inverter;
-    PWM = new SVPWM(inverter, 2.0);
+    PWM = new SPWM(inverter, 2.0);
     _PositionSensor = position_sensor;
-    IQ_Ref = .5;
+    IQ_Ref = 2;
     ID_Ref = 0;
     V_Q = 0;
     V_D = 0;
@@ -79,6 +79,7 @@
          
         V_Q = Q_Integral + _Kp*Q_Error;
         V_D = D_Integral + _Kp*D_Error;
+        //V_D = 0;
         
         
     }
@@ -93,10 +94,13 @@
     
 void CurrentRegulator::Commutate(){
     //count += 1;
+    GPIOC->ODR = (1 << 4); //Toggle pin for debugging
     theta_elec = _PositionSensor->GetElecPosition();
     SampleCurrent(); //Grab most recent current sample
     Update();   //Run control loop
     SetVoltage();   //Set inverter duty cycles
+    GPIOC->ODR = (0 << 4); //Toggle pin for debugging
+
     /*
     if (count==500){
         //printf("%d %d %d %d\n\r", (int) (I_Q*1000), (int) (I_D*1000), (int) (I_A*1000), int (I_B*1000));

--- a/Inverter/Inverter.cpp	Sat Mar 12 19:55:52 2016 +0000
+++ b/Inverter/Inverter.cpp	Tue Mar 29 01:05:46 2016 +0000
@@ -99,13 +99,13 @@
 
 void Inverter::SampleCurrent(void){
  //   Dbg->write(1);
-    //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
+    GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
     I_B = _I_Scale*((float) (ADC1->DR) -  I_B_Offset);
     I_C = _I_Scale*((float) (ADC2->DR)-  I_C_Offset);
     I_A = -I_B - I_C;
     //DAC->DHR12R1 = ADC2->DR; 
     //DAC->DHR12R1 = TIM3->CNT>>2;//ADC2->DR; // pass ADC -> DAC, also clears EOC flag
     ADC1->CR2  |= 0x40000000; 
-    //GPIOC->ODR ^= (1 << 4); //toggle pin for debugging
+    GPIOC->ODR ^= (1 << 4); //toggle pin for debugging
     }
     

--- a/PositionSensor/PositionSensor.cpp	Sat Mar 12 19:55:52 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Tue Mar 29 01:05:46 2016 +0000
@@ -29,9 +29,9 @@
     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   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
+    TIM3->ARR   = 0xfffffff; // reload at 0xfffffff         < TIM auto-reload register
   
-    TIM3->CNT = 0x8000;  //reset the counter before we use it  
+    TIM3->CNT = 0x000;  //reset the counter before we use it  
     
     // Extra Timer for velocity measurement
     /*
@@ -57,7 +57,9 @@
     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);
@@ -68,29 +70,15 @@
  
 float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
     int raw = TIM3->CNT-0x8000;
-    if (raw < 0) raw += _CPR;
-    if (raw >= _CPR) raw -= _CPR;
-    float signed_mech = fmod(((6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f);
-    if (signed_mech < 0){
-        return signed_mech + 6.28318530718f;
-        }
-    else{
-        return signed_mech;
-        }
+    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-0x8000;
-    if (raw < 0) raw += _CPR;
-    if (raw >= _CPR) raw -= _CPR;
-    float signed_elec = fmod((7.0f*(6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f); //7 pole pairs
-    //float signed_elec = (7*(6.28318530718*(TIM3->CNT-0x8000)/(float)_CPR + _offset));
-    if (signed_elec < 0){
-        return signed_elec + 6.28318530718f;
-        }
-    else{
-        return signed_elec;
-        }
+
+    int raw = TIM3->CNT;
+    float unsigned_elec = (6.28318530718f/(float)_CPR) * (float) ((7*raw)%_CPR);
+    return unsigned_elec;
 }
 
 float PositionSensorEncoder::GetElecVelocity(){
@@ -102,16 +90,42 @@
 float PositionSensorEncoder::GetMechVelocity(){
     float rawPeriod = TIM2->CCR1; //Clock Ticks
     float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
-    return dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
-    
+    return dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
     }
 
 void PositionSensorEncoder::ZeroEncoderCount(void){
-    if (ZSense->read() == 1){
+    if (ZSense->read() == 1 & flag == 0){
         if (ZSense->read() == 1){
-            TIM3->CNT=0x8000;
+            GPIOC->ODR ^= (1 << 4);
+            dir = -2*((int)(((TIM3->CR1)-0x000)>>4)&1)+1;
+            int old_count = _CPR*rotations + TIM3->CNT;
+            if( abs(_CPR*(rotations+dir) - old_count) <= _CPR>>2){
+                rotations += dir;
+                }
+            
+            TIM3->CNT = 0x000;
+
             //state = !state;
             //ZTest->write(state);
+            GPIOC->ODR ^= (1 << 4);
+            //flag = 1;
+        }
+        }
+    }
+    
+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);
+
         }
         }
     }
\ No newline at end of file

--- a/PositionSensor/PositionSensor.h	Sat Mar 12 19:55:52 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Tue Mar 29 01:05:46 2016 +0000
@@ -20,9 +20,10 @@
     DigitalIn *ZSense;
     //DigitalOut *ZTest;
     virtual void ZeroEncoderCount(void);
-    int _CPR;
+    virtual void ZeroEncoderCountDown(void);
+    int _CPR, flag, rotations;
     //int state;
-    float _offset, MechPosition;
+    float _offset, MechPosition, dir, test_pos;
 };
 
 #endif
\ No newline at end of file

--- a/Transforms/Transforms.cpp	Sat Mar 12 19:55:52 2016 +0000
+++ b/Transforms/Transforms.cpp	Tue Mar 29 01:05:46 2016 +0000
@@ -5,8 +5,10 @@
 using namespace FastMath;
 
 void Transforms::Park(float alpha, float beta, float theta, float *d, float *q){
-    float cosine = cos(theta);
-    float sine = sin(theta);
+    //float cosine = cos(theta);
+    //float sine = sin(theta);
+    float cosine = FastCos(theta);
+    float sine = FastSin(theta);
     *d = alpha*cosine - beta*sine;
     *q = -beta*cosine - alpha*sine;
     //*d = alpha*cosine + beta*sine;
@@ -16,8 +18,10 @@
     }
 
 void Transforms::InvPark(float d, float q, float theta, float *alpha, float *beta){
-    float cosine = cos(theta);
-    float sine = sin(theta);
+    //float cosine = cos(theta);
+    //float sine = sin(theta);
+    float cosine = FastCos(theta);
+    float sine = FastSin(theta);
     *alpha = d*cosine - q*sine;
     *beta = q*cosine + d*sine;
     }

--- a/main.cpp	Sat Mar 12 19:55:52 2016 +0000
+++ b/main.cpp	Tue Mar 29 01:05:46 2016 +0000
@@ -5,15 +5,23 @@
 #include "FastMath.h"
 #include "Transforms.h"
 #include "CurrentRegulator.h"
-
+//#include "TorqueController.h"
+//#include "ImpedanceController.h"
 PositionSensorEncoder encoder(8192,0);
 //Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
-Inverter inverter(PA_8, PA_9, PA_10, PB_7, 0.02014160156, 0.00005);
+Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.01007080078, 0.00005);
 CurrentRegulator foc(&inverter, &encoder, .005, .5);
+SVPWM  svpwm(&inverter, 2.0f);
+Ticker  testing;
+//Timer t;
 
-Ticker  testing;
-
-
+float v_d = 0;
+float v_q = .1;
+float v_alpha = 0;
+float v_beta = 0;
+float v_a = 0;
+float v_b = 0;
+float v_c = 0;
 
 //SPI spi(PB_15, PB_14, PB_13);
 //GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
@@ -38,37 +46,57 @@
   // toggle on update event
   if (TIM1->SR & TIM_SR_UIF ) {
       inverter.SampleCurrent();
+      //wait(.00002);
+      //foc.Commutate();
       }
   TIM1->SR = 0x0; // reset the status register
     //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
 }
 
-    
+void voltage_foc(void){
+    float theta = encoder.GetElecPosition();
+    InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
+    InvClarke(v_alpha, v_beta, &v_a, &v_b, &v_c);
+    svpwm.Update_DTC(v_a, v_b, v_c);
+    //output.write(theta/6.28318530718f);
+    }
+   
 void Loop(void){
     foc.Commutate();
+    //voltage_foc();
     }
 
 void PrintStuff(void){
     float velocity = encoder.GetMechVelocity();
     float position = encoder.GetMechPosition();
-    printf("%f, %f;\n\r", position, velocity);
+    //printf("%f, %f;\n\r", position, velocity);
+    printf("%f   %d   %d\n\r", position, TIM3->CNT-0x8000, -2*((int)((TIM3->CR1)>>4)&1)+1);
     }
-/*
-void voltage_foc(void){
-    theta = encoder.GetElecPosition() + offset;
-    InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
-    spwm.Update_DTC(v_alpha, v_beta);
-    //output.write(theta/6.28318530718f);
-    }
- */
+    
+
 
+ 
+ /*
+ void gen_sine(void){
+     float f = 1.0f;
+     float time = t.read();
+     float a = .45f*sin(6.28318530718f*f*time) + .5f;
+     float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
+     float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
+     inverter.SetDTC(a, b, c);
+     }
+*/
        
 int main() {
+    //t.start();
     wait(1);
     testing.attach(&Loop, .0001);
-    //testing.attach(&PrintStuff, .05);
+    NVIC_SetPriority(TIM5_IRQn, 1);
+    //testing.attach(&gen_sine, .01);
+    //testing.attach(&PrintStuff, .1);
     //inverter.SetDTC(.1, 0, 0);
-    //inverter.EnableInverter();
+    inverter.EnableInverter();
+    //foc.Commutate();
     while(1) {
         //printf("%f\n\r", encoder.GetElecPosition());
         //wait(.1);