Ben Katz
servodisc goodness
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "cube.h" 00003 #include "communication.h" 00004 00005 #define PI 3.14159265f 00006 #define PWM_ARR 0x2E8 // PWM timer auto-reload 00007 #define DT 0.00002067f // PWM_ARR/36 MHz 00008 #define CPR 8000.0f // Encoder counts/revolution 00009 #define J 0.000065f // Inertia 00010 #define KT 0.0678f // Torque Constant 00011 #define R 0.85f // Resistance 00012 #define V_IN 60.0f // DC input voltage 00013 #define K_SAT 22000.0f // Controller saturation gain 00014 #define DTC_MAX 0.97f // Max duty cycle (limited by bootstrapping) 00015 #define V V_IN*DTC_MAX // Max useable voltage 00016 00017 #define TICKSTORAD(x) (float)x*2.0f*PI/CPR 00018 #define CONSTRAIN(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x))) 00019 00020 Serial pc (PA_2, PA_3); // Serial to programming header 00021 Serial io(PB_6, PB_7); // Differential Serial to JST Header 00022 DigitalIn id_3(PB_3); // ID Setting Jumpers 00023 DigitalIn id_2(PB_4); 00024 DigitalIn id_1(PB_5); 00025 DigitalOut led(PA_15); // Debug LED 00026 DigitalIn d_in(PA_4); // Input from AND Board 00027 00028 //AnalogOut a_out(PA_5); 00029 DigitalOut d_out(PA_5); // LED on output to AND Board 00030 00031 00032 void Control(); 00033 void InitEncoder(); 00034 void InitPWM(); 00035 void InitGPIO(); 00036 void WriteVoltage( float v); 00037 int ReadID(); 00038 00039 void SerialISR(); 00040 00041 00042 00043 /* Control Variables */ 00044 int id; 00045 int q_raw, dir, dq_raw = 0; 00046 float q, q_old, dq, u, e, q_ref, dqdebug = 0; 00047 int count, count2; 00048 int controlmode = 0; 00049 00050 float i_est; 00051 float i_int; 00052 00053 00054 volatile int run_control = 0; 00055 volatile int position_setpoint = 0; 00056 00057 /* Kalman Filter Variables */ 00058 float q_est[2] = {0.0f}; 00059 float q_meas[2] = {0.0f}; 00060 //float F[2][2] = {{1.0f, DT},{0.0f, 1.0f}}; 00061 //float B[2] = {0.0f, DT/J}; 00062 //float P[2][2] = {0}; 00063 //float Q[2] = {1.0f, 0.01f}; 00064 //float Rk[2] = {0.01, 10}; 00065 //float S[2][2] = {0}; 00066 //float Y[2] = {0}; 00067 //float K[2][2] = {0}; 00068 float U; 00069 00070 00071 //int8_t log_vec[1250] = {0}; 00072 //int16_t log_vec_2[1250] = {0}; 00073 00074 00075 /* PWM Timer Interrupt */ 00076 extern "C" void TIM1_UP_TIM16_IRQHandler(void) { 00077 if (TIM1->SR & TIM_SR_UIF ) { 00078 } 00079 count++; 00080 //d_out = !d_out; 00081 /* 00082 if(count>1000 && count<2000){ 00083 q_ref = 1.57f; 00084 //ref = 18000.0f; 00085 } 00086 00087 if(count>2000 && count<3000){ 00088 q_ref = 0.0f; 00089 //ref = 0; 00090 //count = 0; 00091 } 00092 if(count>3000 && count<4000){ 00093 q_ref = 1.57f; 00094 } 00095 00096 if(count>4500){ 00097 controlmode = 1; 00098 } 00099 00100 if(count<5000){ 00101 //log_vec_2[count/4] = (int)(q_est[1]*10.0f); 00102 log_vec[count/4] = q_raw>>4; 00103 } 00104 00105 if(count>20000 && count<21250){ 00106 printf("%d\n\r", log_vec[count2]); 00107 wait_us(80); 00108 //printf("%d\n\r", log_vec_2[count2]); 00109 //wait_us(200); 00110 count2++; 00111 } 00112 00113 00114 */ 00115 Control(); 00116 00117 /* 00118 if(count > 5000){ 00119 //io.printf("derp\n\r"); 00120 //pc.printf("derp\n\r"); 00121 pc.printf("%d \n\r", q_raw); 00122 //printf("%f %f\n\r", dq, dqdebug); 00123 //d_out = !d_out; 00124 count = 0; 00125 } 00126 */ 00127 00128 //a_out.write(q/2.0f); 00129 00130 TIM1->SR = 0x0; // reset the status register 00131 } 00132 00133 // FUNCTIONS TO MODIFY 00134 int get_board_id() 00135 { 00136 return id; 00137 } 00138 00139 void do_rotation(int8_t num_turns, int8_t derp) 00140 { 00141 printf("[BOARD %d] Rotate %d turns!\r\n",get_board_id(),num_turns); 00142 position_setpoint += num_turns; 00143 q_ref = 0.5f*PI*position_setpoint; 00144 run_control = 1; 00145 while(run_control) 00146 { 00147 ; 00148 } 00149 wait(0.0001f); 00150 printf("done.\r\n"); 00151 } 00152 00153 void set_and_board(int8_t value, int8_t derp) 00154 { 00155 printf("[BOARD %d] Set and board %d\r\n",get_board_id(),value); 00156 d_out = value; 00157 } 00158 00159 int8_t get_and_board() 00160 { 00161 uint8_t value = d_in; 00162 led = value; 00163 //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value); 00164 return value; 00165 } 00166 00167 00168 00169 // BEGIN STATE MACHINE CODE 00170 mbed_info_t state_machine_info; 00171 00172 void state_machine_init() 00173 { 00174 printf("Start state machine!\r\n"); 00175 int board_id = get_board_id(); 00176 00177 // set up state_machine_info 00178 state_machine_info.face = (face_t)board_id; 00179 pc.printf("\tboard id: %d\n",board_id); 00180 // set null sequence to disable everything 00181 state_machine_info.seq = NULL; 00182 state_machine_info.rotate = do_rotation; 00183 state_machine_info.set_and = set_and_board; 00184 state_machine_info.get_and = get_and_board; 00185 // prepare for serial 00186 clear_message_buffer(); 00187 } 00188 00189 void handle_serial(Serial* pc) 00190 { 00191 while(pc->readable()) 00192 { 00193 // read it 00194 uint8_t data = pc->getc(); 00195 if(data == '\n') 00196 receive_move_sequence(pc,&state_machine_info); 00197 } 00198 } 00199 00200 // END STATE MACHINE CODE 00201 00202 /* Main Loop */ 00203 int main() { 00204 00205 pc.baud(921600); 00206 io.baud(115200); 00207 00208 pc.printf("\n\r Rubix Controller\n\r"); 00209 print_sample_sequence_hex(); 00210 id_1.mode(PullUp); 00211 id_2.mode(PullUp); 00212 id_3.mode(PullUp); 00213 id = ReadID(); 00214 pc.printf(" Motor ID: %d\n\r", id); 00215 00216 //d_in.mode(PullDown); 00217 led = 1; 00218 d_out = 0; 00219 //wait(.1); 00220 00221 InitPWM(); 00222 InitEncoder(); 00223 //pc.printf("Initializing Encoder\n\r"); 00224 //pc.printf("Initializing PWM\n\r"); 00225 //wait(.1); 00226 //io.attach(&SerialISR); 00227 00228 reset_mbed(); //MUST call this first thing in main - initializes data structures! 00229 state_machine_init(); 00230 while(1) { 00231 //myled = get_board_id(); 00232 handle_serial(&io); 00233 run_sequence_2(&state_machine_info); // run state machine 00234 } 00235 } 00236 00237 /* Position Control */ 00238 void Control(void){ 00239 00240 // Sample Position and Velocity // 00241 q_raw = TIM2->CNT; 00242 dir = -2*(((TIM2->CR1)>>4)&1)+1; 00243 dq_raw = dir*(TIM15->CCR1); 00244 q = TICKSTORAD(q_raw); 00245 //dq = (q - q_old)/DT; 00246 dq = (18000000.0f*4.0f*2.0f*PI/CPR)/((float)dq_raw); 00247 if(isinf(dq)){ dq = 0.0f;} 00248 q_old = q; 00249 00250 q_meas[0] = q; 00251 q_meas[1] = dq; 00252 00253 // Kalman Filter // 00254 // Update Model // 00255 /* 00256 q_est[0] += q_est[1]*F[0][1]; 00257 q_est[1] += B[1]*U; 00258 00259 00260 P[0][0] += Q[0] + DT*P[1][0] + DT*(P[0][1] + DT*P[1][1]); 00261 P[0][1] += DT*P[1][1]; 00262 P[1][0] += DT*P[1][1]; 00263 P[1][1] += Q[1]; 00264 00265 //Calculate Kalman Gains// 00266 S[0][0] = P[0][0] + Rk[0]; 00267 S[0][1] = P[0][1]; 00268 S[1][0] = P[1][0]; 00269 S[1][1] = P[1][1] + Rk[1]; 00270 float denom = (S[0][0]*S[1][1] - S[0][1]*S[1][0]); 00271 K[0][0] = (P[0][0]*S[1][1])/denom - (P[0][1]*S[1][0])/denom; 00272 K[0][1] = (P[0][1]*S[0][0])/denom - (P[0][0]*S[0][1])/denom; 00273 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; 00274 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; 00275 00276 Y[0] = q_meas[0] - q_est[0]; 00277 Y[1] = q_meas[1] - q_est[1]; 00278 00279 // Update Estimate // 00280 q_est[0] += K[0][0]*Y[0] + K[0][1]*Y[1]; 00281 q_est[1] += K[1][0]*Y[0] + K[1][1]*Y[1]; 00282 00283 P[0][0] = -K[0][1]*P[1][0] - P[0][0]*(K[0][0] - 1.0f); 00284 P[0][1] = -K[0][1]*P[1][1] - P[0][1]*(K[0][0] - 1.0f); 00285 P[1][0] = -K[1][0]*P[0][0] - P[1][0]*(K[1][1] - 1.0f); 00286 P[1][1] = -K[1][0]*P[0][1] - P[1][1]*(K[1][1] - 1.0f); 00287 */ 00288 00289 00290 // Control Law // 00291 if(run_control == 1){ 00292 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 00293 //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 00294 00295 } 00296 00297 //q_ref = 0.0f; 00298 if(run_control == 0){ 00299 //e = 0; 00300 e = 100.0f*(q_ref - q) + .25f*(0.0f-dq); 00301 } 00302 if(run_control&(abs(q_ref - q))<.05f){ 00303 printf("control done\n\r");} 00304 run_control = (abs(q_ref - q))>.05f; 00305 00306 u = CONSTRAIN(e, -V, V); 00307 WriteVoltage(u); 00308 U = KT*(u - KT*dq)/R; 00309 i_est = U; 00310 i_int += U 00311 //WriteVoltage(-10.0f); 00312 } 00313 00314 /* Set motor voltage */ 00315 void WriteVoltage(float v){ 00316 if(v>0){ 00317 TIM1->CCR1 = 0; 00318 TIM1->CCR2 = (int) (PWM_ARR*(v/V)); 00319 } 00320 else if(v<=0){ 00321 TIM1->CCR2 = 0; 00322 TIM1->CCR1 = (int) (PWM_ARR*(abs(v)/V)); 00323 } 00324 } 00325 00326 void SerialISR(void){ 00327 00328 io.putc(io.getc()); 00329 00330 } 00331 00332 00333 /* Read ID Jumpers */ 00334 int ReadID(void){ 00335 int i1 = !id_1.read(); 00336 int i2 = !id_2.read(); 00337 int i3 = !id_3.read(); 00338 return (i1<<2) | (i2<<1) | i3; 00339 } 00340 00341 00342 /* Initialize Encoder */ 00343 void InitEncoder(void) { 00344 // configure GPIO PA0 & PA1 as inputs for Encoder 00345 //RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA 00346 GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; // PA0 & PA1 as Alternate Function 00347 GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; // PA0 & PA1 as Inputs 00348 GPIOA->OSPEEDR |= 0x00000011; // GPIO Speed 00349 //GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down 00350 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 00351 GPIOA->AFR[1] |= 0x00000000 ; // 00352 00353 // configure TIM2 as Encoder input 00354 TIM2->DIER = 0x00; 00355 TIM2->EGR = 0x0; 00356 NVIC_DisableIRQ(TIM2_IRQn); 00357 00358 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 00359 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' 00360 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) 00361 TIM2->CCMR1 = 0x5151; // CC1S='01' CC2S='01' 00362 TIM2->CCMR2 = 0x0000; 00363 TIM2->CCER = 0x0011; // CC1P CC2P 00364 TIM2->PSC = 0x0000; // Prescaler = (0+1) 00365 TIM2->CNT = 0x0000; //reset the counter before we use it 00366 00367 TIM2->CR2 = 0x030; //MMS = 101 00368 __TIM15_CLK_ENABLE(); 00369 TIM15->PSC = 0x03; 00370 TIM15->SMCR = 0x4; //TS = 010 for ITR2, SMS = 100 00371 TIM15->CCMR1 = 0x3;// CC1S = 11, IC1 mapped on TRC 00372 TIM15->CCER |= TIM_CCER_CC1P; 00373 TIM15->CCER |= TIM_CCER_CC1E; 00374 TIM15->CR1 = 0x1; 00375 00376 } 00377 00378 00379 /* Initialize PWM */ 00380 void InitPWM(void){ 00381 RCC->AHBENR |= RCC_AHBENR_GPIOAEN; // enable the clock to GPIOA 00382 RCC->AHBENR |= RCC_AHBENR_GPIOBEN; // enable the clock to GPIOB 00383 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock 00384 00385 GPIOA->MODER |= GPIO_MODER_MODER7_1 | GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 ; //PA_7, PA_8, PA_9 to alternate funtion mode 00386 GPIOB->MODER |= GPIO_MODER_MODER0_1; // PB_0 to alternate function mode 00387 GPIOA->AFR[0] |= 0x60000000; // PA_7 to alternate function 6 00388 GPIOA->AFR[1] |= 0x00000066; // PA_8, PA_9 to alternate function 6 00389 GPIOB->AFR[0] |= 0x00000006; // PB_0 to alternate function 6 00390 00391 //PWM Setup 00392 TIM1->CCMR1 |= 0x6060; // Enable output compare 1 and 2 00393 TIM1->CCER |= TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC2E; // enable outputs 1, 2, and complementary outputs 00394 TIM1->BDTR |= TIM_BDTR_MOE | 0xF; // MOE = 1 | set dead-time 00395 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock 00396 TIM1->ARR = PWM_ARR; // set auto reload 00397 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on, 00398 TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 00399 00400 00401 NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn); //Enable TIM1 IRQ 00402 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt 00403 TIM1->CR1 |= 0x40; //CMS = 10, interrupt only when counting up 00404 TIM1->RCR |= 0x001; // update event once per up/down count of tim1 00405 TIM1->EGR |= TIM_EGR_UG; 00406 00407 } 00408
Generated on Tue Jul 19 2022 04:12:03 by
1.7.2