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
cube.cpp@24:c7b3bac429c5, 2017-04-12 (annotated)
- Committer:
- tomras12
- Date:
- Wed Apr 12 20:54:52 2017 +0000
- Revision:
- 24:c7b3bac429c5
- Parent:
- 23:abe76b7c377a
- Child:
- 26:f2bb916262c9
Implemented orientation detection
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 | 24:c7b3bac429c5 | 12 | |
tomras12 | 24:c7b3bac429c5 | 13 | void checkCalib(BNO055 *imu, Serial *pc) { |
tomras12 | 24:c7b3bac429c5 | 14 | pc->printf("Checking calibration status...\r\n"); |
tomras12 | 24:c7b3bac429c5 | 15 | int stat = imu->read_calib_status(); |
tomras12 | 24:c7b3bac429c5 | 16 | while(stat < 192) { |
tomras12 | 24:c7b3bac429c5 | 17 | pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n", |
tomras12 | 24:c7b3bac429c5 | 18 | (stat >> 6) & 0x03, |
tomras12 | 24:c7b3bac429c5 | 19 | (stat >> 4) & 0x03, |
tomras12 | 24:c7b3bac429c5 | 20 | (stat >> 2) & 0x03, |
tomras12 | 24:c7b3bac429c5 | 21 | stat & 0x03); |
tomras12 | 24:c7b3bac429c5 | 22 | wait_ms(250); |
tomras12 | 24:c7b3bac429c5 | 23 | stat = imu->read_calib_status(); |
tomras12 | 24:c7b3bac429c5 | 24 | } |
tomras12 | 24:c7b3bac429c5 | 25 | pc->printf("Looks good buddy, put the cube down now.\r\n"); |
tomras12 | 24:c7b3bac429c5 | 26 | wait(5); |
tomras12 | 24:c7b3bac429c5 | 27 | } |
tomras12 | 23:abe76b7c377a | 28 | |
tomras12 | 23:abe76b7c377a | 29 | /* |
tomras12 | 23:abe76b7c377a | 30 | * Returns PWM duty cycle based on: |
tomras12 | 23:abe76b7c377a | 31 | * - wv wheel velocity |
tomras12 | 23:abe76b7c377a | 32 | * - ae angle error |
tomras12 | 23:abe76b7c377a | 33 | * - bv body velocity |
tomras12 | 23:abe76b7c377a | 34 | * - Kwv wheel vel gain |
tomras12 | 23:abe76b7c377a | 35 | * - Kbt angle gain |
tomras12 | 23:abe76b7c377a | 36 | * - Kbv body vel gain |
tomras12 | 23:abe76b7c377a | 37 | */ |
tomras12 | 23:abe76b7c377a | 38 | double calcPWM(config *c) |
tomras12 | 23:abe76b7c377a | 39 | { |
tomras12 | 23:abe76b7c377a | 40 | // Converts and read the analog input value (value from 0.0 to 1.0): |
tomras12 | 23:abe76b7c377a | 41 | double wv = c->hall->read(); |
tomras12 | 23:abe76b7c377a | 42 | wv = (wv - 2.0) * 5000.0; // Scale the velocity to rad/s |
tomras12 | 23:abe76b7c377a | 43 | |
tomras12 | 23:abe76b7c377a | 44 | double bt = (*(c->angle) - c->eqAngle); |
tomras12 | 23:abe76b7c377a | 45 | |
tomras12 | 23:abe76b7c377a | 46 | double r1 = (c->Kbt * bt + c->Kbv * *(c->vel) + c->Kwv * wv); |
tomras12 | 23:abe76b7c377a | 47 | |
tomras12 | 23:abe76b7c377a | 48 | //Limit PWM range |
tomras12 | 23:abe76b7c377a | 49 | if (r1 > 6.0) {r1 = 6.0;} |
tomras12 | 23:abe76b7c377a | 50 | else if (r1 < -6.0) {r1 = -6.0;} |
tomras12 | 23:abe76b7c377a | 51 | |
tomras12 | 23:abe76b7c377a | 52 | // Normalize for PWM output |
tomras12 | 23:abe76b7c377a | 53 | r1 = ((.4*(r1/6.0)) + 0.5); |
tomras12 | 23:abe76b7c377a | 54 | |
tomras12 | 23:abe76b7c377a | 55 | // Check if cube is too far tilted and send 0 torque |
tomras12 | 23:abe76b7c377a | 56 | // May be redundant, check outer program |
tomras12 | 23:abe76b7c377a | 57 | if (bt > (pi/8) || bt < -(pi/8)){ |
tomras12 | 23:abe76b7c377a | 58 | return .5; |
tomras12 | 23:abe76b7c377a | 59 | } |
tomras12 | 23:abe76b7c377a | 60 | return r1; |
tomras12 | 23:abe76b7c377a | 61 | } |
tomras12 | 23:abe76b7c377a | 62 | |
tomras12 | 23:abe76b7c377a | 63 | void updatePWM(config *c) { |
tomras12 | 23:abe76b7c377a | 64 | c->pwm->write(calcPWM(c)); |
tomras12 | 23:abe76b7c377a | 65 | } |
tomras12 | 23:abe76b7c377a | 66 |