File content as of revision 0:92d18e011d98:

#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;
    }