Diff: main.cpp

Revision:

0:92d18e011d98

Child:

1:27b535673eed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 09 22:35:15 2017 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+
+#define PWM_ARR         0x3E8
+#define DT              0.05f
+#define TICKS           8000.0f
+#define J               0.00015f
+#define KT              0.087f
+#define R               0.85f
+#define V               40.0f
+#define K_SAT            40.0f
+#define DTC_MAX
+
+#define TICKSTORAD(x)           TICKS*x
+#define CONSTRAIN(x,min,max)    ((x)<(min)?(min):((x)>(max)?(max):(x)))
+
+
+Serial motorDriver(PB_6, PB_7);
+//Ticker loop;
+
+
+
+void Control();
+void InitEncoder();
+void InitPWM();
+void WriteVoltage( float v);
+
+
+
+/* Control Variables */
+int q_raw;
+float q, q_old, dq, u, e, q_ref;
+int count = 0;
+
+extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
+  if (TIM1->SR & TIM_SR_UIF ) {
+      }
+      count++;
+      Control();
+      if(count > 1000){
+          printf("%d\n\r", TIM2->CNT);
+          count = 0;
+          }
+  TIM1->SR = 0x0; // reset the status register
+}
+
+/* Main Loop */
+int main() {
+    InitEncoder();
+    InitPWM();
+    while(1) {
+    }
+}
+
+void Control(void){
+    // control loop goes here //
+    q_raw = TIM2->CNT;
+    //printf("%d\n\r", q_raw);
+    q = TICKSTORAD(q_raw);
+    dq = (q - q_old)/DT;
+    q_old = q;
+    e = K_SAT*((q_ref - q) + (-abs(dq)*dq*1.0f*R*J)/(2.0f*KT*(-V - KT*abs(dq))));
+    u = CONSTRAIN(e, -V, V);
+    WriteVoltage(u);
+    TIM1->CCR1 = TIM2->CNT>>9;
+    TIM1->CCR2 = (TIM2->CNT)>>8;
+    }
+
+void WriteVoltage(float v){
+    
+    }
+
+void InitEncoder(void) {
+    // configure GPIO PA0 & PA1 as inputs for Encoder
+    RCC->AHBENR |= RCC_AHBENR_GPIOAEN;                        // enable the clock to GPIOA
+    //RCC->APB1ENR |= 0x00000001;  // Enable clock for GPIOA
+    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           //PA0 & PA1 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ;                 //PA0 & PA1 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
+    GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ;     // Low speed                        /*!< GPIO port output speed register,       Address offset: 0x08      */
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
+    GPIOA->AFR[0]  |= 0x00000011 ;                                          //  AF01 for PA0 & PA1              /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    // configure TIM2 as Encoder input
+    RCC->APB1ENR |= 0x00000001;  // Enable clock for TIM2
+    TIM2->CR1   = 0x0001;       // CEN(Counter Enable)='1'     < TIM control register 1
+    TIM2->SMCR  = 0x0003;       // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM2->CCMR1 = 0x5151;       // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+    TIM2->CCMR2 = 0x0000;       //                             < TIM capture/compare mode register 2
+    TIM2->CCER  = 0x0011;       // CC1P CC2P                   < TIM capture/compare enable register
+    TIM2->PSC   = 0x0000;       // Prescaler = (0+1)           < TIM prescaler
+    TIM2->CNT = 0x0000;         //reset the counter before we use it  
+}
+
+void InitPWM(void){
+    
+    RCC->AHBENR |= RCC_AHBENR_GPIOAEN;                        // enable the clock to GPIOA
+    RCC->AHBENR |= RCC_AHBENR_GPIOBEN;                        // enable the clock to GPIOB
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                        // enable TIM1 clock
+    
+    GPIOA->MODER   |= GPIO_MODER_MODER7_1 | GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 ;     //PA_7, PA_8, PA_9 to alternate funtion mode
+    GPIOB->MODER   |= GPIO_MODER_MODER0_1;                  // PB_0 to alternate function mode
+    GPIOA->AFR[0]    |= 0x60000000;                         // PA_7 to alternate function 6
+    GPIOA->AFR[1]    |= 0x00000066;                         // PA_8, PA_9 to alternate function 6
+    GPIOB->AFR[0]    |= 0x00000006;                         // PB_0 to alternate function 6
+    
+    //PWM Setup
+    TIM1->CCMR1 |= 0x6060;                                  // Enable output compare 1 and 2
+    TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC2E;  // enable outputs 1, 2, and complementary outputs
+    TIM1->BDTR |= TIM_BDTR_MOE | 0xF;                           // MOE = 1 | set dead-time
+    TIM1->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
+    TIM1->ARR = PWM_ARR;                                          // set auto reload, 40 khz
+    TIM1->CR1 |= TIM_CR1_ARPE;                                  // autoreload on, 
+    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM1
+    
+    NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);   //Enable TIM1 IRQ
+
+    TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
+    TIM1->CR1 |= 0x40;//CMS = 10, interrupt only when counting up
+    TIM1->RCR |= 0x001; // update event once per up/down count of tim1 
+    TIM1->EGR |= TIM_EGR_UG;
+    }
\ No newline at end of file