Revision 14:80ce59119d93, committed 2016-10-31

Comitter:

benkatz

Date:

Mon Oct 31 16:48:16 2016 +0000

Parent:

13:a3fa0a31b114

Commit message:

Misc. changes. Finally fixed transforms (turns out B and C current measurements were accidentally swapped)

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -29,6 +29,8 @@
     I_C = 0;
     I_Alpha = 0;
     I_Beta = 0;
+    IQ_Old = 0;
+    ID_Old = 0;
     count = 0;
     _L = L;
     _Kp = Kp;
@@ -38,8 +40,7 @@
     Int_Max = .9;
     DTC_Max = .97;
     //theta_elec = _PositionSensor->GetElecPosition();
-    //pc = new Serial(PA_2, PA_3);
-    //pc->baud(115200);
+
     
     }
 
@@ -95,7 +96,14 @@
 void CurrentRegulator::SampleCurrent(){
     _Inverter->GetCurrent(&I_A, &I_B, &I_C);
     Clarke(I_A, I_B, &I_Alpha, &I_Beta);
-    Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
+    float ID_Sample, IQ_Sample;
+    //Park(I_Alpha, I_Beta, theta_elec, &I_D, &I_Q);
+    
+    Park(I_Alpha, I_Beta, theta_elec, &ID_Sample, &IQ_Sample);    
+    I_D = .5f*ID_Sample + .5f*ID_Old;
+    I_Q = .5f*IQ_Sample + .5f*IQ_Old;
+    ID_Old = I_D;
+    IQ_Old = I_Q;
     //count += 1;
     //if(count > 10000) {
     //    count=0;
@@ -134,7 +142,7 @@
     
     
 void CurrentRegulator::Commutate(){
-    //count += 1;
+    count += 1;
     //GPIOC->ODR = (1 << 4); //Toggle pin for debugging
     theta_elec = _PositionSensor->GetElecPosition();
     _PositionSensor->GetMechPosition();
@@ -143,13 +151,13 @@
     SetVoltage();   //Set inverter duty cycles
     //GPIOC->ODR = (0 << 4); //Toggle pin for debugging
 
-    /*
+    
     if (count==1000){
-        pc->printf("%f\n\r", _PositionSensor->GetMechPosition());
+        //printf("%f\n\r", V_A);
         count = 0;
         }
 
-    }
+    
     
-      */  
+      
       }

--- a/CurrentRegulator/CurrentRegulator.h	Sun May 22 03:47:40 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.h	Mon Oct 31 16:48:16 2016 +0000
@@ -13,7 +13,7 @@
         void Reset();
         virtual float GetQ();
     private:
-        float IQ_Ref, ID_Ref, V_Q, V_D, V_Alpha, V_Beta, V_A, V_B, V_C, I_Q, I_D, I_A, I_B, I_C, I_Alpha, I_Beta, theta_elec, _Kp, _Ki, _L;
+        float IQ_Ref, ID_Ref, V_Q, V_D, V_Alpha, V_Beta, V_A, V_B, V_C, I_Q, I_D, IQ_Old,ID_Old,I_A, I_B, I_C, I_Alpha, I_Beta, theta_elec, _Kp, _Ki, _L;
         float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max, Q_Max;
         void SampleCurrent();
         void SetVoltage();

--- a/Inverter/Inverter.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/Inverter/Inverter.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -37,7 +37,8 @@
     //PWM Setup
 
     TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
-    TIM1->ARR = 0x1194; // 20 Khz
+    //TIM1->ARR = 0x1194; // 20 Khz
+    TIM1->ARR = 0x8CA;
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;     
     
@@ -86,8 +87,8 @@
     I_B_Offset = 0;
     I_C_Offset = 0;
     for (int i=0; i < 1000; i++){
-        I_B_Offset += ADC1->DR;
-        I_C_Offset += ADC2->DR;
+        I_B_Offset += ADC2->DR;
+        I_C_Offset += ADC1->DR;
         ADC1->CR2  |= 0x40000000; 
         wait(.0001);
         }
@@ -100,13 +101,14 @@
     *A = I_A;
     *B = I_B;
     *C = I_C;
+    //printf("I_A: %f    I_B: %f   I_C: %f\n\r", I_A, I_B, I_C);
     }
 
 void Inverter::SampleCurrent(void){
  //   Dbg->write(1);
     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_B = _I_Scale*((float) (ADC2->DR) -  I_B_Offset);
+    I_C = _I_Scale*((float) (ADC1->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

--- a/PositionSensor/PositionSensor.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -3,7 +3,7 @@
 #include "PositionSensor.h"
 //#include <math.h>
 
-PositionSensorSPI::PositionSensorSPI(int CPR, float offset, int ppairs){
+PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){
     //_CPR = CPR;
     _CPR = CPR;
     _ppairs = ppairs;
@@ -16,7 +16,7 @@
     MechOffset = 0;
     }
 
-int PositionSensorSPI::GetRawPosition(){
+int PositionSensorMA700::GetRawPosition(){
         cs->write(0);
     int response = spi->write(0)>>4;
     cs->write(1);
@@ -24,7 +24,7 @@
     }
     
 
-float PositionSensorSPI::GetMechPosition(){
+float PositionSensorMA700::GetMechPosition(){
     cs->write(0);
     int response = spi->write(0)>>4;
     cs->write(1);
@@ -40,7 +40,7 @@
     return MechPosition;
     }
 
-float PositionSensorSPI::GetElecPosition(){
+float PositionSensorMA700::GetElecPosition(){
     cs->write(0);
     int response = spi->write(0)>>4;
     cs->write(1);
@@ -49,16 +49,74 @@
     return elec;
     }
 
-float PositionSensorSPI::GetMechVelocity(){
+float PositionSensorMA700::GetMechVelocity(){
     return 0;
     }
 
-void PositionSensorSPI::ZeroPosition(){
+void PositionSensorMA700::ZeroPosition(){
     rotations = 0;
     MechOffset = GetMechPosition();
     }
     
+PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
+    //_CPR = CPR;
+    _CPR = CPR;
+    _ppairs = ppairs;
+    _offset = offset;
+    rotations = 0;
+    spi = new SPI(PC_12, PC_11, PC_10);
+    spi->format(16, 1);
+    spi->frequency(10000000);
+    cs = new DigitalOut(PA_15);
+    cs->write(1);
+    readAngleCmd = 0xffff;
+    MechOffset = 0;
+    }
 
+int PositionSensorAM5147::GetRawPosition(){
+    cs->write(0);
+    int angle = spi->write(0xffff);
+    angle &= 0x3FFF;    //Extract last 14 bits
+    cs->write(1);    
+    return angle;
+    }
+    
+
+float PositionSensorAM5147::GetMechPosition(){
+    cs->write(0);
+    int angle = spi->write(readAngleCmd);
+    angle &= 0x3FFF;    //Extract last 14 bits
+    cs->write(1);
+    if(angle - old_counts > _CPR/4){
+        rotations -= 1;
+        }
+    else if (angle - old_counts < -_CPR/4){
+        rotations += 1;
+        }
+    old_counts = angle;
+    MechPosition = (6.28318530718f * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    //return MechPosition - MechOffset;
+    return MechPosition;
+    }
+
+float PositionSensorAM5147::GetElecPosition(){
+    cs->write(0);
+    int angle = spi->write(readAngleCmd);
+    angle &= 0x3FFF;    //Extract last 14 bits
+    cs->write(1);
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) - _offset;
+    if(elec < 0) elec += 6.28318530718f;
+    return elec;
+    }
+
+float PositionSensorAM5147::GetMechVelocity(){
+    return 0;
+    }
+
+void PositionSensorAM5147::ZeroPosition(){
+    rotations = 0;
+    MechOffset = GetMechPosition();
+    }
     
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
     _ppairs = ppairs;
@@ -149,7 +207,7 @@
     float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
     out = meas;
     if(meas == vel_old){
-        out = .95f*out_old;
+        out = .99f*out_old;
         }
     else{
         out = meas;

--- a/PositionSensor/PositionSensor.h	Sun May 22 03:47:40 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Mon Oct 31 16:48:16 2016 +0000
@@ -27,9 +27,9 @@
     float _offset, MechPosition, dir, test_pos, vel_old, out_old;
 };
 
-class PositionSensorSPI: public PositionSensor{
+class PositionSensorMA700: public PositionSensor{
 public:
-    PositionSensorSPI(int CPR, float offset, int ppairs);
+    PositionSensorMA700(int CPR, float offset, int ppairs);
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
@@ -41,4 +41,21 @@
     SPI *spi;
     DigitalOut *cs;
 };
+
+class PositionSensorAM5147: public PositionSensor{
+public:
+    PositionSensorAM5147(int CPR, float offset, int ppairs);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual int GetRawPosition();
+    virtual void ZeroPosition();
+private:
+    float _offset, MechPosition, MechOffset;
+    int _CPR, rotations, old_counts, _ppairs;
+    SPI *spi;
+    DigitalOut *cs;
+    int readAngleCmd;
+
+};
 #endif
\ No newline at end of file

--- a/Transforms/Transforms.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/Transforms/Transforms.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -9,10 +9,10 @@
     //float sine = sin(theta);
     float cosine = FastCos(theta);
     float sine = FastSin(theta);
-    *d = alpha*cosine - beta*sine;      //This is a hack - effectively using -beta instead of beta
-    *q = -beta*cosine - alpha*sine;     //I think because I'm using pi as the d axis offset instead of zero, but I need to investigate more.
-    //*d = alpha*cosine + beta*sine;
-    //*q = beta*cosine - alpha*sine;
+    //*d = alpha*cosine - beta*sine;      //This is a hack - effectively using -beta instead of beta
+    //*q = -beta*cosine - alpha*sine;     //I think because I'm using pi as the d axis offset instead of zero, but I need to investigate more.
+    *d = alpha*cosine + beta*sine;
+    *q = beta*cosine - alpha*sine;
     //DAC->DHR12R1 = (int) (*q*49.648f) + 2048;
     //DAC->DHR12R1 = (int) (*q*2048.0f) + 2048;
     }

--- a/main.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/main.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -19,12 +19,11 @@
 Serial pc(PA_2, PA_3);
 
 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motor
-PositionSensorSPI spi(2048, 2.75f, 7);   ///1  I really need an eeprom or something to store this....
+PositionSensorAM5147 spi(16384, 2.7f, 7);   ///1  I really need an eeprom or something to store this....
 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
-int motorID = 40; ///1
-//int motorID = 50;  ///2
+
 
-PositionSensorEncoder encoder(1024, 0, 7); 
+PositionSensorEncoder encoder(4096, 0, 7); 
 
 
 
@@ -46,120 +45,40 @@
   TIM1->SR = 0x0; // reset the status register
 }
 
-// HobbyKing-style startup tone.  Just because.
-void hk_start(void){
-    float dtc  = .1;
-    inverter.SetDTC(0, 0, 0);
-    inverter.EnableInverter();
-    for(int i = 0; i<200; i++){
-        //torqueController.SetTorque(.4);
-        inverter.SetDTC(dtc, 0, 0);
-        wait(0.00047778308);
-        //torqueController.SetTorque(-.4);
-        inverter.SetDTC(0, dtc, 0);
-        wait(0.00047778308);
-        }
-    for(int i = 0; i<200; i++){
-        //torqueController.SetTorque(.4);
-        inverter.SetDTC(dtc, 0, 0);
-        wait(0.00042565508);
-        //torqueController.SetTorque(-.4);
-        inverter.SetDTC(0, dtc, 0);
-        wait(0.00042565508);
-        }    
-    for(int i = 0; i<200; i++){
-        //torqueController.SetTorque(.4);
-        inverter.SetDTC(dtc, 0, 0);
-        wait(0.00037921593);
-        //torqueController.SetTorque(-.4);
-        inverter.SetDTC(0, dtc, 0);
-        wait(0.00037921593);
-        }   
-        inverter.SetDTC(0, 0, 0);
-    wait(1);
-    for (int j = 0; j<3; j++){
-        for(int i = 0; i<240; i++){
-            //torqueController.SetTorque(.4);
-            inverter.SetDTC(dtc, 0, 0);
-            wait(0.00047778308);
-            //torqueController.SetTorque(-.4);
-            inverter.SetDTC(0, dtc, 0);
-            wait(0.00047778308);
-            }   
-            torqueController.SetTorque(0);
-            inverter.SetDTC(0, 0, 0);
-            wait(.2);
-
-        }
+int count = 0;
+void Loop(void){
+    count++;
+    //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    impedanceController.SetImpedance(.1, -0.01, 0);
     
-    }
-
-
-/*      //sinusoidal voltage-mode control, 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);
-    }
-*/
-
-// For decoding serial commands.
- void serialInterrupt(void){
-     //wait(.001);
-     int i = 0;
-     while(pc.readable()){
-         buff[i] = pc.getc();
-         wait(.0001);
-         i++;
-         
-         }
-     int val = (buff[4]<<8) + buff[5];
-     int checksum = buff[2]^buff[3]^buff[4]^buff[5];
-     int validStart = (buff[0] == 255 && buff[1] == 255 && buff[2]==motorID && checksum==buff[6]);
-
-     if(validStart){
-
-                switch(buff[3]){
-                    case 10:
-                        cmd_float[1] = (float)val*val_max[1]/65278.0f;
-                        break;
-                    case 20:
-                        cmd_float[2] = (float)val*val_max[2]/65278.0f;
-                        break;
-                    case 30:
-                        cmd_float[0] = (float)val*val_max[0]/65278.0f;
-                        break;
-                        }
-                        }
-                        
-    
-    //pc.printf("%d  %d  %d  %d  %d  %d  %d \n", start1, start2, id, cmd, byte1, byte2, byte3);
-    //pc.printf("%f, %f, %f\n", cmd_float[0], cmd_float[1], cmd_float[2]);
-    //pc.printf("%d\n", cmd); 
-    //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6], buff[7]);
-     }
-
-void Loop(void){
-    
-    impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
-    //impedanceController.SetImpedance(-.04, 0, 0);
-    //torqueController.SetTorque(0);
+    //torqueController.SetTorque(-.03);
     //foc.Commutate();
     //voltage_foc();
+    if(count>2000){
+        //float e = spi.GetElecPosition();
+        //float v = encoder.GetMechVelocity();
+        //printf("%f\n\r", v);
+        //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
+        count = 0;
+        }
 
     }
 
 void PrintStuff(void){
+    //inverter.SetDTC(0.03, 0.0, 0.0);
+
     //float v = encoder.GetMechVelocity();
     //float position = encoder.GetElecPosition();
-    //float position = encoder.GetMechPosition();
+    int position = spi.GetRawPosition();
     //float m = spi.GetMechPosition();
-    //float e = spi.GetElecPosition();
-    //printf("%f\n\r", e);
+    float e = spi.GetElecPosition();
+    foc.Commutate();
+    float q = foc.GetQ();
+    printf("position: %d   angle: %f    q current: %f\n\r", position, e, q);
+    //inverter.getCurrent()
     //printf("%f   %f   %f   %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
     //printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
+    //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
     }
 
  /*
@@ -178,13 +97,14 @@
     inverter.DisableInverter();
     spi.ZeroPosition();
     wait(.1);
-    inverter.SetDTC(0.2, 0.2, 0.2);
+    inverter.SetDTC(0.03, 0.0, 0.0);
     inverter.EnableInverter();
-    //hk_start();
     foc.Reset();
-    testing.attach(&Loop, .0001);
+    testing.attach(&Loop, .000025);
+    //testing.attach(&PrintStuff, .05);
     NVIC_SetPriority(TIM5_IRQn, 2);
-    pc.baud(115200);
+    pc.baud(921600);
+    pc.printf("HobbyKing Cheeta v1.1\n\r");
     wait(.1);
     while(1) {