Revision 11:c83b18d41e54, committed 2016-05-22

Comitter:

benkatz

Date:

Sun May 22 00:14:59 2016 +0000

Parent:

10:370851e6e132

Child:

12:c473a25f54f7

Commit message:

summer cleaning;

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Thu May 12 05:02:52 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Sun May 22 00:14:59 2016 +0000
@@ -9,7 +9,7 @@
 
 CurrentRegulator::CurrentRegulator(Inverter *inverter, PositionSensor *position_sensor, float Kp, float Ki){
     _Inverter = inverter;
-    PWM = new SPWM(inverter, 2.0);
+    PWM = new SVPWM(inverter, 2.0);
     _PositionSensor = position_sensor;
     IQ_Ref = 0;
     Q_Max = 40.0f;
@@ -45,7 +45,11 @@
     
     
     }
-
+    
+float CurrentRegulator::GetQ(){
+    return I_Q;
+    }
+    
 void CurrentRegulator::Reset(void){
     IQ_Ref = 0;
     ID_Ref = 0;
@@ -139,6 +143,8 @@
         pc->printf("%f\n\r", _PositionSensor->GetMechPosition());
         count = 0;
         }
+
+    }
+    
       */  
-        
-    }
\ No newline at end of file
+      }

--- a/CurrentRegulator/CurrentRegulator.h	Thu May 12 05:02:52 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.h	Sun May 22 00:14:59 2016 +0000
@@ -11,6 +11,7 @@
         void UpdateRef(float D, float Q);
         void Commutate();
         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;
         float Q_Integral, D_Integral, Q_Error, D_Error, Int_Max, DTC_Max, Q_Max;

--- a/ImpedanceController/ImpedanceController.cpp	Thu May 12 05:02:52 2016 +0000
+++ b/ImpedanceController/ImpedanceController.cpp	Sun May 22 00:14:59 2016 +0000
@@ -18,7 +18,5 @@
     
     _torqueController->SetTorque(output);
     
-    
-
     }
     

--- a/PositionSensor/PositionSensor.cpp	Thu May 12 05:02:52 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun May 22 00:14:59 2016 +0000
@@ -15,7 +15,15 @@
     cs->write(1);
     MechOffset = 0;
     }
+
+int PositionSensorSPI::GetRawPosition(){
+        cs->write(0);
+    int response = spi->write(0)>>4;
+    cs->write(1);
+    return response;
+    }
     
+
 float PositionSensorSPI::GetMechPosition(){
     cs->write(0);
     int response = spi->write(0)>>4;
@@ -28,8 +36,8 @@
         }
     old_counts = response;
     MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
-    return MechPosition - MechOffset;
-    
+    //return MechPosition - MechOffset;
+    return MechPosition;
     }
 
 float PositionSensorSPI::GetElecPosition(){
@@ -143,7 +151,7 @@
     
     float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
     float meas = dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
-    
+    out = meas;
     if(meas == vel_old){
         out = .95f*out_old;
         }

--- a/PositionSensor/PositionSensor.h	Thu May 12 05:02:52 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Sun May 22 00:14:59 2016 +0000
@@ -32,6 +32,7 @@
     virtual float GetMechPosition();
     virtual float GetElecPosition();
     virtual float GetMechVelocity();
+    virtual int GetRawPosition();
     virtual void ZeroPosition();
 private:
     float _offset, MechPosition, MechOffset;

--- a/main.cpp	Thu May 12 05:02:52 2016 +0000
+++ b/main.cpp	Sun May 22 00:14:59 2016 +0000
@@ -8,120 +8,86 @@
 #include "TorqueController.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);
+using namespace FastMath;
+using namespace Transforms;
 
 int id[3] = {0};
 float cmd_float[3] = {0.0f};
 int raw[3] = {0};
-float val_max[3] = {18.0f, 1.0f, 0.1f};
+float val_max[3] = {18.0f, 1.0f, 0.1f};    //max angle in radians, stiffness in N-m/rad, damping in N-m*s/rad
 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.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, 7);   ///1  I really need an eeprom or something to store this....
+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....
 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
-PositionSensorSPI spi(2048, 3.0, 21);
 int motorID = 40; ///1
 //int motorID = 50;  ///2
 
-//PositionSensorEncoder encoder(1024, 0, 7);
-PositionSensorEncoder encoder(1024, 0, 21);
+PositionSensorEncoder encoder(1024, 0, 7); 
 
 
-CurrentRegulator foc(&inverter, &spi, .005, .5);  //hall sensor
+
+CurrentRegulator foc(&inverter, &spi, .005, .5);    
 TorqueController torqueController(.031f, &foc);
 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
 
-//CurrentRegulator foc(&inverter, &encoder, .005, .5);    //test motter
-//SVPWM  svpwm(&inverter, 2.0f);
-
 Ticker  testing;
-//Timer t;
 
-/*
-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;
-*/
-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
-
-//DigitalOut chipselect(PB_12);
-
-using namespace FastMath;
-using namespace Transforms;
 
 
 // Current Sampling IRQ
-/*
-extern "C" void TIM2_IRQHandler(void) {
-  // flash on update event
-  if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
-      inverter.SampleCurrent();
-      }
-  TIM2->SR = 0x0; // reset the status register
-}
-*/
-
-
-
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
-  // toggle on update event
   if (TIM1->SR & TIM_SR_UIF ) {
       inverter.SampleCurrent();
-      //wait(.00002);
       //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
 }
 
-
+// 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<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.000956);
-        torqueController.SetTorque(-.4);
-        wait(0.000956);
+    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<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.0008513);
-        torqueController.SetTorque(-.4);
-        wait(0.0008513);
+    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<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.00075843);
-        torqueController.SetTorque(-.4);
-        wait(0.00075843);
+    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);
         }   
-        torqueController.SetTorque(0);
-    wait(.4);
+        inverter.SetDTC(0, 0, 0);
+    wait(1);
     for (int j = 0; j<3; j++){
-        for(int i = 0; i<120; i++){
-            torqueController.SetTorque(.4);
-            wait(0.000956);
-            torqueController.SetTorque(-.4);
-            wait(0.000956);
+        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);
 
         }
@@ -129,7 +95,7 @@
     }
 
 
-/*
+/*      //sinusoidal voltage-mode control, for debugging.
 void voltage_foc(void){
     float theta = encoder.GetElecPosition();
     InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
@@ -138,37 +104,8 @@
     //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]);
-        }
-        
-    }
-    
-*/
 
+// For decoding serial commands.
  void serialInterrupt(void){
      //wait(.001);
      int i = 0;
@@ -203,26 +140,15 @@
     //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.00, 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;
-        }
-    
-     
+    impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    //impedanceController.SetImpedance(-.04, 0, 0);
     //torqueController.SetTorque(0);
     //foc.Commutate();
     //voltage_foc();
+
     }
 
 void PrintStuff(void){
@@ -235,11 +161,9 @@
     //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]);
     }
-    
 
-
- 
  /*
+ ////Throw some sines on the phases.  useful to make sure the hardware works.
  void gen_sine(void){
      float f = 1.0f;
      float time = t.read();
@@ -251,34 +175,18 @@
 */
        
 int main() {
-    //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();
-    hk_start();
-    //foc.Reset();
+    //hk_start();
+    foc.Reset();
     testing.attach(&Loop, .0001);
     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.DisableInverter();
-    //foc.Commutate();
-    wait(.5);
+    wait(.1);
     while(1) {
-        //printf("%f\n\r", encoder.GetElecPosition());
-        //wait(.1);
+
     }
 }