Revision 8:10ae7bc88d6e, committed 2016-04-13

Comitter:

benkatz

Date:

Wed Apr 13 04:09:56 2016 +0000

Parent:

7:dc5f27756e02

Child:

9:d7eb815cb057

Commit message:

Multi-turn mechanical position, MA700 position sensing over SPI. Placeholder torque controller for cogging/ripple compensation;

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Tue Mar 29 01:05:46 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Wed Apr 13 04:09:56 2016 +0000
@@ -11,7 +11,7 @@
     _Inverter = inverter;
     PWM = new SPWM(inverter, 2.0);
     _PositionSensor = position_sensor;
-    IQ_Ref = 2;
+    IQ_Ref = 0;
     ID_Ref = 0;
     V_Q = 0;
     V_D = 0;
@@ -27,7 +27,7 @@
     I_C = 0;
     I_Alpha = 0;
     I_Beta = 0;
-    //count = 0;
+    count = 0;
     _Kp = Kp;
     _Ki = Ki;
     Q_Integral = 0;
@@ -35,9 +35,9 @@
     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);
+    
     }
 
 void CurrentRegulator::SendSPI(){
@@ -93,26 +93,20 @@
     
     
 void CurrentRegulator::Commutate(){
-    //count += 1;
+    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
 
     /*
-    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));
-        pc->putc((unsigned char) (theta_elec*40.0f));
-        pc->putc((unsigned char) (I_A*100.0f+127));
-        pc->putc((unsigned char) (I_B*100.0f+127));
-        pc->putc((unsigned char) (I_Alpha*100.0f+127));
-        pc->putc((unsigned char) (I_Beta*100.0f+127));
-        pc->putc((unsigned char) (I_Q*100.0f+127));
-        pc->putc((unsigned char) (I_D*100.0f+127));
-        pc->putc((0xff));
+    if (count==1000){
+        pc->printf("%f\n\r", _PositionSensor->GetMechPosition());
         count = 0;
         }
         */
+        
     }
\ No newline at end of file

--- a/CurrentRegulator/CurrentRegulator.h	Tue Mar 29 01:05:46 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.h	Wed Apr 13 04:09:56 2016 +0000
@@ -19,8 +19,8 @@
         Inverter* _Inverter;
         PositionSensor* _PositionSensor;
         SVM* PWM;
-        //Serial* pc;
-        //int count;
+        Serial* pc;
+        int count;
             
     
     

--- a/PositionSensor/PositionSensor.cpp	Tue Mar 29 01:05:46 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Apr 13 04:09:56 2016 +0000
@@ -3,6 +3,44 @@
 #include "PositionSensor.h"
 //#include <math.h>
 
+PositionSensorSPI::PositionSensorSPI(int CPR, float offset){
+    //_CPR = CPR;
+    _CPR = 2048;
+    _offset = offset;
+    rotations = 0;
+    spi = new SPI(PC_12, PC_11, PC_10);
+    spi->format(16, 0);
+    cs = new DigitalOut(PA_15);
+    cs->write(1);
+    
+    }
+    
+float PositionSensorSPI::GetMechPosition(){
+    cs->write(0);
+    int response = spi->write(0)>>4;
+    cs->write(1);
+    if(response - old_counts > _CPR/4){
+        rotations -= 1;
+        }
+    else if (response - old_counts < -_CPR/4){
+        rotations += 1;
+        }
+    old_counts = response;
+    MechPosition = (6.28318530718f * ((float) response+(_CPR*rotations)))/ (float)_CPR;
+    return MechPosition;
+    
+    }
+
+float PositionSensorSPI::GetElecPosition(){
+    cs->write(0);
+    int response = spi->write(0)>>4;
+    cs->write(1);
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((7*response)%_CPR)) - _offset;
+    if(elec < 0) elec += 6.28318530718f;
+    return elec;
+    }
+    
+
     
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
     _CPR = CPR;
@@ -29,7 +67,7 @@
     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   = 0xfffffff; // reload at 0xfffffff         < TIM auto-reload register
+    TIM3->ARR   = CPR-1; // reload at 0xfffffff         < TIM auto-reload register
   
     TIM3->CNT = 0x000;  //reset the counter before we use it  
     
@@ -52,9 +90,10 @@
     TIM2->CR1 = 0x01;       //CEN
     */
     TIM3->CR1   = 0x01;     // CEN
-    
-    ZPulse = new InterruptIn(PB_0);
-    ZSense = new DigitalIn(PB_0);
+    ZPulse = new InterruptIn(PC_4);
+    ZSense = new DigitalIn(PC_4);
+    //ZPulse = new InterruptIn(PB_0);
+    //ZSense = new DigitalIn(PB_0);
     ZPulse->enable_irq();
     ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
     //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
@@ -69,16 +108,16 @@
 }
  
 float PositionSensorEncoder::GetMechPosition() {        //returns rotor angle in radians.
-    int raw = TIM3->CNT-0x8000;
+    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 unsigned_elec = (6.28318530718f/(float)_CPR) * (float) ((7*raw)%_CPR);
-    return unsigned_elec;
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((7*raw)%_CPR)) - _offset;
+    if(elec < 0) elec += 6.28318530718f;
+    return elec;
 }
 
 float PositionSensorEncoder::GetElecVelocity(){
@@ -96,15 +135,8 @@
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1 & flag == 0){
         if (ZSense->read() == 1){
-            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;
-                }
-            
+            GPIOC->ODR ^= (1 << 4);   
             TIM3->CNT = 0x000;
-
             //state = !state;
             //ZTest->write(state);
             GPIOC->ODR ^= (1 << 4);

--- a/PositionSensor/PositionSensor.h	Tue Mar 29 01:05:46 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Apr 13 04:09:56 2016 +0000
@@ -1,6 +1,5 @@
 #ifndef POSITIONSENSOR_H
 #define POSITIONSENSOR_H
-
 class PositionSensor {
 public:
     virtual float GetMechPosition() {return 0.0f;}
@@ -26,4 +25,15 @@
     float _offset, MechPosition, dir, test_pos;
 };
 
+class PositionSensorSPI: public PositionSensor{
+public:
+    PositionSensorSPI(int CPR, float offset);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+private:
+    float _offset, MechPosition;
+    int _CPR, rotations, old_counts;
+    SPI *spi;
+    DigitalOut *cs;
+};
 #endif
\ No newline at end of file

--- a/TorqueController/TorqueController.cpp	Tue Mar 29 01:05:46 2016 +0000
+++ b/TorqueController/TorqueController.cpp	Wed Apr 13 04:09:56 2016 +0000
@@ -1,5 +1,5 @@
 
-//cogging torque and torque ripple compensation will go here later
+//cogging torque and torque ripple compensation can go here later
 
 #include "CurrentRegulator.h"
 #include "TorqueController.h"

--- a/main.cpp	Tue Mar 29 01:05:46 2016 +0000
+++ b/main.cpp	Wed Apr 13 04:09:56 2016 +0000
@@ -5,16 +5,24 @@
 #include "FastMath.h"
 #include "Transforms.h"
 #include "CurrentRegulator.h"
-//#include "TorqueController.h"
+#include "TorqueController.h"
 //#include "ImpedanceController.h"
-PositionSensorEncoder encoder(8192,0);
+
+//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);
-CurrentRegulator foc(&inverter, &encoder, .005, .5);
-SVPWM  svpwm(&inverter, 2.0f);
+Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.01007080078, 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);
+
+CurrentRegulator foc(&inverter, &spi, .005, .5);  //hall sensor
+TorqueController torqueController(.061f, &foc);
+//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;
@@ -22,6 +30,7 @@
 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
@@ -42,6 +51,9 @@
   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 ) {
@@ -53,6 +65,9 @@
     //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);
@@ -60,17 +75,20 @@
     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();
+    //float velocity = encoder.GetMechVelocity();
+    //float position = encoder.GetElecPosition();
+    //float position = encoder.GetMechPosition();
+    float m = spi.GetMechPosition();
+    float e = spi.GetElecPosition();
     //printf("%f, %f;\n\r", position, velocity);
-    printf("%f   %d   %d\n\r", position, TIM3->CNT-0x8000, -2*((int)((TIM3->CR1)>>4)&1)+1);
+    printf("Elec:  %f \n\r", m);
     }
     
 
@@ -91,11 +109,12 @@
     //t.start();
     wait(1);
     testing.attach(&Loop, .0001);
+    
     NVIC_SetPriority(TIM5_IRQn, 1);
     //testing.attach(&gen_sine, .01);
     //testing.attach(&PrintStuff, .1);
-    //inverter.SetDTC(.1, 0, 0);
-    inverter.EnableInverter();
+    //inverter.SetDTC(.05, 0, 0);
+    //inverter.EnableInverter();
     //foc.Commutate();
     while(1) {
         //printf("%f\n\r", encoder.GetElecPosition());