File content as of revision 2:89bb6272869b:

//Ben Katz, 2013

#include "QEI.h"
#include "PID.h"

#define pi  3.14159265358979323846


Serial pc(USBTX, USBRX);
//Use X4 encoding.
QEI wheel(p16, p17, NC, 1200, QEI::X4_ENCODING);       //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))


DigitalOut EncoderVcc (p18);    //Encoder VCC pin.  Alternatively, just connect these to the mbed's vout and gnd, but tihs can make wiring easier
DigitalOut EncoderGnd(p19);     //Encoder ground pin
PwmOut Motor1(p21);             //Ouput pin to h-bridge 1
PwmOut Motor2 (p22);            //Output pin to h-bridge 2
AnalogIn Current (p20);         //Current sense pin

int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
Timer timer;



float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
{
    return ((pulses/steps_per_rev)*(2*pi));
}

float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
{
    return ((pulses/steps_per_rev)*360);
}


void signal_to_hbridge( float signal)   //Input of -1 means full reverse, 1 means full forward
{                                       //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
    if (signal < 0) {
        Motor1.write(0);
        Motor2.write(signal * -1);
    } else if (signal > 0) {
        Motor1.write(signal);
        Motor2.write(0);
    } else {
        Motor1.write(0);
        Motor2.write(0);
    }
}

void pulse_motor(float pulse_time, float pulse_interval)  //Usefull for testing peak torque.
                                                          //pulse_time = motor on time.  pulse_interval = motor off time
{
    timer.start();
    while (timer.read() < pulse_time){
    Motor1.write(1);
    Motor2.write(0);
                                                //Prints current draw to PC
    printf("%F", (Current.read())*3.3*4/.525);  //Arithmetic converts the analog input to Amps
    printf("\n");                               //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
    
    }
    timer.stop();
    timer.reset();
    wait(pulse_time);
    Motor1.write(0);
    Motor2.write(0);
    wait(pulse_interval);
}



int main()
{
    EncoderVcc.write(1);        //Encoder VCC and GND attached to digital output pins to make wiring easier
    EncoderGnd.write(0);
    Motor1.period(.00005);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
    Motor2.period(.00005);
    PIDController controller(180, 0.05, 3, 0); //(desired position, P gain, D gain, I gain)
    
    /*
    Insert servo control loop below
    */
    
    
    while(1) {
    
        signal_to_hbridge(controller.Update());

    }
}



PIDController::PIDController(float desired_position, float p_gain, float d_gain, float i_gain)
{
    kp = p_gain;
    kd = d_gain;
    ki = i_gain;
    error = 0;
    old_error = 0;
    goal_position = desired_position;

}


float PIDController::Update(void)  //This function is called to set the desired position of the servo
{
    wait(.0004);        //This delay allows enough time to register a position difference between cycles.  
                        //Without the delay, velocity is read as 0, so there is no D feedback.
                        //The delay can be decreased when adding more servos, as the computation time becomes more significant.
    float desired_position = this->goal_position;  //All value are stored in the PIDController object created, and can be changed at any time
    float p_gain = this->kp;
    float d_gain = this->kd;
    float i_gain = this->ki;
    
    float currentPosition = pulsesToDegrees(wheel.getPulses());  //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians

    this->error = (currentPosition - desired_position);
    this->integral_error += this->error;

    float command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);

    this->old_error = error;

    return command;

}