Revision 3:6a0015d88d06, committed 2016-03-09

Comitter:

benkatz

Date:

Wed Mar 09 04:00:48 2016 +0000

Parent:

2:8724412ad628

Child:

4:c023f7b6f462

Commit message:

more motter;

Changed in this revision

--- a/CurrentRegulator/CurrentRegulator.cpp	Fri Feb 19 04:13:06 2016 +0000
+++ b/CurrentRegulator/CurrentRegulator.cpp	Wed Mar 09 04:00:48 2016 +0000
@@ -11,7 +11,7 @@
     _Inverter = inverter;
     PWM = new SPWM(inverter, 2.0);
     _PositionSensor = position_sensor;
-    IQ_Ref = 5;
+    IQ_Ref = 1;
     ID_Ref = 0;
     V_Q = 0;
     V_D = 0;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ImpedanceController/ImpedanceController.cpp	Wed Mar 09 04:00:48 2016 +0000
@@ -0,0 +1,2 @@
+#include "ImpedanceController.h"
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ImpedanceController/ImpedanceController.h	Wed Mar 09 04:00:48 2016 +0000
@@ -0,0 +1,11 @@
+
+
+
+class ImpedanceController{
+    public:
+        
+    
+    private:
+        float reference, K, B, output;   //Referene position (rad), motor stiffness (N-m/rad), motor damping (N-m*s/rad), output(N-m)
+    
+    };
\ No newline at end of file

--- a/Inverter/Inverter.cpp	Fri Feb 19 04:13:06 2016 +0000
+++ b/Inverter/Inverter.cpp	Wed Mar 09 04:00:48 2016 +0000
@@ -39,18 +39,7 @@
     TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
     TIM1->ARR = 0x1194; // 20 Khz
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
-    TIM1->CR1 |= TIM_CR1_CEN;  
-    
-    //NVIC->ISER[0] |= 1<< (TIM2_IRQn); // enable the TIM2 IRQ    
-     //TIM2->DIER |= TIM_DIER_UIE; // enable update interrupt
-    //TIM2->CR1 |= TIM_CR1_ARPE; // autoreload on, 
-    //TIM2->EGR |= TIM_EGR_UG;               
-    //TIM2->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
-    //TIM2->ARR = 0x8CA; // 
-    //TIM2->CCER |= TIM_CCER_CC1NP;
-      //TIM1->CR1 &= ~(TIM_CR1_CEN);
-    //TIM1->CR1 |= TIM_CR1_CMS;      
-
+    TIM1->CR1 |= TIM_CR1_CEN;     
     
     // ADC Setup
      RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2

--- a/PositionSensor/PositionSensor.cpp	Fri Feb 19 04:13:06 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Mar 09 04:00:48 2016 +0000
@@ -3,11 +3,11 @@
 #include "PositionSensor.h"
 #include <math.h>
 
-
     
 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
     _CPR = CPR;
     _offset = offset;
+    MechPosition = 0;
     
     // Enable clock for GPIOA
     __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
@@ -33,6 +33,26 @@
   
     TIM3->CNT = 0x8000;  //reset the counter before we use it  
     
+    // Extra Timer for velocity measurement
+    /*
+    __TIM2_CLK_ENABLE();
+    TIM3->CR2 = 0x030;  //MMS = 101
+    
+    TIM2->PSC = 0x03;
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
+    TIM2->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
+    
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->CCER |= TIM_CCER_CC1P;
+    //TIM2->CCER |= TIM_CCER_CC1NP;
+    TIM2->CCER |= TIM_CCER_CC1E;
+    
+    
+    TIM2->CR1 = 0x01;       //CEN
+    */
+    TIM3->CR1   = 0x01;     // CEN
+    
     ZPulse = new InterruptIn(PB_0);
     ZSense = new DigitalIn(PB_0);
     ZPulse->enable_irq();
@@ -50,14 +70,20 @@
     int raw = TIM3->CNT-0x8000;
     if (raw < 0) raw += _CPR;
     if (raw >= _CPR) raw -= _CPR;
-    return 6.28318530718f*(raw)/(float)_CPR + _offset;    
+    float signed_mech = fmod(((6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f); //7 pole pairs
+    if (signed_mech < 0){
+        return signed_mech + 6.28318530718f;
+        }
+    else{
+        return signed_mech;
+        }
 }
 
 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);
+    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;
@@ -67,6 +93,19 @@
         }
 }
 
+float PositionSensorEncoder::GetElecVelocity(){
+    float rawPeriod = TIM2->CCR1; //Clock Ticks
+    float  dir = (((TIM3->CR1)>>4)&1)*2-1;    // +/- 1
+    return dir*7*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
+    }
+    
+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;
+    
+    }
+
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1){
         if (ZSense->read() == 1){

--- a/PositionSensor/PositionSensor.h	Fri Feb 19 04:13:06 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Wed Mar 09 04:00:48 2016 +0000
@@ -13,6 +13,8 @@
     PositionSensorEncoder(int CPR, float offset);
     virtual float GetMechPosition();
     virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
 private:
     InterruptIn *ZPulse;
     DigitalIn *ZSense;
@@ -20,7 +22,7 @@
     virtual void ZeroEncoderCount(void);
     int _CPR;
     //int state;
-    float _offset;
+    float _offset, MechPosition;
 };
 
 #endif
\ No newline at end of file

--- a/main.cpp	Fri Feb 19 04:13:06 2016 +0000
+++ b/main.cpp	Wed Mar 09 04:00:48 2016 +0000
@@ -42,6 +42,12 @@
 void Loop(void){
     foc.Commutate();
     }
+
+void PrintStuff(void){
+    float velocity = encoder.GetMechVelocity();
+    float position = encoder.GetMechPosition();
+    printf("%f, %f;\n\r", position, velocity);
+    }
 /*
 void voltage_foc(void){
     theta = encoder.GetElecPosition() + offset;
@@ -56,10 +62,12 @@
     wait(1);
 
     testing.attach(&Loop, .0001);
-    //inverter.SetDTC(.2, .5, .97);
+    //testing.attach(&PrintStuff, .05);
+    //inverter.SetDTC(.1, 0, 0);
     //inverter.EnableInverter();
 
     while(1) {
-
+        //printf("%f\n\r", encoder.GetElecPosition());
+        //wait(.1);
     }
 }