File content as of revision 2:36cffabd0c01:

//mbed ES201 Dev Board (V1.1) Elevator Tester
// Pololu Motor - https://www.pololu.com/product/2821
// 12 V: 11,000 RPM and 300 mA free-run, 5 oz-in (0.3 kg-cm) and 5 A stall

#include "mbed.h"
#include "MotCon.h"
#include "PID.h"

#define VCC 5.0f

PID controller(0.0, 0.0, 0.0, .01);
Serial pc(USBTX, USBRX);
AnalogIn ir_range(p20);
AnalogIn sonar_range(p16);
InterruptIn sonar_pulse(p17);
DigitalIn sonar_pulse_pin(p17);
MotCon mot(p25, p27, p28);
DigitalOut led1(LED1);
DigitalOut led2(LED2);

Timer t;
Timer pulse_t;
volatile float pulse_dist=0.0;

volatile float co;
volatile float Tdelay = .001;
volatile float T1_Pk =1.6f;
volatile float T1_Ik =0.45f;
volatile float T1_Dk =0.0;
volatile float T1_set_point1 = 0.0;
volatile float dist, ir_volts;
    
float get_range(void){
    double v_avg=0.0;
    for(int i=0;i<100;i++){
        v_avg += ir_range * 3.3;
    }    
    double volts = v_avg/100.0;    
    //Employ range equation here
    double distance = (4.5*pow(volts,3))-(27.0*pow(volts, 2))+(57.0*volts)-19.0;    
    
    //double distance = volts;
    return distance;
}

void pulse_measure(void){
    if(sonar_pulse_pin){    //if pulse pin is high
        pulse_t.start();    //start the pulse timer
        sonar_pulse.fall(&pulse_measure);   //set next interrupt for falling edge
    }
    else{                   //if pulse pin is low
        pulse_dist = pulse_t.read_us();    //read the pulse timer
        pulse_t.stop();                 //stop the timer
        pulse_t.reset();                //reset timer to 0
        sonar_pulse.rise(&pulse_measure);   //reset interrupt for rising edge
    }
}

int main() {        
    wait(.2);
    pc.baud(921600);
      
    sonar_pulse.rise(&pulse_measure);
    t.start();
    
    //resets the controllers internals
    controller.reset();
    pc.printf("Waiting 2 seconds\r\n");
    wait(2.0);
    //Distance sensor limits
    controller.setInputLimits(6.0, 24.0);   //0 inches to 26 inches
    //Pwm output from -1.0 to 1.0
    controller.setOutputLimits(-.72, 0.72);
    //If there's a bias.
    controller.setBias(0.0);
    controller.setMode(AUTO_MODE);
    
    controller.setInterval(Tdelay);
      
    //We want the process variable to be 0.0 at start
    controller.setSetPoint(T1_set_point1);
    controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
    
    int state = 0;
    float sp = 10.0;
        
    while(1){
       
        switch(state){
            case 0:
                sp = 10.0; //10 inches
                state = 1;
                break;
            case 1:
                sp = 13.0;   //13 inches
                state = 2;
                break;
            case 2:
                sp = 16.0;  //16 inches
                state = 3;
                break;
            case 3:
                sp = 20.0;  //22 inches
                state = 4;
                break;
            case 4:
                sp = 13.0;  //13 inches
                state = 0;
                break;
            default:
                sp = 0.0;
                state = 0;
                break;
        } //switch                
        
        //set the set point
        controller.setSetPoint(sp);
        
        float timeLapse = t.read() + 5.0;

        while(t.read() < timeLapse){
            if(t.read() < 0.0)  //if timer goes negative, reset to 0.0
            {
                t.reset();
                timeLapse = t.read() + 5.0;
            }
                
            if(pc.readable())
            {
                char c = pc.getc();                        
                if(c == 'S' || c == 's')
                {                    
                    pc.printf("Enter value for set point\r\n");        
                    pc.scanf("%f",&sp);
                    controller.setSetPoint(sp);
                    pc.printf("%f\r\n",sp);                
                }
                if(c == 'P' || c == 'p')
                {
                    pc.printf("Enter value for Pk\r\n");
                    pc.scanf("%f",&T1_Pk);                
                    pc.printf("%f\r\n",T1_Pk);                                            
                }
                if(c == 'I' || c == 'i')
                {
                    pc.printf("Enter value for Ik\r\n");
                    pc.scanf("%f",&T1_Ik);
                    pc.printf("%f\r\n",T1_Ik);
                    wait(1.0);            
                }                             
                if(c == 'D' || c == 'd')
                {
                    pc.printf("Enter value for Dk\r\n");
                    pc.scanf("%f",&T1_Dk);
                    pc.printf("%f\r\n",T1_Dk);
                    wait(1.0);            
                } 
                if(c == 'T' || c == 't')
                {
                    pc.printf("Enter value for Tdelay\r\n");        
                    pc.scanf("%f",&Tdelay);
                    controller.setInterval(Tdelay);
                    pc.printf("%f\r\n",Tdelay);                
                }

                //Just read the sensors
                if(c == 'R' || c == 'r')
                {                
                    mot= 0.0;
                    sp = 0.0;
                    state = 0;
                    while(1){
                        float dist = get_range();
                        float ir_volts = ir_range * 3.3;
                                                                    
                        pc.printf("%.2f,%.2f\r\n", dist, ir_volts);
                        wait(.05); 
                        if(pc.readable()){
                            char c = pc.getc();
                            break;
                        }                            
                    }             
                    char c = pc.getc();  
                    c = 0;
                }
                controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
                //T1_P = 0.0;
                //T1_I = 0.0; //reset integral    
            }

            dist = get_range();
            ir_volts = ir_range * 3.3;
            
            //Update the process variable.
            controller.setProcessValue(dist);
            //Set the new output.
            co = controller.compute();
        
            mot.mot_control(-co); //-co?
                                               
            led2 = 1;
            
            wait(Tdelay);
            pc.printf("T=%.3f SP=%.3f Dist=%5.2f co=%6.4f Pk=%6.3f Ik=%7.4f\r\n", t.read(), sp, dist, co, T1_Pk, T1_Ik);
            //pc.printf("dc=%7.3f ir range=%7.3f sonar analog=%7.3f pulse=%.1f\r\n", (float)mot, get_range(), sonar_An_range, pulse_dist/147.0f);         
            led2 = 0;       
            
            if(dist > 23.0){
               mot= 0.0;
               
               pc.printf("Caution: Elevator too high.  Lowering for 2 sec.\r\n");
               for(int i=2;i>=0;i--){
                    pc.printf("%d sec left   \r", i);
                    wait(1);
                    if(pc.readable()){
                        char c = pc.getc();
                        if(c=='r')
                            c='r';     
                        else
                            c=0;  
                        break;
                    }
                }                 
                state=0;          
                pc.printf("\r\n");
            }                                       
        } //while(t.read() < timeLapse)   
    } //while(1)
}//main