Ben Katz / rubix_controller

servodisc goodness

Dependencies:   mbed-dev-f303

main.cpp@0:92d18e011d98, 2017-10-09 (annotated)

Committer:
benkatz
Date:
Mon Oct 09 22:35:15 2017 +0000
Revision:
0:92d18e011d98
Child:
1:27b535673eed
1st commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:92d18e011d98 1 #include "mbed.h"
benkatz 0:92d18e011d98 2
benkatz 0:92d18e011d98 3 #define PWM_ARR 0x3E8
benkatz 0:92d18e011d98 4 #define DT 0.05f
benkatz 0:92d18e011d98 5 #define TICKS 8000.0f
benkatz 0:92d18e011d98 6 #define J 0.00015f
benkatz 0:92d18e011d98 7 #define KT 0.087f
benkatz 0:92d18e011d98 8 #define R 0.85f
benkatz 0:92d18e011d98 9 #define V 40.0f
benkatz 0:92d18e011d98 10 #define K_SAT 40.0f
benkatz 0:92d18e011d98 11 #define DTC_MAX
benkatz 0:92d18e011d98 12
benkatz 0:92d18e011d98 13 #define TICKSTORAD(x) TICKS*x
benkatz 0:92d18e011d98 14 #define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x)))
benkatz 0:92d18e011d98 15
benkatz 0:92d18e011d98 16
benkatz 0:92d18e011d98 17 Serial motorDriver(PB_6, PB_7);
benkatz 0:92d18e011d98 18 //Ticker loop;
benkatz 0:92d18e011d98 19
benkatz 0:92d18e011d98 20
benkatz 0:92d18e011d98 21
benkatz 0:92d18e011d98 22 void Control();
benkatz 0:92d18e011d98 23 void InitEncoder();
benkatz 0:92d18e011d98 24 void InitPWM();
benkatz 0:92d18e011d98 25 void WriteVoltage( float v);
benkatz 0:92d18e011d98 26
benkatz 0:92d18e011d98 27
benkatz 0:92d18e011d98 28
benkatz 0:92d18e011d98 29 /* Control Variables */
benkatz 0:92d18e011d98 30 int q_raw;
benkatz 0:92d18e011d98 31 float q, q_old, dq, u, e, q_ref;
benkatz 0:92d18e011d98 32 int count = 0;
benkatz 0:92d18e011d98 33
benkatz 0:92d18e011d98 34 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
benkatz 0:92d18e011d98 35 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 0:92d18e011d98 36 }
benkatz 0:92d18e011d98 37 count++;
benkatz 0:92d18e011d98 38 Control();
benkatz 0:92d18e011d98 39 if(count > 1000){
benkatz 0:92d18e011d98 40 printf("%d\n\r", TIM2->CNT);
benkatz 0:92d18e011d98 41 count = 0;
benkatz 0:92d18e011d98 42 }
benkatz 0:92d18e011d98 43 TIM1->SR = 0x0; // reset the status register
benkatz 0:92d18e011d98 44 }
benkatz 0:92d18e011d98 45
benkatz 0:92d18e011d98 46 /* Main Loop */
benkatz 0:92d18e011d98 47 int main() {
benkatz 0:92d18e011d98 48 InitEncoder();
benkatz 0:92d18e011d98 49 InitPWM();
benkatz 0:92d18e011d98 50 while(1) {
benkatz 0:92d18e011d98 51 }
benkatz 0:92d18e011d98 52 }
benkatz 0:92d18e011d98 53
benkatz 0:92d18e011d98 54 void Control(void){
benkatz 0:92d18e011d98 55 // control loop goes here //
benkatz 0:92d18e011d98 56 q_raw = TIM2->CNT;
benkatz 0:92d18e011d98 57 //printf("%d\n\r", q_raw);
benkatz 0:92d18e011d98 58 q = TICKSTORAD(q_raw);
benkatz 0:92d18e011d98 59 dq = (q - q_old)/DT;
benkatz 0:92d18e011d98 60 q_old = q;
benkatz 0:92d18e011d98 61 e = K_SAT*((q_ref - q) + (-abs(dq)*dq*1.0f*R*J)/(2.0f*KT*(-V - KT*abs(dq))));
benkatz 0:92d18e011d98 62 u = CONSTRAIN(e, -V, V);
benkatz 0:92d18e011d98 63 WriteVoltage(u);
benkatz 0:92d18e011d98 64 TIM1->CCR1 = TIM2->CNT>>9;
benkatz 0:92d18e011d98 65 TIM1->CCR2 = (TIM2->CNT)>>8;
benkatz 0:92d18e011d98 66 }
benkatz 0:92d18e011d98 67
benkatz 0:92d18e011d98 68 void WriteVoltage(float v){
benkatz 0:92d18e011d98 69
benkatz 0:92d18e011d98 70 }
benkatz 0:92d18e011d98 71
benkatz 0:92d18e011d98 72 void InitEncoder(void) {
benkatz 0:92d18e011d98 73 // configure GPIO PA0 & PA1 as inputs for Encoder
benkatz 0:92d18e011d98 74 RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA
benkatz 0:92d18e011d98 75 //RCC->APB1ENR |= 0x00000001; // Enable clock for GPIOA
benkatz 0:92d18e011d98 76 GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; //PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */
benkatz 0:92d18e011d98 77 GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; //PA0 & PA1 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */
benkatz 0:92d18e011d98 78 GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */
benkatz 0:92d18e011d98 79 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */
benkatz 0:92d18e011d98 80 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
benkatz 0:92d18e011d98 81 GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
benkatz 0:92d18e011d98 82 // configure TIM2 as Encoder input
benkatz 0:92d18e011d98 83 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
benkatz 0:92d18e011d98 84 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1
benkatz 0:92d18e011d98 85 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
benkatz 0:92d18e011d98 86 TIM2->CCMR1 = 0x5151; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
benkatz 0:92d18e011d98 87 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
benkatz 0:92d18e011d98 88 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
benkatz 0:92d18e011d98 89 TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
benkatz 0:92d18e011d98 90 TIM2->CNT = 0x0000; //reset the counter before we use it
benkatz 0:92d18e011d98 91 }
benkatz 0:92d18e011d98 92
benkatz 0:92d18e011d98 93 void InitPWM(void){
benkatz 0:92d18e011d98 94
benkatz 0:92d18e011d98 95 RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA
benkatz 0:92d18e011d98 96 RCC->AHBENR |= RCC_AHBENR_GPIOBEN; // enable the clock to GPIOB
benkatz 0:92d18e011d98 97 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
benkatz 0:92d18e011d98 98
benkatz 0:92d18e011d98 99 GPIOA->MODER |= GPIO_MODER_MODER7_1 | GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 ; //PA_7, PA_8, PA_9 to alternate funtion mode
benkatz 0:92d18e011d98 100 GPIOB->MODER |= GPIO_MODER_MODER0_1; // PB_0 to alternate function mode
benkatz 0:92d18e011d98 101 GPIOA->AFR[0] |= 0x60000000; // PA_7 to alternate function 6
benkatz 0:92d18e011d98 102 GPIOA->AFR[1] |= 0x00000066; // PA_8, PA_9 to alternate function 6
benkatz 0:92d18e011d98 103 GPIOB->AFR[0] |= 0x00000006; // PB_0 to alternate function 6
benkatz 0:92d18e011d98 104
benkatz 0:92d18e011d98 105 //PWM Setup
benkatz 0:92d18e011d98 106 TIM1->CCMR1 |= 0x6060; // Enable output compare 1 and 2
benkatz 0:92d18e011d98 107 TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC2E; // enable outputs 1, 2, and complementary outputs
benkatz 0:92d18e011d98 108 TIM1->BDTR |= TIM_BDTR_MOE | 0xF; // MOE = 1 | set dead-time
benkatz 0:92d18e011d98 109 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
benkatz 0:92d18e011d98 110 TIM1->ARR = PWM_ARR; // set auto reload, 40 khz
benkatz 0:92d18e011d98 111 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
benkatz 0:92d18e011d98 112 TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1
benkatz 0:92d18e011d98 113
benkatz 0:92d18e011d98 114 NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); //Enable TIM1 IRQ
benkatz 0:92d18e011d98 115
benkatz 0:92d18e011d98 116 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
benkatz 0:92d18e011d98 117 TIM1->CR1 |= 0x40;//CMS = 10, interrupt only when counting up
benkatz 0:92d18e011d98 118 TIM1->RCR |= 0x001; // update event once per up/down count of tim1
benkatz 0:92d18e011d98 119 TIM1->EGR |= TIM_EGR_UG;
benkatz 0:92d18e011d98 120 }