File content as of revision 9:61f214b91751:

#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            30.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);                       // LED on 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;


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.001f);
    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;
    //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;
    //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;       
                             
    }