File content as of revision 3:cae0b305d54c:

//Ben Katz, 2013

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

#define pi  3.14159265358979323846


Serial pc(USBTX, USBRX);

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

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;



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()
{

    Motor1.period(.00005);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
    Motor2.period(.00005);
    PIDController controller(40, .1, 0.1, 3, 0, 0.0007, 0.0000001); //(desired position, torque, position P gain, position D gain, position I gain, torque P gain, torque I gain)
    //timer.start();
    //float currentSum = 0;
    //float angleSum = 0;
    
    wait(.7);
   
    wait(.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;
    current_position = 0;
    torque_command = 0;
    c_torque = 0;
    error = 0;
    old_error = 0;
    error_sum = 0;
    goal_position = desired_position;
    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);    
    ///*
   
    
   // */
    //this->current_position = pulsesToDegrees(wheel.getPulses());  //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
    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()*3.3/.525);
    }
    else{
        current_torque = (Current.read()*3.3/.525);
    }
   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;
}