Revision 8:70122bad5f90, committed 2016-04-23

Comitter:

bwang

Date:

Sat Apr 23 21:29:32 2016 +0000

Parent:

7:caebf421f288

Commit message:

foc'ed in the bot

Changed in this revision

--- a/PositionSensor/PositionSensor.cpp	Wed Mar 30 06:50:11 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sat Apr 23 21:29:32 2016 +0000
@@ -81,12 +81,14 @@
     }
 }
 
+int PositionSensorEncoder::GetRawPosition() {
+    return TIM2->CNT;
+}
+
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1){
         if (ZSense->read() == 1){
-            ZTest->write(state);
             TIM2->CNT=0;
-            state = !state;
         }
     }
 }
\ No newline at end of file

--- a/PositionSensor/PositionSensor.h	Wed Mar 30 06:50:11 2016 +0000
+++ b/PositionSensor/PositionSensor.h	Sat Apr 23 21:29:32 2016 +0000
@@ -5,6 +5,7 @@
 public:
     virtual float GetMechPosition() {return 0.0f;}
     virtual float GetElecPosition() {return 0.0f;}
+    virtual int GetRawPosition() {return 0;}
 };
   
   
@@ -13,6 +14,7 @@
     PositionSensorEncoder(int CPR, float offset);
     virtual float GetMechPosition();
     virtual float GetElecPosition();
+    virtual int GetRawPosition();
 private:
     InterruptIn *ZPulse;
     DigitalIn *ZSense;

--- a/config.h	Wed Mar 30 06:50:11 2016 +0000
+++ b/config.h	Sat Apr 23 21:29:32 2016 +0000
@@ -1,7 +1,7 @@
 #ifndef __CONFIG_H
 #define __CONFIG_H
 
-#define set_dtc(phase, value) *phase = 1.0f - (value)
+#define set_dtc(phase, value) *phase = (value)
 
 #define PWMA PA_8
 #define PWMB PA_9
@@ -25,9 +25,9 @@
 #define I_OFFSET (AVDD * R_DOWN * R_UP / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
 #define I_SCALE (R_BIAS * R_DOWN * I_SCALE_RAW / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
 
-#define K_LOOP 0.02f
-#define KI_BASE 0.008f
-#define BUS_VOLTAGE 20.0f
+#define K_LOOP 0.02
+#define KI_BASE 0.008
+#define BUS_VOLTAGE 20.0
 
 #define KP (K_LOOP / BUS_VOLTAGE)
 #define KI (KI_BASE * K_LOOP / BUS_VOLTAGE)

--- a/main.cpp	Wed Mar 30 06:50:11 2016 +0000
+++ b/main.cpp	Sat Apr 23 21:29:32 2016 +0000
@@ -9,34 +9,31 @@
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
-//DigitalOut toggle(PC_10);
+DigitalOut toggle(PC_10);
 
 PositionSensorEncoder pos(CPR, 0);
 
-Serial pc(USBTX, USBRX);
-
 int state = 0;
 int adval1, adval2;
-float ia, ib, ic, alpha, beta, d, q, vd, vq, p = 0.0f;
+float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
 
 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
 
-float d_integral = 0.0f, q_integral = 0.0f;
-float last_d = 0.0f, last_q = 0.0f;
-float d_ref = -0.0f, q_ref = 30.0f;
+double d_integral = 0.0f, q_integral = 0.0f;
+double last_d = 0.0f, last_q = 0.0f;
+double d_ref = 0.0f, q_ref = -5.0f;
 
 void commutate();
 void zero_current();
 void config_globals();
-void startup_msg();
 
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
-        //toggle = 1;
+        toggle = 1;
         ADC1->CR2  |= 0x40000000; 
         volatile int delay;
         for (delay = 0; delay < 35; delay++);
-        //toggle = 0;
+        toggle = 0;
         adval1 = ADC1->DR;
         adval2 = ADC2->DR;
         commutate();
@@ -57,9 +54,7 @@
     ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
 }
 
-void config_globals() {
-    pc.baud(115200);
-    
+void config_globals() {    
     //Enable clocks for GPIOs
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
@@ -74,13 +69,14 @@
     NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
 
     TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
-    TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up
+    //TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up
     TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, 
     TIM1->RCR |= 0x01; //update event once per up/down count of tim1 
     TIM1->EGR |= TIM_EGR_UG;
     
     TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
-    TIM1->ARR = 0x1770; //15 Khz
+    TIM1->ARR = 0x2EE0;
+    //TIM1->ARR = 0x1770; //15 Khz
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;
     
@@ -126,31 +122,16 @@
     wait_ms(250);
     zero_current();
     en = 1;
-}
-
-void startup_msg() {
-    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
-    pc.printf("%s\n\r", "====Config Data====");
-    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
-    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
-    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
-    pc.printf("Loop KP: %f\n\r", KP);
-    pc.printf("Loop KI: %f\n\r", KI);
-    pc.printf("Ia offset: %f mV\n\r", ia_supp_offset);
-    pc.printf("Ib offset: %f mV\n\r", ib_supp_offset);
-    pc.printf("\n\r");
-}    
+} 
 
 void commutate() {
     p = pos.GetElecPosition() - POS_OFFSET;
-         
     if (p < 0) p += 2 * PI;
-    if (p > 2 * PI) p -= 2 * PI;
     
     float sin_p = sinf(p);
     float cos_p = cosf(p);
     
-    //float pos_dac = 0.8f * p / (2 * PI) + 0.1f;
+    //float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
     //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
     
     ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
@@ -185,11 +166,11 @@
     if (vq < -1.0f) vq = -1.0f;
     if (vq > 1.0f) vq = 1.0f;
     
-    //DAC->DHR12R2 = (unsigned int) (q * 20 + 2048);
-    //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
+    vd = 0.0f;
+    vq = 1.0f;
     
-    //vd = 0.0f;
-    //vq = 1.0f;
+    DAC->DHR12R2 = (unsigned int) (q * 20 + 2048);
+    //DAC->DHR12R2 = (unsigned int) (vq * 2000 + 2048);
     
     float valpha = vd * cos_p - vq * sin_p;
     float vbeta = vd * sin_p + vq * cos_p;
@@ -198,8 +179,6 @@
     float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
     float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
     
-    //DAC->DHR12R2 = (unsigned int) (-va * 1500 + 2048);
-    
     set_dtc(a, 0.5f + 0.5f * va);
     set_dtc(b, 0.5f + 0.5f * vb);
     set_dtc(c, 0.5f + 0.5f * vc);
@@ -207,20 +186,7 @@
     
 int main() {
     config_globals();
-    startup_msg();
     
     for (;;) {
-        //pc.printf("%f\n\r", p);
-        //wait_ms(100);
-        /*
-        q_ref = 0.0f;
-        wait(3);
-        toggle = state;
-        state = !state;
-        q_ref = -50.0f;
-        wait(3);
-        toggle = state;
-        state = !state;
-        */
     }
 }