File content as of revision 6:b89ca8add6ee:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
// this program can turn the motor clockwise or counterclockwise depending on the value p or n typed into the terminal. with 's' you can stop the motor

MODSERIAL pc(USBTX,USBRX);
PwmOut motorspeed(D5);
DigitalOut motorposition(D4);
DigitalOut led1(D8);
DigitalOut led2(D11);
AnalogIn pot(A1);
DigitalIn button1(D13);
DigitalIn button2(D12);
Ticker potmeterreadout;
Ticker encoderreadout;
Encoder motor1(PTD0,PTC4); // motor 1 is from encoder, otherwise it would be a pain in the ass to change all that code to motorencoder


float PwmPeriod = 0.0001f;

const double M1_TS = 0.01f;                                                             // timestep
const double M1_KP = 0.216, M1_KI = 1.8, M1_KD = 0.0;                                   // controller gains for motor 1
double m1_err_int = 0, m1_prev_err = 0;                                                 // initiallize errors
const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; // derivative filter coefficients
double m1_f_v1 = 0.0, m1_f_v2 = 0.0;                                                    // filter variables
/*
// biquad filter for emg signals
double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0, const double b1, const double b2){
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
    }
*/
// PID controller function
double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double&e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
  double e_der = (e - e_prev)/Ts;       // derivative
  //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
  e_prev = e;
  e_int = e_int + Ts*e;                 // integral   
  return Kp*e + Ki*e_int;    //PID controller
  }



volatile double potvalue = 0.0;
volatile float position = 0.0;
void readpot(){
    potvalue = pot.read()*6.28;
    position = motor1.getPosition()/4200.00*6.28;
    pc.printf("pos: %f, speed %f reference velocity = %.2f\r\n",position, motor1.getSpeed(), potvalue);
    //motorspeed = potvalue;
    float motorPID = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
    motorspeed = motorPID;
    }
 // output PID controller is not yet tested.

int main()
{  
    
    pc.baud(9600);
    potmeterreadout.attach(readpot,0.2f);
    motorspeed.period(PwmPeriod);
    //float motorspeed = 0.0f;
    while (true) {
        
            
            //pc.printf("reference velocity = %.2f\r\n", potvalue);     
        

            if ((button2 == 1)&&(button1 == 0)) {

                motorposition = 0;         // motor turns anticlockwise
                led2 = 0;
                
            }
            if ((button2 ==0)&&(button1 ==1)){
            
                motorposition = 1;         // motor turns anticlockwise
                led2 = 1;
                }
        //}

    }
}