servodisc goodness
Diff: main.cpp
- Revision:
- 5:96cd67bcac8c
- Parent:
- 4:6e290eb553cd
- Child:
- 6:1143996ac690
diff -r 6e290eb553cd -r 96cd67bcac8c main.cpp --- a/main.cpp Wed Jan 10 04:44:10 2018 +0000 +++ b/main.cpp Sun Jan 14 23:18:09 2018 +0000 @@ -56,7 +56,7 @@ float U; -int16_t log_vec[2500] = {0}; +//int8_t log_vec[1250] = {0}; //int16_t log_vec_2[1250] = {0}; @@ -66,6 +66,8 @@ } count++; + //d_out = !d_out; + if(count>1000 && count<2000){ q_ref = 1.57f; //ref = 18000.0f; @@ -83,48 +85,52 @@ 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); + log_vec[count/4] = q_raw>>4; } - if(count>20000 && count<22500){ - //printf("%d\n\r", log_vec[count2]); - //wait_us(200); + 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 %f\n\r", q_raw, e); - printf("%f %f\n\r", dq, dqdebug); + pc.printf("%d \n\r", q_raw); + //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.baud(921600); io.baud(921600); - + SystemCoreClockUpdate(); + //printf("%d\n\r", SystemCoreClock); //pc.printf("\n\r Rubix Controller\n\r"); id_1.mode(PullUp); id_2.mode(PullUp); id_3.mode(PullUp); id = GetID(); - pc.printf(" Motor ID: %d\n\r", id); + //pc.printf(" Motor ID: %d\n\r", id); //d_in.mode(PullDown); led = 1; @@ -159,7 +165,7 @@ // Kalman Filter // // Update Model // - + /* q_est[0] += q_est[1]*F[0][1]; q_est[1] += B[1]*U; @@ -191,7 +197,7 @@ 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 //