File content as of revision 0:31ccfbaf5bff:

#include "mbed.h"
#include "QEI.h"

#define pi  3.14159265358979323846


// typedef struct
 struct sPID
{
    double dState; // Last position input
    double iState; // Integrator state
    double iMax, iMin; // Maximum and minimum allowable integrator state
    double iGain, // integral gain
           pGain, // proportional gain
           dGain; // derivative gain
};



double UpdatePID(sPID * pid, double error, double position)
{
    double pTerm, dTerm, iTerm;
    
    pTerm = pid->pGain * error; // calculate the proportional term
    
   
    pid->iState += error;  // calculate the integral state with appropriate limiting

    if (pid->iState > pid->iMax) pid->iState = pid->iMax;
    else if (pid->iState < pid->iMin) pid->iState = pid->iMin;

    iTerm = pid->iGain * pid->iState; // calculate the integral term
    dTerm = pid->dGain * (pid->dState - position);

    pid->dState = position;

    return pTerm + dTerm + iTerm;
}

void setPID(sPID *pid, double P, double I, double D,
                      double iMx, double iMn, double dS, double iS){
 
    pid->pGain = P;
    pid->iGain = I;
    pid->dGain = D;
    pid->iMax = iMx;
    pid->iMin = iMn;
    pid->dState = dS;
    pid->iState = iS;
 
 }
 
Serial pc(USBTX, USBRX);

int baudRate = 460800;



int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
int pulseRead = 0;

float ctrlDeg = 0.0;
float shaftDeg = 0.0;
float cmdSig = 0.0;
float degErr = 0.0;

Timer timer;

//Use X4 encoding.
QEI wheel(p11, p12, NC, steps_per_rev, QEI::X4_ENCODING);       //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))

//Output to motors. PWM signal is sent to either one, depending on desired rotational direction. CW or CCW.
DigitalOut cwCH(p22);             //Ouput pin to h-bridge 1
DigitalOut ccwCH(p26);
PwmOut motor(p24);            //Output pin to h-bridge 2
AnalogIn Current (p19);         //Current sense pin
AnalogIn ctrlPot (p16);
AnalogIn currPot (p18);


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) {
          cwCH  = 0;
          ccwCH = 1;
          motor.write(signal * -1);
//        Motor1.write(0);
//        Motor2.write(signal * -1);
    } else if (signal > 0) {
          ccwCH = 0;
          cwCH  = 1; 
          motor.write(signal);
//        Motor1.write(signal);
//        Motor2.write(0);
    } else {
        cwCH = 0;
        ccwCH = 0;
//        Motor1.write(0);
//        Motor2.write(0);
    }
}


int main() {
      
      pc.baud(baudRate); 
      //will run a loop to apply the PID controller. I will command a movement with the 
      //pot, then will run the updatePID function to find the error difference and
      //learn how to get good feedback with the PID controller
      sPID mPid;
      sPID* pmPid;
      float currVal, prev_curr;
//      float currFactor;
      pmPid = &mPid;    // .1
           //      P   , I, D, Imax, Imin, Dstate, Istate
//      setPID(pmPid,0.07, 0, .6, 0.45, -0.1, 0, 0); //gains for when working with Degs
      setPID(pmPid,0.9, 0, 7, 0.45, -0.1, 0, 0); //gains when working in radians
    
      float t, t_ref;

      //initialize PID controller
      motor.period(0.00005f);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)

      timer.start();
      t_ref = 0;
      while (1) {
            //Ctrl pot range will be -30 to 30 degrees, just to have high point resolution
            //Gather inputs
            ctrlDeg = ctrlPot*(1.0472f)- 0.5236f;            
            shaftDeg = pulsesToRadians(wheel.getPulses());
            degErr = ctrlDeg - shaftDeg;
            
//            currFactor = currPot*3.3*(1/3.3);
            currVal = Current.read()*3.3/.140f;
            
            
            //Update PID vars and send CMD signal to motor
            cmdSig = UpdatePID(pmPid, degErr, shaftDeg);
            signal_to_hbridge(cmdSig);
            t = timer.read();
            //Print out result
           // pc.printf("t: %010lu\t SP: %3.3f\t PV: %3.3f\t CV: %3.3f\n",
//                      t, ctrlDeg, shaftDeg, cmdSig);
            if (t - t_ref > 0.1) {
                pc.printf("%6.3f\t %5.2f\t %3.3f\t %3.3f\t %3.3f\n",
                  //t, ctrlDeg, shaftDeg, cmdSig);
                   t, ctrlDeg, shaftDeg, degErr, currVal);
                   t_ref = t;
               //Current protection of motor
               if (((currVal > 6.0f) &&  (abs(currVal - prev_curr) <0.20))) break;
               
               prev_curr = currVal;
                      }//end if t_ref
            wait(0.0001f);
        }//end while loop
        //assuming motor needs braknig
        signal_to_hbridge(0);
}