File content as of revision 26:4f84448b4d46:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "FastPWM.h"

enum robotStates {KILLED, ACTIVE};
robotStates currentState = KILLED;

QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
MODSERIAL pc(USBTX, USBRX);

// Defining outputs

// Leds can be used to indicate status
DigitalOut ledG(LED_GREEN);
DigitalOut ledR(LED_RED);
DigitalOut ledB(LED_BLUE);

DigitalOut motor1_direction(D4);
PwmOut motor1_pwm(D5);

// Defining inputs
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);
InterruptIn button1(D2);
AnalogIn pot1(A0);
AnalogIn pot2(A1);

// PWM settings
float pwmPeriod = 1.0/5000.0;
int frequency_pwm = 10000; //10kHz PWM

volatile float x;
volatile float x_prev =0; 
volatile float y; // filtered 'output' of ReadAnalogInAndFilter 

// Initializing encoder
Ticker encoderTicker;
Ticker controllerTicker;
volatile int counts = 0;
volatile float revs = 0.00;

// MOTOR CONTROL PART    
bool m1_direction = false;
int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
const float maxAngle = 2*3.14*posRevRange; // max angle in radians
const float Ts = 0.1;

// Function getReferencePosition returns reference angle within range
float getReferencePosition(){
    double r;
    if(m1_direction == false){
        // Clockwise rotation yields positive reference
        r = maxAngle*pot1.read();
        }
    if(m1_direction == true){
        // Counterclockwise rotation yields negative reference
        r = -1*maxAngle*pot1.read();
        }
    return r;
    }

// Initializing position and integral errors
float e_pos = 0;
float e_int = 0;
float e_der = 0;
float e_prev = 0;
float e = e_pos + e_int + e_der;

// readEncoder reads counts and revs and logs results to serial window
void getError(float &e_pos, float &e_int, float &e_der){
    counts = Encoder.getPulses();
    double motor1Position = 2*3.14*(counts/(131*64.0f));
    pc.printf("%0.2f revolutions \r\n", motor1Position);
    e_pos = getReferencePosition() - motor1Position;
    e_int = e_int + Ts*e_pos;
    e_der = (e_pos - e_prev)/Ts;
    e = e_pos + e_int + e_der; // not sure if this is really even all that useful
    e_prev = e_pos; // Store current error as we'll need it to compute the next derivative error
    pc.printf("Error is %0.2f \r \n", e);
    }

const float k_p = 0.1; // use potmeter 2 to adjust k_p within range 0 to 1
const float k_i = 0.05; // Still needs a reasonable value
const float k_d = 0.01; // Again, still need to pick a reasonable value

// motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
float motorController(float e_pos, float e_int, float e_der){    
    // NOTE: do I still need the maxangle bit here?
    double motorValue = (e_pos*k_p) + 0.35 + k_i*e_int + k_d*e_der; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
    return motorValue;
    }
    
// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
void setMotor1(float motorValue){
    switch(currentState){
                case KILLED:
                    motor1_pwm.write(0.0);
                    ledR = 0;
                    ledG = 1;
                    ledB = 1;
                    break;
                case ACTIVE:
                    // Set motor direction
                    if (motorValue >=0){
                        motor1_direction = 0;
                        }
                    else {
                        motor1_direction = 1;
                        }
                    // Set motor speed
                    if (fabs(motorValue)>1){ 
                        motor1_pwm = 1;
                        }
                    else {
                        motor1_pwm.write(fabs(motorValue));
                        }
                    ledR = 1;
                    ledG = 1;
                    ledB = 0;      
                    break;
            }
    }
    
void measureAndControl(){
    getError(e_pos, e_int, e_der);
    float motorValue = motorController(e_pos, e_int, e_der);
    pc.printf("Position action is %.2f \r \n", k_p*e_pos);
    pc.printf("Derivative action is %.2f \r \n", k_d*e_der);
    pc.printf("Integral action is %.2f", k_i*e_int);
    pc.printf("Total action is %.2f", k_p*e_pos + k_d*e_der + k_i*e_int);
    pc.printf("Motorvalue is %.2f \r \n", motorValue);
    pc.printf("Actual error is %.2f \r \n", e_pos);
    setMotor1(motorValue);
    }

void killSwitch(){
    pc.printf("Motors turned off");
    currentState = KILLED;
    }
    
void turnMotorsOn(){
    pc.printf("Motors turned on");
    currentState = ACTIVE; 
    }

    
void M1switchDirection(){
    m1_direction = !m1_direction;
    pc.printf("Switched direction!");
    }



int main()
    {
    pc.printf("Main function");
    motor1_direction = 0; // False = clockwise rotation
    motor1_pwm.period(pwmPeriod);//T=1/f 
    sw2.fall(&killSwitch);
    sw3.fall(&turnMotorsOn);
    button1.rise(&M1switchDirection);
    pc.baud(115200);
    controllerTicker.attach(measureAndControl, Ts);
      
    pc.printf("Encoder ticker attached and baudrate set");    
    }