File content as of revision 4:4e7b392ed0aa:

//Ben Katz, 2013

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

#define pi  3.14159265358979323846


Serial pc(USBTX, USBRX);

AnalogIn p_pot (p15);
AnalogIn d_pot (p16);
AnalogIn a_pot (p17);

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;


// Setup parameters
float volt2amp = 3.3 / .525; //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
float PWM_frequency = 20000.0;  // MC33926 h-bridge limited 20 kHz (.00005 seconds)

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


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())*volt2amp);  //Arithmetic converts the analog input to Amps
      printf("\n");
    }

    timer.stop();
    timer.reset();

    wait(pulse_time);

    Motor1.write(0);
    Motor2.write(0);

    wait(pulse_interval);
  }



int main()
{

    Motor1.period(1 / PWM_frequency);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
    Motor2.period(1 / PWM_frequency);

    float desired_position = 40;
    float torque = 0.1;
    float position_P_gain = 0.1;
    float position_D_gain = 3;
    float position_I_gain = 0;
    float torque_P_gain = 0.0007;
    float torque_I_gain = 0.0000001;


    PIDController controller(desired_position, torque, position_P_gain, position_D_gain, position_I_gain, torque_P_gain, torque_I_gain);
    //timer.start();

    wait(1.5);

    printf("%F", pulsesToDegrees(wheel.getPulses()));
    printf("\n\r");

    while(1){
        signal_to_hbridge(controller.command_position_tm());
        printf("%F", pulsesToDegrees(wheel.getPulses()));
    }

}


PIDController::PIDController(float desired_position, float desired_torque, float p_gain_p, float d_gain_p, float i_gain_p, float p_gain_c, float i_gain_c)
{
    kp_p = p_gain_p;
    kd_p = d_gain_p;
    ki_p = i_gain_p;
    kp_c = p_gain_c;
    ki_c = i_gain_c;

    torque = desired_torque;
    goal_position = desired_position;

    current_position = 0;
    torque_command = 0;
    c_torque = 0;
    error = 0;
    old_error = 0;
    error_sum = 0;
    direction = 0;
    counter = 0;

    timer.start();
    command = 0;
}

//voltage mode position control
float PIDController::command_position(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_p;
    float d_gain = this->kd_p;
    float i_gain = this->ki_p;
    if (this->timer.read_us() >  400){
         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;

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

        this->old_error = error;
        }

    return this->command;

}


float PIDController::command_position_tm(void){  //Torque Mode position control
    wait(.0003);

    float desired_position = this->goal_position;
    float p_gain = this->kp_p;
    float d_gain = this->kd_p;
    float i_gain = this->ki_p;

    if (this->timer.read_us() >  400){
        this->current_position = pulsesToDegrees(wheel.getPulses());  //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
        this->old_error = this->error;
        this->error = (this->current_position - desired_position);
        this-> command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
        this->integral_error += this->error;
        this->torque_command = command;
        this->timer.reset();
       }


    return this->command_torque();


}



float PIDController::command_torque(void){
    wait(.0004);
    float current_torque;

    if (this->direction != 0){
        current_torque = this->direction*(Current.read() * volt2amp);
    }
    else{
        current_torque = (Current.read() * volt2amp);
    }
   float average = 0;

    for (int i = 0; i < 4; i++){
        this->past_currents[i] = this->past_currents[i+1];
        average += past_currents[i];
    }
    average += current_torque;
    average = average/5;
    average = current_torque;
    this->past_currents[4] = current_torque;

    this->c_torque = average;
    this->c_error = (this->torque - average);

    this->error_sum += this->c_error;

    this->torque_command += -1*(this->kp_c*this->c_error  + this->ki_c*this->error_sum);
    if (this->torque_command > 0){
        this->direction = -1;
    }
    else if(this->torque_command < 0){
        this->direction = 1;
    }
    else{
        this->direction = 0;
   }
   if (this->torque_command > 1){
        this->torque_command = 1;
        }
   else if (this->torque_command < -1){
        this->torque_command = -1;
        }

    return this->torque_command;
}