Ben Katz / rubix_controller

servodisc goodness

Dependencies:   mbed-dev-f303

main.cpp@1:27b535673eed, 2017-10-27 (annotated)

Committer:
benkatz
Date:
Fri Oct 27 19:21:20 2017 +0000
Revision:
1:27b535673eed
Parent:
0:92d18e011d98
Child:
3:2e9713c61c2d
hardware seems to work;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:92d18e011d98 1 #include "mbed.h"
benkatz 0:92d18e011d98 2
benkatz 1:27b535673eed 3 #define PI 3.14159265f
benkatz 1:27b535673eed 4 #define PWM_ARR 0x2E8 // PWM timer auto-reload
benkatz 1:27b535673eed 5 #define DT 0.00002067f // PWM_ARR/36 MHz
benkatz 1:27b535673eed 6 #define CPR 8000.0f // Encoder counts/revolution
benkatz 1:27b535673eed 7 #define J 0.00015f // Inertia
benkatz 1:27b535673eed 8 #define KT 0.087f // Torque Constant
benkatz 1:27b535673eed 9 #define R 0.85f // Resistance
benkatz 1:27b535673eed 10 #define V_IN 40.0f // DC input voltage
benkatz 1:27b535673eed 11 #define K_SAT 40.0f // Controller saturation gain
benkatz 1:27b535673eed 12 #define DTC_MAX 0.95f // Max duty cycle (limited by bootstrapping)
benkatz 1:27b535673eed 13 #define V V_IN*DTC_MAX // Max useable voltage
benkatz 0:92d18e011d98 14
benkatz 1:27b535673eed 15 #define TICKSTORAD(x) x*2*PI/CPR
benkatz 0:92d18e011d98 16 #define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x)))
benkatz 0:92d18e011d98 17
benkatz 1:27b535673eed 18 Serial pc (PA_2, PA_3); // Serial to programming header
benkatz 1:27b535673eed 19 Serial io(PB_6, PB_7); // Differential Serial to JST Header
benkatz 1:27b535673eed 20 DigitalIn id_1(PB_3); // ID Setting Jumpers
benkatz 1:27b535673eed 21 DigitalIn id_2(PB_4);
benkatz 1:27b535673eed 22 DigitalIn id_3(PB_5);
benkatz 1:27b535673eed 23 DigitalOut led(PA_15); // Debug LED
benkatz 1:27b535673eed 24 DigitalIn d_in(PA_4); // LED on input from AND Board
benkatz 1:27b535673eed 25 DigitalOut d_out(PA_5); // LED on output to AND Board
benkatz 0:92d18e011d98 26
benkatz 0:92d18e011d98 27
benkatz 0:92d18e011d98 28 void Control();
benkatz 0:92d18e011d98 29 void InitEncoder();
benkatz 0:92d18e011d98 30 void InitPWM();
benkatz 1:27b535673eed 31 void InitGPIO();
benkatz 0:92d18e011d98 32 void WriteVoltage( float v);
benkatz 1:27b535673eed 33 int GetID();
benkatz 0:92d18e011d98 34
benkatz 0:92d18e011d98 35
benkatz 0:92d18e011d98 36
benkatz 0:92d18e011d98 37 /* Control Variables */
benkatz 1:27b535673eed 38 int id;
benkatz 0:92d18e011d98 39 int q_raw;
benkatz 0:92d18e011d98 40 float q, q_old, dq, u, e, q_ref;
benkatz 0:92d18e011d98 41 int count = 0;
benkatz 0:92d18e011d98 42
benkatz 1:27b535673eed 43 /* PWM Timer Interrupt */
benkatz 1:27b535673eed 44 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
benkatz 0:92d18e011d98 45 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 0:92d18e011d98 46 }
benkatz 0:92d18e011d98 47 count++;
benkatz 0:92d18e011d98 48 Control();
benkatz 1:27b535673eed 49 if(count > 5000){
benkatz 1:27b535673eed 50 io.printf("derp\n\r");
benkatz 1:27b535673eed 51 pc.printf("derp\n\r");
benkatz 1:27b535673eed 52 led = !led;
benkatz 1:27b535673eed 53 d_out = !d_out;
benkatz 0:92d18e011d98 54 count = 0;
benkatz 0:92d18e011d98 55 }
benkatz 0:92d18e011d98 56 TIM1->SR = 0x0; // reset the status register
benkatz 0:92d18e011d98 57 }
benkatz 0:92d18e011d98 58
benkatz 0:92d18e011d98 59 /* Main Loop */
benkatz 0:92d18e011d98 60 int main() {
benkatz 1:27b535673eed 61 pc.printf("\n\r Rubix Controller\n\r");
benkatz 1:27b535673eed 62 id = GetID();
benkatz 1:27b535673eed 63 pc.printf(" Motor ID: %d\n\r", id);
benkatz 1:27b535673eed 64
benkatz 1:27b535673eed 65 io.baud(921600);
benkatz 1:27b535673eed 66
benkatz 1:27b535673eed 67 id_1.mode(PullUp);
benkatz 1:27b535673eed 68 id_2.mode(PullUp);
benkatz 1:27b535673eed 69 id_3.mode(PullUp);
benkatz 1:27b535673eed 70 d_in.mode(PullDown);
benkatz 1:27b535673eed 71 led = 1;
benkatz 1:27b535673eed 72 d_out = 1;
benkatz 1:27b535673eed 73
benkatz 0:92d18e011d98 74 InitEncoder();
benkatz 0:92d18e011d98 75 InitPWM();
benkatz 1:27b535673eed 76 wait(.1);
benkatz 1:27b535673eed 77
benkatz 0:92d18e011d98 78 while(1) {
benkatz 0:92d18e011d98 79 }
benkatz 0:92d18e011d98 80 }
benkatz 0:92d18e011d98 81
benkatz 1:27b535673eed 82 /* Position Control */
benkatz 0:92d18e011d98 83 void Control(void){
benkatz 0:92d18e011d98 84 q_raw = TIM2->CNT;
benkatz 0:92d18e011d98 85 q = TICKSTORAD(q_raw);
benkatz 0:92d18e011d98 86 dq = (q - q_old)/DT;
benkatz 0:92d18e011d98 87 q_old = q;
benkatz 1:27b535673eed 88 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
benkatz 0:92d18e011d98 89 u = CONSTRAIN(e, -V, V);
benkatz 1:27b535673eed 90 //WriteVoltage(u);
benkatz 1:27b535673eed 91 WriteVoltage(10.0f);
benkatz 1:27b535673eed 92 }
benkatz 1:27b535673eed 93
benkatz 1:27b535673eed 94 /* Set motor voltage */
benkatz 1:27b535673eed 95 void WriteVoltage(float v){
benkatz 1:27b535673eed 96 if(v>0){
benkatz 1:27b535673eed 97 TIM1->CCR1 = 0;
benkatz 1:27b535673eed 98 TIM1->CCR2 = (int) (PWM_ARR*(v/V));
benkatz 1:27b535673eed 99 }
benkatz 1:27b535673eed 100 else if(v<0){
benkatz 1:27b535673eed 101 TIM1->CCR2 = 0;
benkatz 1:27b535673eed 102 TIM1->CCR1 = (int) (PWM_ARR*(v/V));
benkatz 1:27b535673eed 103 }
benkatz 0:92d18e011d98 104 }
benkatz 0:92d18e011d98 105
benkatz 1:27b535673eed 106 /* Read ID Jumpers */
benkatz 1:27b535673eed 107 int GetID(void){
benkatz 1:27b535673eed 108 int i1 = !id_1.read();
benkatz 1:27b535673eed 109 int i2 = !id_2.read();
benkatz 1:27b535673eed 110 int i3 = !id_3.read();
benkatz 1:27b535673eed 111 return (i1<<2) | (i2<<1) | i3;
benkatz 0:92d18e011d98 112 }
benkatz 0:92d18e011d98 113
benkatz 1:27b535673eed 114 /* Initialize Encoder */
benkatz 0:92d18e011d98 115 void InitEncoder(void) {
benkatz 1:27b535673eed 116 pc.printf("Initializing Encoder\n\r");
benkatz 0:92d18e011d98 117 // configure GPIO PA0 & PA1 as inputs for Encoder
benkatz 1:27b535673eed 118 RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA
benkatz 1:27b535673eed 119 GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; // PA0 & PA1 as Alternate Function
benkatz 1:27b535673eed 120 GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; // PA0 & PA1 as Inputs
benkatz 1:27b535673eed 121 GPIOA->OSPEEDR |= 0x00000011; // GPIO Speed
benkatz 1:27b535673eed 122 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down
benkatz 1:27b535673eed 123 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1
benkatz 1:27b535673eed 124 GPIOA->AFR[1] |= 0x00000000 ; //
benkatz 1:27b535673eed 125
benkatz 0:92d18e011d98 126 // configure TIM2 as Encoder input
benkatz 1:27b535673eed 127 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
benkatz 1:27b535673eed 128 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1'
benkatz 1:27b535673eed 129 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3)
benkatz 1:27b535673eed 130 TIM2->CCMR1 = 0x5151; // CC1S='01' CC2S='01'
benkatz 1:27b535673eed 131 TIM2->CCMR2 = 0x0000;
benkatz 1:27b535673eed 132 TIM2->CCER = 0x0011; // CC1P CC2P
benkatz 1:27b535673eed 133 TIM2->PSC = 0x0000; // Prescaler = (0+1)
benkatz 1:27b535673eed 134 TIM2->CNT = 0x0000; //reset the counter before we use it
benkatz 0:92d18e011d98 135 }
benkatz 0:92d18e011d98 136
benkatz 1:27b535673eed 137 /* Initialize PWM */
benkatz 0:92d18e011d98 138 void InitPWM(void){
benkatz 1:27b535673eed 139 pc.printf("Initializing PWM\n\r");
benkatz 1:27b535673eed 140 RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA
benkatz 1:27b535673eed 141 RCC->AHBENR |= RCC_AHBENR_GPIOBEN; // enable the clock to GPIOB
benkatz 1:27b535673eed 142 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
benkatz 0:92d18e011d98 143
benkatz 0:92d18e011d98 144 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 145 GPIOB->MODER |= GPIO_MODER_MODER0_1; // PB_0 to alternate function mode
benkatz 0:92d18e011d98 146 GPIOA->AFR[0] |= 0x60000000; // PA_7 to alternate function 6
benkatz 0:92d18e011d98 147 GPIOA->AFR[1] |= 0x00000066; // PA_8, PA_9 to alternate function 6
benkatz 0:92d18e011d98 148 GPIOB->AFR[0] |= 0x00000006; // PB_0 to alternate function 6
benkatz 0:92d18e011d98 149
benkatz 0:92d18e011d98 150 //PWM Setup
benkatz 0:92d18e011d98 151 TIM1->CCMR1 |= 0x6060; // Enable output compare 1 and 2
benkatz 0:92d18e011d98 152 TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC2E; // enable outputs 1, 2, and complementary outputs
benkatz 1:27b535673eed 153 TIM1->BDTR |= TIM_BDTR_MOE | 0xF; // MOE = 1 | set dead-time
benkatz 1:27b535673eed 154 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
benkatz 1:27b535673eed 155 TIM1->ARR = PWM_ARR; // set auto reload
benkatz 1:27b535673eed 156 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
benkatz 1:27b535673eed 157 TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1
benkatz 0:92d18e011d98 158
benkatz 1:27b535673eed 159 NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); //Enable TIM1 IRQ
benkatz 0:92d18e011d98 160
benkatz 1:27b535673eed 161 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
benkatz 1:27b535673eed 162 TIM1->CR1 |= 0x40; //CMS = 10, interrupt only when counting up
benkatz 1:27b535673eed 163 TIM1->RCR |= 0x001; // update event once per up/down count of tim1
benkatz 1:27b535673eed 164 TIM1->EGR |= TIM_EGR_UG;
benkatz 0:92d18e011d98 165 }