Diff: main.cpp

Revision:

22:c8a1f2071bb0

Parent:

20:bf9ea5125d52

--- a/main.cpp	Thu Mar 02 15:31:45 2017 +0000
+++ b/main.cpp	Thu Nov 02 22:28:09 2017 +0000
@@ -1,3 +1,4 @@
+/*
 const unsigned int BOARDNUM = 0x2;
 
 //const unsigned int a_id =                            
@@ -6,9 +7,9 @@
     
 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
 
-
+*/
 
-#include "CANnucleo.h"
+//#include "CANnucleo.h"
 #include "mbed.h"
 #include "PositionSensor.h"
 #include "structs.h"
@@ -24,16 +25,17 @@
 COMStruct com;
 
 
-
+/*
 CANnucleo::CAN          can(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
 CANnucleo::CANMessage   rxMsg;
 CANnucleo::CANMessage   txMsg;
+*/
 int                     ledState;
 int                     counter = 0;
 int canCmd = 1000;
 volatile bool           msgAvailable = false;
 
-DigitalOut toggle(PA_0);
+//DigitalOut toggle(PA_0);
 Ticker  loop;
 /**
  * @brief   'CAN receive-complete' interrup handler.
@@ -42,6 +44,7 @@
  * @param   
  * @retval  
  */
+ /*
 void onMsgReceived() {
     msgAvailable = true;
     //printf("ping\n\r");
@@ -76,97 +79,68 @@
     //sendCMD(TX_ID+c_ID, c1);
     }
     
+*/
 
 Serial pc(PA_2, PA_3);
 
-PositionSensorAM5147 spi(16384, 4.7, NPP);   ///1  I really need an eeprom or something to store this....
+//PositionSensorAM5147 spi(16384, 0 , NPP);   ///1  I really need an eeprom or something to store this....
 //PositionSensorEncoder encoder(4096, 0, 21); 
 
 int count = 0;
-void commutate(void){
-       
-       count ++;
-
-       //pc.printf("%f\n\r", controller.theta_elec);
-        //Get rotor angle
-       //spi.GetMechPosition();
-       controller.i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset);    //Calculate phase currents from ADC readings
-       controller.i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset);
-       controller.i_a = -controller.i_b - controller.i_c;
-       
-       
-       dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q);    //dq0 transform on currents
-       
-       ///Control Law///
-       float i_d_error = controller.i_d_ref - controller.i_d;
-       float i_q_error = controller.i_q_ref - controller.i_q;
-       float v_d_ff = controller.i_d_ref*R_TOTAL;   //feed-forward voltage
-       float v_q_ff = controller.i_q_ref*R_TOTAL;
-       controller.d_int += i_d_error;   
-       controller.q_int += i_q_error;
-       
-       limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q));
-       //controller.d_int = fminf(fmaxf(controller.d_int, -D_INT_LIM), D_INT_LIM);
-       //controller.q_int = fminf(fmaxf(controller.q_int, -Q_INT_LIM), Q_INT_LIM);
-       
-       
-       controller.v_d = K_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff;  
-       controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff; 
-       //controller.v_d = 10*v_d_ff;
-       //controller.v_q = 10*v_q_ff; 
-       limit_norm(&controller.v_d, &controller.v_q, controller.v_bus);
-       
-       abc(controller.theta_elec, controller.v_d, controller.v_q, &controller.v_u, &controller.v_v, &controller.v_w); //inverse dq0 transform on voltages
-       svm(controller.v_bus, controller.v_u, controller.v_v, controller.v_w, &controller.dtc_u, &controller.dtc_v, &controller.dtc_w); //space vector modulation
-
-       gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
-       gpio.pwm_v->write(1.0f-controller.dtc_v);
-       gpio.pwm_w->write(1.0f-controller.dtc_w);  
-       
-       controller.theta_elec = spi.GetElecPosition();   
-       //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u);  //write duty cycles
-       //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v);
-       //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w);
-
-       //gpio.pwm_u->write(1.0f - .1f);  //write duty cycles
-       //gpio.pwm_v->write(1.0f - .1f);
-       //gpio.pwm_w->write(1.0f - .15f);
-
-
-       if(count >1000){
-           controller.i_q_ref = -controller.i_q_ref;
-           count  = 0;
-           //pc.printf("%f\n\r", controller.theta_elec);
-           //pc.printf("%f    %f    %f\n\r", controller.i_a, controller.i_b, controller.i_c);
-           //pc.printf("%f    %f\n\r", controller.i_d, controller.i_q);
-           //pc.printf("%d    %d\n\r", controller.adc1_raw, controller.adc2_raw);
-            }
-       }
        
 
 // Current Sampling IRQ
-extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
+volatile float dtc1 = 0;
+volatile float dtc2 = 0;
+volatile float dtc3 = 0;
+extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
   if (TIM1->SR & TIM_SR_UIF ) {
-        //toggle = 1;
-        ADC1->CR2  |= 0x40000000;  
-        //volatile int delay;
-        //for (delay = 0; delay < 55; delay++);
+        
+        GPIOC->ODR ^= (1 << 4);
+        
+        ADC1->CR  |= ADC_CR_ADSTART;  
+        volatile int eoc;
+        while(!eoc){
+            eoc = ADC1->ISR & ADC_ISR_EOC;
+            }
+        
+        controller.adc1_raw = ADC1->DR;
+        controller.adc2_raw = ADC2->DR;
         
-        controller.adc2_raw = ADC2->DR;
-        controller.adc1_raw = ADC1->DR;
-        //toggle = 0;
-        commutate();        
+        //spi.Sample();
+        //controller.dtheta_mech = spi.GetMechVelocity();
+        //controller.theta_elec = spi.GetElecPosition();
+        
+        commutate(&controller, &gpio, controller.theta_elec);   
+        //TIM1->CCR1 = 0x708*(dtc3);
+        //TIM1->CCR2 = 0x708*(dtc2);
+        //TIM1->CCR3 = 0x708*(dtc1);
+        GPIOC->ODR ^= (1 << 4);
+        
+        //gpio.pwm_u->write(1.0f);  //write duty cycles
+        
+
          
       }
   TIM1->SR = 0x0; // reset the status register
 }
 
        
+void enter_torque_mode(void){
+    
+    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    controller.i_d_ref = 0;
+    controller.i_q_ref = 0;
+    reset_foc(&controller);
+    gpio.enable->write(1);
+    }
+    
        
 int main() {
 
+
     controller.v_bus = V_BUS;
-    spi.ZeroPosition();
+    //spi.ZeroPosition();
     Init_All_HW(&gpio);
 
     wait(.1);
@@ -176,19 +150,20 @@
     gpio.pwm_v->write(1.0f);
     gpio.pwm_w->write(1.0f);
     zero_current(&controller.adc1_offset, &controller.adc2_offset);
+    //gpio.enable->write(0);
     reset_foc(&controller);
-    TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
+    
+    //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
     gpio.enable->write(1);
-       //gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
-       //gpio.pwm_v->write(1.0f - .05f);
-       //gpio.pwm_w->write(1.0f - .1f);
+    gpio.pwm_u->write(1.0f - .05f);  //write duty cycles
+    gpio.pwm_v->write(1.0f - .05f);
+    gpio.pwm_w->write(1.0f - .1f);
     
     wait(.1);
-    NVIC_SetPriority(TIM5_IRQn, 2);
     //loop.attach(&commutate, .000025);
-    can.frequency(1000000);                     // set bit rate to 1Mbps
-    can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
-    can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
+    //can.frequency(1000000);                     // set bit rate to 1Mbps
+    //can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
+    //can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
     
     pc.baud(921600);
     wait(.01);
@@ -196,9 +171,12 @@
     pc.printf("ADC1 Offset: %d    ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
     wait(.01);
     
-
-       controller.i_d_ref = 0;
-       controller.i_q_ref = 0;
+    
+    enter_torque_mode();
+    //enter_zeroing_mode();
+    
+    GPIOC->ODR = 0;
+    
     while(1) {
 
     }