![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
cube.cpp
- Committer:
- tomras12
- Date:
- 2017-04-12
- Revision:
- 24:c7b3bac429c5
- Parent:
- 23:abe76b7c377a
- Child:
- 26:f2bb916262c9
File content as of revision 24:c7b3bac429c5:
/* * cube.h * April 11, 2017 * * Control software for balancing cube senior design project * * Will Church * Tom Rasmussen */ #include "cube.h" 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(5); } /* * 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)); }