File content as of revision 0:9bf9230f8cfa:

#include "mbed.h"                    //Include mbed library (Pin and C/C++ API)
#include "QEI.h"
#include "Motor.h"

/************** Declare Objects ******************************************/ 
Serial pc(USBTX, USBRX);             //Declare serial port object to pc
//AnalogIn IRSensor(p20);              //Declare Analog and digital inputs
QEI Encoder(p5, p6, NC, 64,QEI::X4_ENCODING);
Motor mot(p25, p27, p28);
PwmOut dummyPwm(p23);       //[DO NOT MODIFY] Dummy variable used to set the PWM period of all signals

/************** Time variables to regulate execution of code **************/
Timer t;                        //[DO NOT MODIFY] Timer to regulate control sequence 
Timer tlog;                     //[DO NOT MODIFY] Timer to log data 
float dt;                       //[DO NOT MODIFY] Variable to measure time window left in program 
float Fs = 20.0;                //[TO BE MODIFIED by USER using TERA TERM] Sampling Frequency (default of 20 Hz) 
                                
int main() {
    float choice;
    float choice2;
    float choice3;
    float Encmeas = 0;
    float heightEnc = 0;
    float hErr = 0;
    float dc;
    float dcHold = 0.423;
    float dcNew;
    float kp = 0.04;
    float ki = 0.005;
    float kd = 0.00;
    float lastH = 6.0;
    float derErr;
    
    pc.baud(115200);
    
    while(1) {                  //[DO NOT MODIFY WHILE LINE] Indefinite while loop 
        dummyPwm.period(0.00005);       //[DO NOT MODIFY] This sets the period of all PWM signals to 0.00005 sec
        
        mot.speed(0.0);
        pc.printf("Desired Height (inches): \n");
        pc.scanf("%f", &choice);  //desired height
        
        pc.printf("Sampling Frequency (Hz) between 2 - 200: \n");
        pc.scanf("%f", &Fs);
        
        pc.printf("Desired Duration (s): \n");
        pc.scanf("%f", &choice2); //duration
        
        pc.printf("Type 1 for Logic Controller, 2 for P Controller, 3 for PI Controller, 4 for PID Controller:\n");
        pc.scanf("%f", &choice3);  
          
        wait(0.5);      //Small wait... to give you some time
        t.start();      //[DO NOT MODIFY] Start Timer that regulate control sequence
        tlog.start();       //[DO NOT MODIFY] Start Timer that you will use to log time data
        
        float intError = 0.0;
        
        while(tlog.read() <= choice2) {  //[INSERT] Entering Control Loop: Modify condition in while loop  
                            //While loop must execute as long as specified by user
            Encmeas=(float)Encoder.getPulses();
            heightEnc=(0.0202*(Encmeas)+5.1115);
            hErr=choice-heightEnc;
            if(choice3 == 1) {
                if(hErr>0){
                    dc = 0.3;
                    }
                else if(hErr<0){
                    dc = -0.3;
                    }
                else{
                    dc = 0;
                    }
                dcNew = dc + dcHold;
                if((dcNew>1) or (dcNew<-1)){
                    printf("ERROR: Duty Cycle Out of Range.");
                    exit(0);
                    }
                else{
                    dcNew = dcNew*1.0;  // 1.0 is the polarity mulitplier from the Calibration Bin File
                    }
                }
            else if(choice3 == 2){
                dcNew = kp*hErr;
                }
            else if(choice3 == 3){
                intError = intError + (hErr*(1/Fs));
                dc = (kp*hErr) + (ki*intError);
                dcNew = dc + dcHold;
                if((dcNew>1) or (dcNew<-1)){
                    printf("ERROR: Duty Cycle Out of Range.");
                    exit(0);
                    }
                }
            else if(choice3 == 4){
                intError = intError + (hErr*(1/Fs));
                derErr = -(Encmeas - lastH)*(1/Fs);
                lastH = Encmeas;
                dc = (kp*hErr) + (ki*intError) + (kd*derErr);
                dcNew = dc + dcHold;
                if((dcNew>1) or (dcNew<-1)){
                    printf("%f\n%f", dcNew, dc);
                    printf("ERROR: Duty Cycle Out of Range.");
                    exit(0);
                    }
                else{
                    dcNew = dcNew*1.0;  // 1.0 is the polarity mulitplier from the Calibration Bin File
                    }
                }
            
            mot.speed(dcNew*-1.0); //1.0 is the polarity multiplier
            
            pc.printf("%f, %f, %f, %f\n", tlog.read(), Encmeas, hErr, dcNew);   //using this print format to import data simpler
            //pc.printf("Current Time: %f\n", tlog.read());
            //pc.printf("Measured Height: %f(in)\n", Encmeas);
            //pc.printf("Error: %f(in)\n", hErr);
            //pc.printf("Duty Cycle: %f\n", dcNew);
            
        //[DO NOT MODIFY the following lines of code] This code makes sure the control sequence is executed every "1/Fs" seconds
        float Ts = 1/Fs;        //[DO NOT MODIFY] Ts is the control period, inverse of control sampling frequency
                dt = Ts-t.read();       //[DO NOT MODIFY]
                t.stop();           //[DO NOT MODIFY]
                t.reset();          //[DO NOT MODIFY]
                if (dt >= 0) {          //[DO NOT MODIFY]
                    wait(dt);       //[DO NOT MODIFY]
                }               //[DO NOT MODIFY]
                t.start();          //[DO NOT MODIFY]
        }               //[DO NOT MODIFY] End of Control While Loop
    t.stop();           //[DO NOT MODIFY] 
    t.reset();          //[DO NOT MODIFY]
    tlog.stop();            //[DO NOT MODIFY] Stop and reset timer used when logging data
    tlog.reset();           //[DO NOT MODIFY]
    
    mot.speed(0.0);
    
    }   //End of indefinite while loop
}       //End of main function