File content as of revision 1:f3fe6d2b7639:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "math.h"

MODSERIAL pc(USBTX,USBRX);
PwmOut speed1(D5);
DigitalOut dir1(D4);
DigitalOut led1(D8);
DigitalOut led2(D11);
AnalogIn pot(A1);
DigitalIn button1(D13);
DigitalIn button2(D12);
Ticker potmeterreadout;
Ticker encoderreadout;
Encoder motor1(PTD0,PTC4);

float PwmPeriod = 0.0001f;

double count = 0; //set the counts of the encoder
volatile double angle = 0;//set the angles

double setpoint = 180;//I am setting it to move through 180 degrees
double Kp = 0.15;// you can set these constants however you like depending on trial & error
double Ki = 1;
double Kd = 0.5;

float last_error = 0;
float error1 = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|

volatile double potvalue = 0.0;
volatile double position = 0.0;
/*void readpot()
{
    
}
*/
void PIDcalculation()
{
    potvalue = pot.read();
    angle = motor1.getPosition()/4200.00*6.28;
    setpoint = potvalue*setpoint;
    //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
    //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);
    
    if (angle <= setpoint) {
        dir1 = 1;// Forward motion
    } else {
        dir1 = 0;//Reverse motion
    }
    
    error1 = setpoint - angle;
    if (error1 <= 0.2) {
        speed1 = 0;
    }

    changeError = error1 - last_error; // derivative term
    totalError += error1; //accumalate errors to find integral term
    pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
    if (pidTerm >= 255) {
        pidTerm = 255;
    } else if (pidTerm <= -255) {
        pidTerm = -255;
    } else {
        pidTerm = pidTerm;
    }
    //constraining to appropriate value
    pidTerm_scaled = abs(pidTerm)/255;//make sure it's a positive value
    last_error = error1;
}

int main()
{
    encoderreadout.attach(PIDcalculation,0.001f);
    speed1.period(PwmPeriod);

    while(true) {



        speed1 = pidTerm_scaled;

        pc.printf("WHEEL ANGLE:%d, PID_scaled = %f, error = %f\r", angle, pidTerm_scaled, error1);

    }

}