Ben Katz
servodisc goodness
Diff: main.cpp
- Revision:
- 3:2e9713c61c2d
- Parent:
- 1:27b535673eed
- Child:
- 4:6e290eb553cd
--- a/main.cpp Fri Oct 27 19:34:31 2017 +0000
+++ b/main.cpp Wed Dec 13 05:04:38 2017 +0000
@@ -4,15 +4,15 @@
#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 J 0.000065f // Inertia
+#define KT 0.0678f // 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_IN 20.0f // DC input voltage
+#define K_SAT 22000.0f // Controller saturation gain
+#define DTC_MAX 0.97f // Max duty cycle (limited by bootstrapping)
#define V V_IN*DTC_MAX // Max useable voltage
-#define TICKSTORAD(x) x*2*PI/CPR
+#define TICKSTORAD(x) (float)x*2.0f*PI/CPR
#define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x)))
Serial pc (PA_2, PA_3); // Serial to programming header
@@ -31,64 +31,182 @@
void InitGPIO();
void WriteVoltage( float v);
int GetID();
+void SerialISR(void);
/* Control Variables */
int id;
-int q_raw;
-float q, q_old, dq, u, e, q_ref;
-int count = 0;
+int q_raw, dir, dq_raw = 0;
+float q, q_old, dq, u, e, q_ref, dqdebug = 0;
+int count, count2;
+int controlmode =0;
+
+/* Kalman Filter Variables */
+float q_est[2] = {0.0f};
+float q_meas[2] = {0.0f};
+float F[2][2] = {{1.0f, DT},{0.0f, 1.0f}};
+float B[2] = {0.0f, DT/J};
+float P[2][2] = {0};
+float Q[2] = {1.0f, 0.01f};
+float Rk[2] = {0.01, 10};
+float S[2][2] = {0};
+float Y[2] = {0};
+float K[2][2] = {0};
+float U;
+
+
+int16_t log_vec[2500] = {0};
+//int16_t log_vec_2[1250] = {0};
+
/* PWM Timer Interrupt */
extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
}
count++;
+
+ if(count>1000 && count<2000){
+ q_ref = 1.57f;
+ //ref = 18000.0f;
+ }
+
+ if(count>2000 && count<3000){
+ q_ref = 0.0f;
+ //ref = 0;
+ //count = 0;
+ }
+ if(count>3000 && count<4000){
+ q_ref = -1.57f;
+ }
+
+ if(count>4500){
+ controlmode = 1;
+ }
+ if(count<5000){
+ //log_vec_2[count/4] = (int)(q_est[1]*10.0f);
+ log_vec[count/2] = (int)(q*1000.0f);
+ }
+
+ if(count>20000 && count<22500){
+ //printf("%d\n\r", log_vec[count2]);
+ //wait_us(200);
+ //printf("%d\n\r", log_vec_2[count2]);
+ //wait_us(200);
+ count2++;
+ }
+
+
Control();
+ /*
if(count > 5000){
- io.printf("derp\n\r");
- pc.printf("derp\n\r");
- led = !led;
- d_out = !d_out;
+ //io.printf("derp\n\r");
+ //pc.printf("derp\n\r");
+ //pc.printf("%d %f\n\r", q_raw, e);
+ printf("%f %f\n\r", dq, dqdebug);
+ //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);
+ pc.baud(921600);
io.baud(921600);
-
+
+ //pc.printf("\n\r Rubix Controller\n\r");
id_1.mode(PullUp);
id_2.mode(PullUp);
id_3.mode(PullUp);
- d_in.mode(PullDown);
+ id = GetID();
+ pc.printf(" Motor ID: %d\n\r", id);
+
+ //d_in.mode(PullDown);
led = 1;
d_out = 1;
+ //wait(.1);
- InitEncoder();
InitPWM();
- wait(.1);
-
+ InitEncoder();
+ //pc.printf("Initializing Encoder\n\r");
+ //pc.printf("Initializing PWM\n\r");
+ //wait(.1);
while(1) {
}
}
/* Position Control */
void Control(void){
- q_raw = TIM2->CNT;
+
+ // Sample Position and Velocity //
+ q_raw = TIM2->CNT;
+ dir = -2*(((TIM2->CR1)>>4)&1)+1;
+ dq_raw = dir*(TIM15->CCR1);
q = TICKSTORAD(q_raw);
- dq = (q - q_old)/DT;
+ //dq = (q - q_old)/DT;
+ dq = (18000000.0f*4.0f*2.0f*PI/CPR)/((float)dq_raw);
+ if(isinf(dq)){ dq = 0.0f;}
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
+
+ q_meas[0] = q;
+ q_meas[1] = dq;
+
+ // Kalman Filter //
+ // Update Model //
+
+ q_est[0] += q_est[1]*F[0][1];
+ q_est[1] += B[1]*U;
+
+
+ P[0][0] += Q[0] + DT*P[1][0] + DT*(P[0][1] + DT*P[1][1]);
+ P[0][1] += DT*P[1][1];
+ P[1][0] += DT*P[1][1];
+ P[1][1] += Q[1];
+
+ //Calculate Kalman Gains//
+ S[0][0] = P[0][0] + Rk[0];
+ S[0][1] = P[0][1];
+ S[1][0] = P[1][0];
+ S[1][1] = P[1][1] + Rk[1];
+ float denom = (S[0][0]*S[1][1] - S[0][1]*S[1][0]);
+ K[0][0] = (P[0][0]*S[1][1])/denom - (P[0][1]*S[1][0])/denom;
+ K[0][1] = (P[0][1]*S[0][0])/denom - (P[0][0]*S[0][1])/denom;
+ K[1][0] = (P[1][0]*S[1][1])/(S[0][0]*S[1][1] - S[0][1]*S[1][0]) - (P[1][1]*S[1][0])/denom;
+ K[1][1] = (P[1][1]*S[0][0])/(S[0][0]*S[1][1] - S[0][1]*S[1][0]) - (P[1][0]*S[0][1])/denom;
+
+ Y[0] = q_meas[0] - q_est[0];
+ Y[1] = q_meas[1] - q_est[1];
+
+ // Update Estimate //
+ q_est[0] += K[0][0]*Y[0] + K[0][1]*Y[1];
+ q_est[1] += K[1][0]*Y[0] + K[1][1]*Y[1];
+
+ P[0][0] = -K[0][1]*P[1][0] - P[0][0]*(K[0][0] - 1.0f);
+ P[0][1] = -K[0][1]*P[1][1] - P[0][1]*(K[0][0] - 1.0f);
+ P[1][0] = -K[1][0]*P[0][0] - P[1][0]*(K[1][1] - 1.0f);
+ P[1][1] = -K[1][0]*P[0][1] - P[1][1]*(K[1][1] - 1.0f);
+
+
+
+ // Control Law //
+ if(controlmode == 0){
+ e = K_SAT*((q_ref - q) + (abs(dq)*dq*1.3f*R*J)/(2.0f*KT*(-V - KT*abs(dq)))); // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response
+ //e = K_SAT*((q_ref - q) + (abs(q_est[1])*q_est[1]*1.3f*R*J)/(2.0f*KT*(-V - 1.0f*KT*abs(q_est[1])))); // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response
+
+ }
+ //q_ref = 0.0f;
+ if(controlmode == 1){
+ e = 0;
+ //e = 40.0f*(q_ref - q) + .2f*(0.0f-dq);
+ }
u = CONSTRAIN(e, -V, V);
- //WriteVoltage(u);
- WriteVoltage(10.0f);
+ WriteVoltage(u);
+ U = KT*(u - KT*dq)/R;
+ //WriteVoltage(-10.0f);
}
/* Set motor voltage */
@@ -97,12 +215,19 @@
TIM1->CCR1 = 0;
TIM1->CCR2 = (int) (PWM_ARR*(v/V));
}
- else if(v<0){
+ else if(v<=0){
TIM1->CCR2 = 0;
- TIM1->CCR1 = (int) (PWM_ARR*(v/V));
+ TIM1->CCR1 = (int) (PWM_ARR*(abs(v)/V));
}
}
+void SerialISR(void){
+ // PC Serial Interrupt //
+
+
+ }
+
+
/* Read ID Jumpers */
int GetID(void){
int i1 = !id_1.read();
@@ -113,17 +238,20 @@
/* 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->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->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
+ TIM2->DIER = 0x00;
+ TIM2->EGR = 0x0;
+ NVIC_DisableIRQ(TIM2_IRQn);
+
RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1'
TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3)
@@ -132,11 +260,21 @@
TIM2->CCER = 0x0011; // CC1P CC2P
TIM2->PSC = 0x0000; // Prescaler = (0+1)
TIM2->CNT = 0x0000; //reset the counter before we use it
+
+ TIM2->CR2 = 0x030; //MMS = 101
+ __TIM15_CLK_ENABLE();
+ TIM15->PSC = 0x03;
+ TIM15->SMCR = 0x4; //TS = 010 for ITR2, SMS = 100
+ TIM15->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC
+ TIM15->CCER |= TIM_CCER_CC1P;
+ TIM15->CCER |= TIM_CCER_CC1E;
+ TIM15->CR1 = 0x1;
+
}
+
/* 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
@@ -156,10 +294,12 @@
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;
- }
\ No newline at end of file
+ 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.