servodisc goodness
main.cpp
- Committer:
- benkatz
- Date:
- 2018-03-07
- Revision:
- 11:16d807d6b9c5
- Parent:
- 10:4b7f2653fb45
File content as of revision 11:16d807d6b9c5:
#include "mbed.h" #include "cube.h" #include "communication.h" #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.000065f // Inertia #define KT 0.0678f // Torque Constant #define R 0.85f // Resistance #define V_IN 60.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) (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 Serial io(PB_6, PB_7); // Differential Serial to JST Header DigitalIn id_3(PB_3); // ID Setting Jumpers DigitalIn id_2(PB_4); DigitalIn id_1(PB_5); DigitalOut led(PA_15); // Debug LED DigitalIn d_in(PA_4); // Input from AND Board //AnalogOut a_out(PA_5); DigitalOut d_out(PA_5); // LED on output to AND Board void Control(); void InitEncoder(); void InitPWM(); void InitGPIO(); void WriteVoltage( float v); int ReadID(); void SerialISR(); /* Control Variables */ int id; int q_raw, dir, dq_raw = 0; float q, q_old, dq, u, e, q_ref, dqdebug = 0; int count, count2; int controlmode = 0; float i_est; float i_int; volatile int run_control = 0; volatile int position_setpoint = 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; //int8_t log_vec[1250] = {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++; //d_out = !d_out; /* 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/4] = q_raw>>4; } if(count>20000 && count<21250){ printf("%d\n\r", log_vec[count2]); wait_us(80); //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"); pc.printf("%d \n\r", q_raw); //printf("%f %f\n\r", dq, dqdebug); //d_out = !d_out; count = 0; } */ //a_out.write(q/2.0f); TIM1->SR = 0x0; // reset the status register } // FUNCTIONS TO MODIFY int get_board_id() { return id; } void do_rotation(int8_t num_turns, int8_t derp) { printf("[BOARD %d] Rotate %d turns!\r\n",get_board_id(),num_turns); position_setpoint += num_turns; q_ref = 0.5f*PI*position_setpoint; run_control = 1; while(run_control) { ; } wait(0.0001f); printf("done.\r\n"); } void set_and_board(int8_t value, int8_t derp) { printf("[BOARD %d] Set and board %d\r\n",get_board_id(),value); d_out = value; } int8_t get_and_board() { uint8_t value = d_in; led = value; //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value); return value; } // BEGIN STATE MACHINE CODE mbed_info_t state_machine_info; void state_machine_init() { printf("Start state machine!\r\n"); int board_id = get_board_id(); // set up state_machine_info state_machine_info.face = (face_t)board_id; pc.printf("\tboard id: %d\n",board_id); // set null sequence to disable everything state_machine_info.seq = NULL; state_machine_info.rotate = do_rotation; state_machine_info.set_and = set_and_board; state_machine_info.get_and = get_and_board; // prepare for serial clear_message_buffer(); } void handle_serial(Serial* pc) { while(pc->readable()) { // read it uint8_t data = pc->getc(); if(data == '\n') receive_move_sequence(pc,&state_machine_info); } } // END STATE MACHINE CODE /* Main Loop */ int main() { pc.baud(921600); io.baud(115200); pc.printf("\n\r Rubix Controller\n\r"); print_sample_sequence_hex(); id_1.mode(PullUp); id_2.mode(PullUp); id_3.mode(PullUp); id = ReadID(); pc.printf(" Motor ID: %d\n\r", id); //d_in.mode(PullDown); led = 1; d_out = 0; //wait(.1); InitPWM(); InitEncoder(); //pc.printf("Initializing Encoder\n\r"); //pc.printf("Initializing PWM\n\r"); //wait(.1); //io.attach(&SerialISR); reset_mbed(); //MUST call this first thing in main - initializes data structures! state_machine_init(); while(1) { //myled = get_board_id(); handle_serial(&io); run_sequence_2(&state_machine_info); // run state machine } } /* Position Control */ void Control(void){ // 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 = (18000000.0f*4.0f*2.0f*PI/CPR)/((float)dq_raw); if(isinf(dq)){ dq = 0.0f;} q_old = q; 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(run_control == 1){ 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(run_control == 0){ //e = 0; e = 100.0f*(q_ref - q) + .25f*(0.0f-dq); } if(run_control&(abs(q_ref - q))<.05f){ printf("control done\n\r");} run_control = (abs(q_ref - q))>.05f; u = CONSTRAIN(e, -V, V); WriteVoltage(u); U = KT*(u - KT*dq)/R; i_est = U; i_int += 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*(abs(v)/V)); } } void SerialISR(void){ io.putc(io.getc()); } /* Read ID Jumpers */ int ReadID(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) { // configure GPIO PA0 & PA1 as inputs for Encoder //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 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) 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 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){ 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 GPIOA->AFR[0] |= 0x60000000; // PA_7 to alternate function 6 GPIOA->AFR[1] |= 0x00000066; // PA_8, PA_9 to alternate function 6 GPIOB->AFR[0] |= 0x00000006; // PB_0 to alternate function 6 //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 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; }