File content as of revision 4:f6862a157db9:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include <iostream>
#include "FastPWM.h"
#include "QEI.h"
#include <math.h>
//#include "HIDScope.h"


Serial pc(USBTX, USBRX);
DigitalIn but_1(D1); //D1 to BUT1
DigitalOut msignal_1(D6); //Signal to motor controller 
DigitalOut direction_1(D7); //ouput pin for direction of rotation

//These seemed the best values after trying them out and using wikipedia's info
const float k_p_1 = 1;  
const float k_i_1 = 1;
const float k_d_1 = 0.1;
const float t_s = 0.0025; //sample time in sec; same for both motors

//inputs for LPF; still to fill in!
float b1;
float b2;
float b3;
float b4;
float b5;

float outcome_1 = 0;
float setpoint_1;
bool dir_1;
bool moving_1 = false;

float getSetpoint_1(){
    float a; //error
    cin >> a;   //input on keyboard
    return a;
    }
    
float getError_1(){
    float errorSum_1 = setpoint_1 - outcome_1;
    return errorSum_1;
    }

float checkMovement_1(){
    if (outcome_1 >= 1){
        dir_1 = true;   //CCW
        moving_1 = true;
        pc.printf("Positive movement\r\n");
        }
    else if (outcome_1 <= -1){
        dir_1 = false;     //CW
        moving_1 = true;
        pc.printf("Negative movement\r\n");
        }
     else{
        direction_1 = 0;
        moving_1 = false;
        pc.printf("No movement\r\n");
        }
    }
    
    void motor_1 (){ //runs the motor
  
    if(moving_1){
    msignal_1 = 1; // Turn motor on
    }
    else{
    msignal_1 = 0; // Turn motor off
    }
    
 }
    

/*PID controller code
Proportional part general formula: u_k = k_p * e 
Integral part general formula: u_i = k_i * integral e dt
Derivative part general formula: u_d = k_d * derivative e */

float PIDControl_1(float error){
    static float error_integral = 0;
    static float error_prev = error;
    static BiQuad LPF (b1, b2, b3, b4, b5); 
    
    //proportional
    float u_k_1 = k_p_1 * error;
    
    //integral
    error_integral = error_integral + error * t_s;
    float u_i_1 = k_i_1 * error_integral;
    
    //differential
    float error_derivative = (error - error_prev) / t_s;
   // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
   // float u_d_1 = k_d_1 * filtered_error;
    float u_d_1 = k_d_1 * error_derivative; //this is very sensitive to noise, hence the LPF above
    error_prev = error;
    
    return u_k_1 + u_i_1 + u_d_1;
    }
    
    
int main(){   
    pc.baud(115200);
    startmain:
    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
   
    
    while(true){
    pc.printf("Please input your error.\r\n");
    //setpoint_1 = getSetpoint_1();
    //float err_1 = getError_1();
    float err_1 = getSetpoint_1();
    pc.printf("Your error is: %f\r\n",err_1);
    pc.printf("-------\r\n-------\r\n");
    wait(0.5);
    outcome_1 = PIDControl_1(err_1);
    pc.printf("The outcome is %f\r\n",outcome_1);
    direction_1 = dir_1;
    checkMovement_1();
    motor_1();
    
    
   //only works if error is constantly put in:
   // if (but_1==0){
   // goto startmain; //allows to input multiple numbers after each other
   // }
    
    
    }
}