Ben Katz
servodisc goodness
Diff: main.cpp
- Revision:
- 1:27b535673eed
- Parent:
- 0:92d18e011d98
- Child:
- 3:2e9713c61c2d
--- a/main.cpp Mon Oct 09 22:35:15 2017 +0000
+++ b/main.cpp Fri Oct 27 19:21:20 2017 +0000
@@ -1,43 +1,56 @@
#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 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) TICKS*x
+#define TICKSTORAD(x) x*2*PI/CPR
#define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x)))
-
-Serial motorDriver(PB_6, PB_7);
-//Ticker loop;
-
+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;
-extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
+/* PWM Timer Interrupt */
+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);
+ 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
@@ -45,56 +58,88 @@
/* 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){
- // 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))));
+ 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);
- TIM1->CCR1 = TIM2->CNT>>9;
- TIM1->CCR2 = (TIM2->CNT)>>8;
+ //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));
+ }
}
-void WriteVoltage(float 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
- //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 */
+ 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' < 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
+ 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){
-
- 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
+ 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
@@ -105,16 +150,16 @@
//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
+ 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
+ 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;
+ 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
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Created: | 09 Oct 2017 |
| Imports: | 6 |
| Forks: | 0 |
| Commits: | 12 |
| Dependents: | 0 |
| Dependencies: | 1 |
| Followers: | 2 |
This repository is Public (Unlisted).
The code in this repository is MIT licensed.