File content as of revision 5:f82e86e63928:

#include "QEI.h"
#include "PID.h"
#include "mbed.h"
#include "math.h"


#define pi  3.14159265358979323846


Serial PC(USBTX, USBRX);

AnalogIn Current(p17);         //Current   Connect to pin CS
PwmOut   pwm(p22);
DigitalOut INA(p19);
DigitalOut INB(p20);

Ticker position_ticker;


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


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

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


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) {
        INA=1;
        INB=0;
        pwm.write(1);
    } else if (signal > 0) {
        INA=0;
        INB=1;
        pwm.write(1);
    } else {
        INA=0;
        INB=0;
        pwm.write(0);   
    }
}

 

void initialize()
{
    INA=0;
    INB=0;
    pwm.write(0);
}
void forward()
{
    INA.write(1);
    INB.write(0);
    pwm.write(1);   
}
void back()
{
    INA.write(0);
    INB.write(1);
    pwm.write(1);   
}
void begin()
{
    INA.write(1);
    INB.write(0);
    pwm.write(0.05);   
}
int get_position(int position)
{
       if (position > 40){
             position = 40;
             return position;
            }
        else if (position< 1){
             position = 1;
             return position;
        }
        else
        {
         return position;    
        }
}

int main()
{
    int position;
    wait(5);
    printf("Iiitializing.....\n\r");
    
    initialize();
    pwm.period(1/ PWM_frequency);       //Set PWM frequency.  limited 20 kHz (.00005 seconds)
  
    wait(1.5);

    printf("Begining\n\r");
    

    //position_ticker.attach(&update_position, 0.5);
    begin();
    wait(2);
    int direction;
    
    while(1){

       int current_position=pulsesToDegrees(wheel.getPulses());
       
       int position=get_position(current_position);
        
        if(position==1||position==0)
        {
            direction=1;    
        }
        else if(position==40)
        {
            direction=-1;    
        }    
       
       
       float command = sin(direction*position*pi/40 );
       
      
        if(command>0)
            { forward();}
        else if(command<0)
            { back();}
        
       

        printf("position:%d\n",position);
        printf("%f\n",command);

     //signal_to_hbridge(controller.command_position());
    }

}


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;
    float current_Position = pulsesToDegrees(wheel.getPulses());  //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
    int currentPosition=(int)current_Position; 
    this->error = (currentPosition - desired_position);
    this->integral_error += this->error;
    if (this->integral_error > 1.0)
        this->integral_error = 1.0;
    else if (this->integral_error < -1.0)
        this->integral_error = -1.0;
    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;
}