This is the one where I went back and un-did the cube.cpp file
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
Diff: main.cpp
- Revision:
- 28:b813a8f685c3
- Parent:
- 26:f2bb916262c9
- Child:
- 29:37dc63b57d6e
diff -r c3ca97f1db60 -r b813a8f685c3 main.cpp --- a/main.cpp Tue Apr 18 03:15:07 2017 +0000 +++ b/main.cpp Tue Apr 18 19:58:56 2017 +0000 @@ -48,10 +48,10 @@ new AnalogIn(A0), //hall "Balancing on Y edge"}; //descr -struct config balZ = {-89.9276, //Kbt - -14.9398, //Kbv - -0.001, //Kwv - -pi/4, //eqAngle +struct config balZ = {-72.4921, //Kbt + -9.9672, //Kbv + -0.00025, //Kwv + -.7000, //eqAngle &(euler_angles.p), //angle &(velocity.z), //vel new PwmOut(PE_9), //pwm @@ -83,12 +83,20 @@ } void controlLoop() { - // Detect the current orientation - - detectOrientation(); - //pc->printf("%s\r\n", currentConfig->descr); // Update the motor torque - //updatePWM(currentConfig); + if(currentConfig->Kbt == 0) { // detect fallen state + pwmint.detach(); + //currentConfig->pwm->write(0.5); + //bt = 0.0; + myled = 0; + EN1 = 0; + //P2 = 0; + //P3 = 0; + isActive = false; + pc->printf("Loop now inactive\r\n"); + } else { + updatePWM(currentConfig); + } } void onButtonPress() @@ -96,7 +104,7 @@ wait_ms(10); // Activate control loop if not active if(!isActive) { - pwmint.attach(&controlLoop, .001); + pwmint.attach(&controlLoop, .01); EN1 = 1; myled = 1; isActive = true; @@ -116,6 +124,62 @@ } } +void checkCalib(BNO055 *imu, Serial *pc) { + pc->printf("Checking calibration status...\r\n"); + int stat = imu->read_calib_status(); + while(stat < 192) { + pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n", + (stat >> 6) & 0x03, + (stat >> 4) & 0x03, + (stat >> 2) & 0x03, + stat & 0x03); + wait_ms(250); + stat = imu->read_calib_status(); + } + pc->printf("Looks good buddy, put the cube down now.\r\n"); + wait(2); +} + +/* + * Returns PWM duty cycle based on: + * - wv wheel velocity + * - ae angle error + * - bv body velocity + * - Kwv wheel vel gain + * - Kbt angle gain + * - Kbv body vel gain + */ +double calcPWM(config *c) +{ + // Converts and read the analog input value (value from 0.0 to 1.0): + double wv = c->hall->read(); + wv = (wv - 1.65) * 5000.0 / 1.65; // Scale the velocity to rad/s + + double bt = (*(c->angle) - c->eqAngle); + + double r1 = -(c->Kbt * bt + c->Kbv * -1.0 * (*(c->vel)) + c->Kwv * wv); + + //Limit PWM range + if (r1 > 6.0) {r1 = 6.0;} + else if (r1 < -6.0) {r1 = -6.0;} + + // Normalize for PWM output + r1 = ((.4*(r1/6.0)) + 0.5); + + // Check if cube is too far tilted and send 0 torque + // May be redundant, check outer program + /*if (bt > (pi/8) || bt < -(pi/8)){ + return .5; + }*/ + return r1; +} + +void updatePWM(config *c) { + + c->pwm->write(calcPWM(c)); + //pc->printf("%6.4f\r\n", calcPWM(c)); +} + /* * TODO: Documentation */ @@ -148,19 +212,24 @@ //checkCalib(imu, pc); // Initialize config - //detectOrientation(); + detectOrientation(); pc->printf("Push button to begin loop"); while(1) { + imu->get_Euler_Angles(&euler_angles); + imu->get_velocities(&velocity); + detectOrientation(); + //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r); //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z); - //pc->printf("%6.4f\r\n", *(currentConfig->angle)); + //pc->printf("%s\r\n", (currentConfig->descr)); + pc->printf("%6.4f\r\n", *(currentConfig->angle)); // cuberotate: pc->printf("Orientation: %6.4f %6.4f %6.4f\r\n", euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi); - pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", + /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, - euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi); + euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/ // pc->printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n", @@ -169,11 +238,6 @@ //pc->printf("Theta:%+6.4f [rad], P:%+6.4f [rad], R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s], WV:%+6.4f [rad/s] \r\n", // bt, euler_angles.p, r1, velocity.z, wv); - imu->get_Euler_Angles(&euler_angles); - imu->get_velocities(&velocity); - - - //wait(0.2); } }