File content as of revision 23:abe76b7c377a:

/*
 * 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));
}