Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
cube.cpp@23:abe76b7c377a, 2017-04-12 (annotated)
- Committer:
- tomras12
- Date:
- Wed Apr 12 00:29:45 2017 +0000
- Revision:
- 23:abe76b7c377a
- Child:
- 24:c7b3bac429c5
Refactor to make code more adaptable
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tomras12 | 23:abe76b7c377a | 1 | /* |
tomras12 | 23:abe76b7c377a | 2 | * cube.h |
tomras12 | 23:abe76b7c377a | 3 | * April 11, 2017 |
tomras12 | 23:abe76b7c377a | 4 | * |
tomras12 | 23:abe76b7c377a | 5 | * Control software for balancing cube senior design project |
tomras12 | 23:abe76b7c377a | 6 | * |
tomras12 | 23:abe76b7c377a | 7 | * Will Church |
tomras12 | 23:abe76b7c377a | 8 | * Tom Rasmussen |
tomras12 | 23:abe76b7c377a | 9 | */ |
tomras12 | 23:abe76b7c377a | 10 | |
tomras12 | 23:abe76b7c377a | 11 | #include "cube.h" |
tomras12 | 23:abe76b7c377a | 12 | |
tomras12 | 23:abe76b7c377a | 13 | /* |
tomras12 | 23:abe76b7c377a | 14 | * Returns PWM duty cycle based on: |
tomras12 | 23:abe76b7c377a | 15 | * - wv wheel velocity |
tomras12 | 23:abe76b7c377a | 16 | * - ae angle error |
tomras12 | 23:abe76b7c377a | 17 | * - bv body velocity |
tomras12 | 23:abe76b7c377a | 18 | * - Kwv wheel vel gain |
tomras12 | 23:abe76b7c377a | 19 | * - Kbt angle gain |
tomras12 | 23:abe76b7c377a | 20 | * - Kbv body vel gain |
tomras12 | 23:abe76b7c377a | 21 | */ |
tomras12 | 23:abe76b7c377a | 22 | double calcPWM(config *c) |
tomras12 | 23:abe76b7c377a | 23 | { |
tomras12 | 23:abe76b7c377a | 24 | // Converts and read the analog input value (value from 0.0 to 1.0): |
tomras12 | 23:abe76b7c377a | 25 | double wv = c->hall->read(); |
tomras12 | 23:abe76b7c377a | 26 | wv = (wv - 2.0) * 5000.0; // Scale the velocity to rad/s |
tomras12 | 23:abe76b7c377a | 27 | |
tomras12 | 23:abe76b7c377a | 28 | double bt = (*(c->angle) - c->eqAngle); |
tomras12 | 23:abe76b7c377a | 29 | |
tomras12 | 23:abe76b7c377a | 30 | double r1 = (c->Kbt * bt + c->Kbv * *(c->vel) + c->Kwv * wv); |
tomras12 | 23:abe76b7c377a | 31 | |
tomras12 | 23:abe76b7c377a | 32 | //Limit PWM range |
tomras12 | 23:abe76b7c377a | 33 | if (r1 > 6.0) {r1 = 6.0;} |
tomras12 | 23:abe76b7c377a | 34 | else if (r1 < -6.0) {r1 = -6.0;} |
tomras12 | 23:abe76b7c377a | 35 | |
tomras12 | 23:abe76b7c377a | 36 | // Normalize for PWM output |
tomras12 | 23:abe76b7c377a | 37 | r1 = ((.4*(r1/6.0)) + 0.5); |
tomras12 | 23:abe76b7c377a | 38 | |
tomras12 | 23:abe76b7c377a | 39 | // Check if cube is too far tilted and send 0 torque |
tomras12 | 23:abe76b7c377a | 40 | // May be redundant, check outer program |
tomras12 | 23:abe76b7c377a | 41 | if (bt > (pi/8) || bt < -(pi/8)){ |
tomras12 | 23:abe76b7c377a | 42 | return .5; |
tomras12 | 23:abe76b7c377a | 43 | } |
tomras12 | 23:abe76b7c377a | 44 | return r1; |
tomras12 | 23:abe76b7c377a | 45 | } |
tomras12 | 23:abe76b7c377a | 46 | |
tomras12 | 23:abe76b7c377a | 47 | void updatePWM(config *c) { |
tomras12 | 23:abe76b7c377a | 48 | c->pwm->write(calcPWM(c)); |
tomras12 | 23:abe76b7c377a | 49 | } |
tomras12 | 23:abe76b7c377a | 50 |