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
main.cpp
- Committer:
- tomras12
- Date:
- 2017-04-18
- Revision:
- 29:37dc63b57d6e
- Parent:
- 28:b813a8f685c3
File content as of revision 29:37dc63b57d6e:
/* * main.cpp * April 11, 2017 * * Control software for balancing cube senior design project * * Will Church * Tom Rasmussen */ #include "cube.h" /* Initialize general I/O */ DigitalOut myled(LED1); InterruptIn button(USER_BUTTON); Serial *pc; // Serial connection to pc DigitalOut EN1(D0); // Interrupt I2C i2c(PB_9, PB_8); // SDA, SCL BNO055 *imu; // IMU void detectOrientation() { if (abs(euler_angles.r - balX.eqAngle) < TM && abs(euler_angles.p) < TA) {currentConfig = &balX;} else if (abs(euler_angles.p - balY.eqAngle) < TM && abs(euler_angles.r) < TA) {currentConfig = &balY;} else if (abs(euler_angles.p - balZ.eqAngle) < TM && abs(euler_angles.r + 1.6) < TA) {currentConfig = &balZ;} else {currentConfig = &fall;} } void controlLoop() { // Update the motor torque 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() { wait_ms(100); // Debounce button // Activate control loop if not active if(!isActive) { // Reset equilibrium angle to current angle currentConfig->eqAngle = *(currentConfig->angle); pwmint.attach(&controlLoop, .01); EN1 = 1; myled = 1; isActive = true; pc->printf("Loop now active\r\n"); } else { 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"); } } 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 */ int main() { // Initialize serial pc = new Serial(SERIAL_TX, SERIAL_RX); // Initialize IMU pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); wait_ms(1000); // Allows IMU time to power up pc->printf("Initializing IMU...\r\n"); imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin while (imu->chip_ready() == 0) { pc->printf("Bosch BNO055 is NOT available!!\r\n"); } // Initialize pwm to 0 torque balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range // error // Set frequency of PWMs balZ.pwm->period(0.0002); // set pwm frequency // Attach the button interrupt to the callback function // This button toggles if the loop is enabled isActive= false; button.rise(&onButtonPress); // Calibrate until sys calib is at 3 //checkCalib(imu, pc); // Initialize config 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("%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", 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", // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv); //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); //wait(0.2); } }