Revision 9:d7eb815cb057, committed 2016-05-10

Comitter:

benkatz

Date:

Tue May 10 01:15:57 2016 +0000

Parent:

8:10ae7bc88d6e

Child:

10:370851e6e132

Commit message:

Fixed current scaling hack

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Tue May 10 01:15:57 2016 +0000
@@ -12,6 +12,7 @@
     PWM = new SPWM(inverter, 2.0);
     _PositionSensor = position_sensor;
     IQ_Ref = 0;
+    Q_Max = 40.0f;
     ID_Ref = 0;
     V_Q = 0;
     V_D = 0;
@@ -35,8 +36,8 @@
     Int_Max = .9;
     DTC_Max = .97;
     //theta_elec = _PositionSensor->GetElecPosition();
-    pc = new Serial(PA_2, PA_3);
-    pc->baud(115200);
+    //pc = new Serial(PA_2, PA_3);
+    //pc->baud(115200);
     
     }
 
@@ -45,7 +46,42 @@
     
     }
 
+void CurrentRegulator::Reset(void){
+    IQ_Ref = 0;
+    ID_Ref = 0;
+    V_Q = 0;
+    V_D = 0;
+    V_Alpha = 0;
+    V_Beta = 0;
+    V_A = 0;
+    V_B = 0;
+    V_C = 0;
+    I_Q = 0;
+    I_D = 0;
+    I_A = 0;
+    I_B = 0;
+    I_C = 0;
+    I_Alpha = 0;
+    I_Beta = 0;
+    //pc->printf("%f, %f\n\r", Q_Integral, D_Integral);
+    wait(.03);
+    Q_Integral = 0;
+    D_Integral = 0;    
+    }
+    
 void CurrentRegulator::UpdateRef(float D, float Q){
+    if(Q>Q_Max){
+        Q = Q_Max;
+        }
+    else if(Q<-Q_Max){
+        Q = -Q_Max;
+        }
+    if(D>Q_Max){
+        D = Q_Max;
+        }
+    else if(D<-Q_Max){
+        D = -Q_Max;
+        }
     IQ_Ref = Q;
     ID_Ref = D;
     }
@@ -78,35 +114,31 @@
         else if(D_Integral < -Int_Max) D_Integral = -Int_Max;       
          
         V_Q = Q_Integral + _Kp*Q_Error;
-        V_D = D_Integral + _Kp*D_Error;
-        //V_D = 0;
-        
-        
+        V_D = D_Integral + _Kp*D_Error;        
     }
         
 void CurrentRegulator::SetVoltage(){
     InvPark(V_D, V_Q, theta_elec, &V_Alpha, &V_Beta);
     InvClarke(V_Alpha, V_Beta, &V_A, &V_B, &V_C);
     PWM->Update_DTC(V_A, V_B, V_C);
-    //PWM->Update_DTC(V_Alpha, V_Beta);
     }
     
     
 void CurrentRegulator::Commutate(){
-    count += 1;
-    GPIOC->ODR = (1 << 4); //Toggle pin for debugging
+    //count += 1;
+    //GPIOC->ODR = (1 << 4); //Toggle pin for debugging
     theta_elec = _PositionSensor->GetElecPosition();
     _PositionSensor->GetMechPosition();
     SampleCurrent(); //Grab most recent current sample
     Update();   //Run control loop
     SetVoltage();   //Set inverter duty cycles
-    GPIOC->ODR = (0 << 4); //Toggle pin for debugging
+    //GPIOC->ODR = (0 << 4); //Toggle pin for debugging
 
     /*
     if (count==1000){
         pc->printf("%f\n\r", _PositionSensor->GetMechPosition());
         count = 0;
         }
-        */
+      */  
         
     }
\ No newline at end of file

--- a/CurrentRegulator/CurrentRegulator.h	Wed Apr 13 04:09:56 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.h	Tue May 10 01:15:57 2016 +0000
@@ -1,5 +1,6 @@
-#ifndef CURRENTRETULATOR_H
+#ifndef CURRENTREGULATOR_H
 #define CURRENTREGULATOR_H
+
 #include "Inverter.h"
 #include "SVM.h"
 #include "PositionSensor.h"
@@ -9,9 +10,10 @@
         CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, float Kp, float Ki);
         void UpdateRef(float D, float Q);
         void Commutate();
+        void Reset();
     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;
-        float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max;
+        float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max, Q_Max;
         void SampleCurrent();
         void SetVoltage();
         void Update();

--- a/ImpedanceController/ImpedanceController.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/ImpedanceController/ImpedanceController.cpp	Tue May 10 01:15:57 2016 +0000
@@ -1,2 +1,24 @@
+#include "TorqueController.h"
 #include "ImpedanceController.h"
+#include "CurrentRegulator.h"
+#include "PositionSensor.h"
 
+
+ImpedanceController::ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel){
+    _torqueController = torqueController;
+    _sensor_pos = sensor_pos;
+    _sensor_vel = sensor_vel;
+    }
+
+ void ImpedanceController::SetImpedance(float K, float B, float ref){
+    float position = _sensor_pos->GetMechPosition();
+    float velocity = _sensor_vel->GetMechVelocity();
+    float error = ref-position;
+    float output = K*error + B*velocity;
+    
+    _torqueController->SetTorque(output);
+    
+    
+
+    }
+    

--- a/ImpedanceController/ImpedanceController.h	Wed Apr 13 04:09:56 2016 +0000
+++ b/ImpedanceController/ImpedanceController.h	Tue May 10 01:15:57 2016 +0000
@@ -1,12 +1,19 @@
-
-
+#ifndef IMPEDANCECONTROLLER_H
+#define IMPEDANCECONTROLLER_H
+#include "TorqueController.h"
+#include "PositionSensor.h"
 
 class ImpedanceController{
     public:
         //CurrentRegulator();
-        virtual void SetImpedance(float K, float B, float );
+        ImpedanceController(TorqueController *torqueController, PositionSensor *sensor_pos, PositionSensor *sensor_vel);
+        virtual void SetImpedance(float K, float B, float ref);
     
     private:
         float reference, _K, _B, output;   //Referene position (rad), motor stiffness (N-m/rad), motor damping (N-m*s/rad), output(N-m)
+        TorqueController* _torqueController;
+        PositionSensor* _sensor_pos;
+        PositionSensor* _sensor_vel;
+    };
     
-    };
\ No newline at end of file
+#endif
\ No newline at end of file

--- a/Inverter/Inverter.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/Inverter/Inverter.cpp	Tue May 10 01:15:57 2016 +0000
@@ -62,6 +62,8 @@
      SetDTC(0.0f, 0.0f, 0.0f);
      wait(.2);
      ZeroCurrent();
+     wait(.1);
+     DisableInverter();
     }
 
 void Inverter::SetDTC(float DTC_A, float DTC_B, float DTC_C){
@@ -85,10 +87,11 @@
         I_B_Offset += ADC1->DR;
         I_C_Offset += ADC2->DR;
         ADC1->CR2  |= 0x40000000; 
+        wait(.0001);
         }
     I_B_Offset = I_B_Offset/1000.0f;
     I_C_Offset = I_C_Offset/1000.0f;
-    printf("B_Offset:  %f     C_Offset:  %f\n\r", I_B_Offset, I_C_Offset);
+    //printf("B_Offset:  %f     C_Offset:  %f\n\r", I_B_Offset, I_C_Offset);
     }
 
 void Inverter::GetCurrent(float *A, float *B, float *C){

--- a/PositionSensor/PositionSensor.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Tue May 10 01:15:57 2016 +0000
@@ -12,7 +12,7 @@
     spi->format(16, 0);
     cs = new DigitalOut(PA_15);
     cs->write(1);
-    
+    MechOffset = 0;
     }
     
 float PositionSensorSPI::GetMechPosition(){
@@ -27,7 +27,7 @@
         }
     old_counts = response;
     MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
-    return MechPosition;
+    return MechPosition - MechOffset;
     
     }
 
@@ -39,6 +39,15 @@
     if(elec < 0) elec += 6.28318530718f;
     return elec;
     }
+
+float PositionSensorSPI::GetMechVelocity(){
+    return 0;
+    }
+
+void PositionSensorSPI::ZeroPosition(){
+    rotations = 0;
+    MechOffset = GetMechPosition();
+    }
     
 
     
@@ -72,7 +81,7 @@
     TIM3->CNT = 0x000;  //reset the counter before we use it  
     
     // Extra Timer for velocity measurement
-    /*
+    
     __TIM2_CLK_ENABLE();
     TIM3->CR2 = 0x030;  //MMS = 101
     
@@ -88,7 +97,7 @@
     
     
     TIM2->CR1 = 0x01;       //CEN
-    */
+    
     TIM3->CR1   = 0x01;     // CEN
     ZPulse = new InterruptIn(PC_4);
     ZSense = new DigitalIn(PC_4);
@@ -127,9 +136,29 @@
     }
     
 float PositionSensorEncoder::GetMechVelocity(){
+    float out = 0;
     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; 
+    float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
+    
+    if(meas == vel_old){
+        out = .95f*out_old;
+        }
+    else{
+        out = meas;
+        }
+    
+    /*
+    if(abs(vel) < 2.0f){
+        vel = 0;
+        }
+        */
+    vel_old = meas;
+    
+    //out = .5f*out + .5f*out_old;
+    out_old = out;
+    return out;
     }
 
 void PositionSensorEncoder::ZeroEncoderCount(void){

--- a/PositionSensor/PositionSensor.h	Wed Apr 13 04:09:56 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Tue May 10 01:15:57 2016 +0000
@@ -4,6 +4,7 @@
 public:
     virtual float GetMechPosition() {return 0.0f;}
     virtual float GetElecPosition() {return 0.0f;}
+    virtual float GetMechVelocity() {return 0.0f;}
 };
   
   
@@ -22,7 +23,7 @@
     virtual void ZeroEncoderCountDown(void);
     int _CPR, flag, rotations;
     //int state;
-    float _offset, MechPosition, dir, test_pos;
+    float _offset, MechPosition, dir, test_pos, vel_old, out_old;
 };
 
 class PositionSensorSPI: public PositionSensor{
@@ -30,8 +31,10 @@
     PositionSensorSPI(int CPR, float offset);
     virtual float GetMechPosition();
     virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual void ZeroPosition();
 private:
-    float _offset, MechPosition;
+    float _offset, MechPosition, MechOffset;
     int _CPR, rotations, old_counts;
     SPI *spi;
     DigitalOut *cs;

--- a/TorqueController/TorqueController.h	Wed Apr 13 04:09:56 2016 +0000
+++ b/TorqueController/TorqueController.h	Tue May 10 01:15:57 2016 +0000
@@ -1,5 +1,7 @@
-#ifndef CURRENTRETULATOR_H
-#define CURRENTREGULATOR_H
+#ifndef TORQUECONTROLLER_H
+#define TORQUECONTROLLER_H
+
+#include "CurrentRegulator.h"
 
 class TorqueController{
 public:

--- a/main.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/main.cpp	Tue May 10 01:15:57 2016 +0000
@@ -6,19 +6,40 @@
 #include "Transforms.h"
 #include "CurrentRegulator.h"
 #include "TorqueController.h"
-//#include "ImpedanceController.h"
+#include "ImpedanceController.h"
+
+///SPI Input Stuff
+//DigitalIn cselect(PB_12);
+//InterruptIn select(PB_12);
+//DigitalIn mosi(PB_15);
+//SPISlave input(PB_15, PB_14, PB_13, PB_12);
+
+int id[3] = {0};
+float cmd_float[3] = {0.0f};
+int raw[3] = {0};
+float val_max[3] = {18.0f, 1.0f, 0.1f};
+int buff[8];
+Serial pc(PA_2, PA_3);
 
 //PositionSensorEncoder encoder(8192,4.0f);
 //Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
-Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.01007080078, 0.00005);  //hall motter
+Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motter
 //Inverter inverter(PA_10, PA_9, PA_8, PB_7, 0.01007080078, 0.00005);  //test motter
-PositionSensorSPI spi(2048, 2.75f);
+PositionSensorSPI spi(2048, 2.75f);   ///1  I really need an eeprom or something to store this....
+//PositionSensorSPI spi(2048, 1.34f); ///2
+//PositionSensorSPI spi(2048, 1);
+int motorID = 40; ///1
+//int motorID = 50;  ///2
+
+PositionSensorEncoder encoder(1024, 0);
 
 CurrentRegulator foc(&inverter, &spi, .005, .5);  //hall sensor
-TorqueController torqueController(.061f, &foc);
+TorqueController torqueController(.031f, &foc);
+ImpedanceController impedanceController(&torqueController, &spi, &encoder);
+
 //CurrentRegulator foc(&inverter, &encoder, .005, .5);    //test motter
+//SVPWM  svpwm(&inverter, 2.0f);
 
-//SVPWM  svpwm(&inverter, 2.0f);
 Ticker  testing;
 //Timer t;
 
@@ -31,6 +52,8 @@
 float v_b = 0;
 float v_c = 0;
 */
+float ref = 0.0;
+int count = 0;
 
 //SPI spi(PB_15, PB_14, PB_13);
 //GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
@@ -59,7 +82,7 @@
   if (TIM1->SR & TIM_SR_UIF ) {
       inverter.SampleCurrent();
       //wait(.00002);
-      //foc.Commutate();
+      //foc.Commutate(); ///Putting the loop here doesn't work for some reason.  Need to figure out why
       }
   TIM1->SR = 0x0; // reset the status register
     //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
@@ -76,19 +99,102 @@
     //output.write(theta/6.28318530718f);
     }
 */
+/*
+void read(void){
+    int startByte;
+    if(input.receive()){
+        //startByte = input.read();
+        //if(startByte == 65535){
+            //startByte = input.read();
+            //wait(.000005);
+            raw[0] = input.read();
+            raw[1] = input.read();
+            raw[2] = input.read();
+            id[0] = raw[0]>>14;
+            id[1] = raw[1]>>14;
+            id[2] = raw[2]>>14;
+            printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
+            for(int i = 0; i<3; i++){
+                    cmd_float[id[i]] = (val_max[id[i]])*(float)(raw[i] - (id[i]<<14))/16383.0f;
+                    }
+           //  }
+         // else{
+          //    input.read();
+          //    input.read();
+           //   input.read();
+           //   }
+        //printf("%d   %d   %d \n\r", raw[0], raw[1], raw[2]);
+        }
+        
+    }
+    
+*/
+
+ 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){
-    foc.Commutate();
+    
+    impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    //impedanceController.SetImpedance(-.4, -0.006, 0);
+
+    count = count+1;
+    
+    if(count > 1000){
+        //ref= -1*ref;
+        //printf("%f   %f   %f \n\r",  cmd_float[0], cmd_float[1], cmd_float[2]);
+        //float e = spi.GetElecPosition();
+        //printf("%f\n\r", e);
+        count = 0;
+        }
+    
+     
+    //torqueController.SetTorque(0);
+    //foc.Commutate();
     //voltage_foc();
     }
 
 void PrintStuff(void){
-    //float velocity = encoder.GetMechVelocity();
+    //float v = encoder.GetMechVelocity();
     //float position = encoder.GetElecPosition();
     //float position = encoder.GetMechPosition();
-    float m = spi.GetMechPosition();
+    //float m = spi.GetMechPosition();
     float e = spi.GetElecPosition();
-    //printf("%f, %f;\n\r", position, velocity);
-    printf("Elec:  %f \n\r", m);
+    printf("%f\n\r", e);
+    //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]);
     }
     
 
@@ -106,16 +212,31 @@
 */
        
 int main() {
-    //t.start();
-    wait(1);
+    //mosi.mode(PullDown);
+    //cselect.mode(PullUp);
+    inverter.DisableInverter();
+    spi.ZeroPosition();
+    //input.format(16, 0);
+    //input.frequency(100000);
+    //select.fall(&read);
+
+
+    //NVIC_SetPriority(EXTI15_10_IRQn, 1);
+    wait(.1);
+    inverter.SetDTC(0.2, 0.2, 0.2);
+    inverter.EnableInverter();
+    foc.Reset();
     testing.attach(&Loop, .0001);
-    
-    NVIC_SetPriority(TIM5_IRQn, 1);
+    NVIC_SetPriority(TIM5_IRQn, 2);
+    pc.baud(115200);
+    //pc.attach(&serialInterrupt);
+    //printf("hello\n\r");
     //testing.attach(&gen_sine, .01);
     //testing.attach(&PrintStuff, .1);
     //inverter.SetDTC(.05, 0, 0);
-    //inverter.EnableInverter();
+    //inverter.DisableInverter();
     //foc.Commutate();
+    wait(.5);
     while(1) {
         //printf("%f\n\r", encoder.GetElecPosition());
         //wait(.1);