![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
Diff: cube.cpp
- Revision:
- 23:abe76b7c377a
- Child:
- 24:c7b3bac429c5
diff -r 9f3022fe9084 -r abe76b7c377a cube.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/cube.cpp Wed Apr 12 00:29:45 2017 +0000 @@ -0,0 +1,50 @@ +/* + * cube.h + * April 11, 2017 + * + * Control software for balancing cube senior design project + * + * Will Church + * Tom Rasmussen + */ + + #include "cube.h" + +/* + * 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 - 2.0) * 5000.0; // Scale the velocity to rad/s + + double bt = (*(c->angle) - c->eqAngle); + + double r1 = (c->Kbt * bt + c->Kbv * *(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)); +} + \ No newline at end of file