File content as of revision 1:27b535673eed:

#include "mbed.h"

#define PI              3.14159265f
#define PWM_ARR         0x2E8               // PWM timer auto-reload 
#define DT              0.00002067f         // PWM_ARR/36 MHz
#define CPR             8000.0f             // Encoder counts/revolution
#define J               0.00015f            // Inertia
#define KT              0.087f              // Torque Constant
#define R               0.85f               // Resistance
#define V_IN            40.0f               // DC input voltage
#define K_SAT           40.0f               // Controller saturation gain
#define DTC_MAX         0.95f               // Max duty cycle (limited by bootstrapping)
#define V               V_IN*DTC_MAX        // Max useable voltage

#define TICKSTORAD(x)           x*2*PI/CPR  
#define CONSTRAIN(x,min,max)    ((x)<(min)?(min):((x)>(max)?(max):(x)))

Serial pc (PA_2, PA_3);                     // Serial to programming header
Serial io(PB_6, PB_7);                      // Differential Serial to JST Header
DigitalIn id_1(PB_3);                       // ID Setting Jumpers
DigitalIn id_2(PB_4);
DigitalIn id_3(PB_5);
DigitalOut led(PA_15);                      // Debug LED
DigitalIn d_in(PA_4);                       // LED on input from AND Board
DigitalOut d_out(PA_5);                     // LED on output to AND Board


void Control();
void InitEncoder();
void InitPWM();
void InitGPIO();
void WriteVoltage( float v);
int GetID();



/* Control Variables */
int id;
int q_raw;
float q, q_old, dq, u, e, q_ref;
int count = 0;

/* PWM Timer Interrupt */
extern "C" void TIM1_UP_TIM16_IRQHandler(void) {    
  if (TIM1->SR & TIM_SR_UIF ) {
      }
      count++;
      Control();
      if(count > 5000){
          io.printf("derp\n\r");
          pc.printf("derp\n\r");
          led = !led;
          d_out = !d_out;
          count = 0;
          }
  TIM1->SR = 0x0; // reset the status register
}

/* Main Loop */
int main() {
    pc.printf("\n\r Rubix Controller\n\r");
    id = GetID();
    pc.printf(" Motor ID:  %d\n\r", id);
    
    io.baud(921600);
    
    id_1.mode(PullUp);
    id_2.mode(PullUp);
    id_3.mode(PullUp);
    d_in.mode(PullDown);
    led = 1;
    d_out = 1;
    
    InitEncoder();
    InitPWM();
    wait(.1);

    while(1) {
    }
}

/* Position Control */
void Control(void){
    q_raw = TIM2->CNT;
    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))));   // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response
    u = CONSTRAIN(e, -V, V);
    //WriteVoltage(u);
    WriteVoltage(10.0f);
    }
    
/* Set motor voltage */
void WriteVoltage(float v){
    if(v>0){
        TIM1->CCR1 = 0;
        TIM1->CCR2 = (int) (PWM_ARR*(v/V));
        }
    else if(v<0){
        TIM1->CCR2 = 0;
        TIM1->CCR1 = (int) (PWM_ARR*(v/V));
        }
    }

/* Read ID Jumpers */
int GetID(void){
    int i1 = !id_1.read();
    int i2 = !id_2.read();
    int i3 = !id_3.read();
    return (i1<<2) | (i2<<1) | i3;
    }

/* Initialize Encoder */
void InitEncoder(void) {
    pc.printf("Initializing Encoder\n\r");
    // configure GPIO PA0 & PA1 as inputs for Encoder
    RCC->AHBENR |= RCC_AHBENR_GPIOAEN;                                      // enable the clock to GPIOA
    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           // PA0 & PA1 as Alternate Function 
    GPIOA->OTYPER  |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ;                 // PA0 & PA1 as Inputs 
    GPIOA->OSPEEDR |= 0x00000011;                                           // GPIO Speed
    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ;           // Pull Down   
    GPIOA->AFR[0]  |= 0x00000011 ;                                          //  AF01 for PA0 & PA1 
    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  
    
    // configure TIM2 as Encoder input
    RCC->APB1ENR |= 0x00000001;                             // Enable clock for TIM2
    TIM2->CR1   = 0x0001;                                   // CEN(Counter Enable)='1'   
    TIM2->SMCR  = 0x0003;                                   // SMS='011' (Encoder mode 3)   
    TIM2->CCMR1 = 0x5151;                                   // CC1S='01' CC2S='01'          
    TIM2->CCMR2 = 0x0000;                                                                 
    TIM2->CCER  = 0x0011;                                   // CC1P CC2P                    
    TIM2->PSC   = 0x0000;                                   // Prescaler = (0+1)            
    TIM2->CNT = 0x0000;                                     //reset the counter before we use it  
}

/* Initialize PWM */
void InitPWM(void){
    pc.printf("Initializing PWM\n\r");
    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
    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;                                
    }