main.cpp

00001 //mbed ES201 Dev Board (V1.1) Elevator Tester
00002 // Pololu Motor - https://www.pololu.com/product/2821
00003 // 12 V: 11,000 RPM and 300 mA free-run, 5 oz-in (0.3 kg-cm) and 5 A stall
00004 
00005 #include "mbed.h"
00006 #include "MotCon.h"
00007 #include "PID.h"
00008 
00009 #define VCC 5.0f
00010 
00011 PID controller(0.0, 0.0, 0.0, .01);
00012 Serial pc(USBTX, USBRX);
00013 AnalogIn ir_range(p20);
00014 AnalogIn sonar_range(p16);
00015 InterruptIn sonar_pulse(p17);
00016 DigitalIn sonar_pulse_pin(p17);
00017 MotCon mot(p25, p27, p28);
00018 DigitalOut led1(LED1);
00019 DigitalOut led2(LED2);
00020 
00021 Timer t;
00022 Timer pulse_t;
00023 volatile float pulse_dist=0.0;
00024 
00025 volatile float co;
00026 volatile float Tdelay = .001;
00027 volatile float T1_Pk =1.6f;
00028 volatile float T1_Ik =0.45f;
00029 volatile float T1_Dk =0.0;
00030 volatile float T1_set_point1 = 0.0;
00031 volatile float dist, ir_volts;
00032     
00033 float get_range(void){
00034     double v_avg=0.0;
00035     for(int i=0;i<100;i++){
00036         v_avg += ir_range * 3.3;
00037     }    
00038     double volts = v_avg/100.0;    
00039     //Employ range equation here
00040     double distance = (4.5*pow(volts,3))-(27.0*pow(volts, 2))+(57.0*volts)-19.0;    
00041     
00042     //double distance = volts;
00043     return distance;
00044 }
00045 
00046 void pulse_measure(void){
00047     if(sonar_pulse_pin){    //if pulse pin is high
00048         pulse_t.start();    //start the pulse timer
00049         sonar_pulse.fall(&pulse_measure);   //set next interrupt for falling edge
00050     }
00051     else{                   //if pulse pin is low
00052         pulse_dist = pulse_t.read_us();    //read the pulse timer
00053         pulse_t.stop();                 //stop the timer
00054         pulse_t.reset();                //reset timer to 0
00055         sonar_pulse.rise(&pulse_measure);   //reset interrupt for rising edge
00056     }
00057 }
00058 
00059 int main() {        
00060     wait(.2);
00061     pc.baud(921600);
00062       
00063     sonar_pulse.rise(&pulse_measure);
00064     t.start();
00065     
00066     //resets the controllers internals
00067     controller.reset();
00068     pc.printf("Waiting 2 seconds\r\n");
00069     wait(2.0);
00070     //Distance sensor limits
00071     controller.setInputLimits(6.0, 24.0);   //0 inches to 26 inches
00072     //Pwm output from -1.0 to 1.0
00073     controller.setOutputLimits(-.72, 0.72);
00074     //If there's a bias.
00075     controller.setBias(0.0);
00076     controller.setMode(AUTO_MODE);
00077     
00078     controller.setInterval(Tdelay);
00079       
00080     //We want the process variable to be 0.0 at start
00081     controller.setSetPoint(T1_set_point1);
00082     controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
00083     
00084     int state = 0;
00085     float sp = 10.0;
00086         
00087     while(1){
00088        
00089         switch(state){
00090             case 0:
00091                 sp = 10.0; //10 inches
00092                 state = 1;
00093                 break;
00094             case 1:
00095                 sp = 13.0;   //13 inches
00096                 state = 2;
00097                 break;
00098             case 2:
00099                 sp = 16.0;  //16 inches
00100                 state = 3;
00101                 break;
00102             case 3:
00103                 sp = 20.0;  //22 inches
00104                 state = 4;
00105                 break;
00106             case 4:
00107                 sp = 13.0;  //13 inches
00108                 state = 0;
00109                 break;
00110             default:
00111                 sp = 0.0;
00112                 state = 0;
00113                 break;
00114         } //switch                
00115         
00116         //set the set point
00117         controller.setSetPoint(sp);
00118         
00119         float timeLapse = t.read() + 5.0;
00120 
00121         while(t.read() < timeLapse){
00122             if(t.read() < 0.0)  //if timer goes negative, reset to 0.0
00123             {
00124                 t.reset();
00125                 timeLapse = t.read() + 5.0;
00126             }
00127                 
00128             if(pc.readable())
00129             {
00130                 char c = pc.getc();                        
00131                 if(c == 'S' || c == 's')
00132                 {                    
00133                     pc.printf("Enter value for set point\r\n");        
00134                     pc.scanf("%f",&sp);
00135                     controller.setSetPoint(sp);
00136                     pc.printf("%f\r\n",sp);                
00137                 }
00138                 if(c == 'P' || c == 'p')
00139                 {
00140                     pc.printf("Enter value for Pk\r\n");
00141                     pc.scanf("%f",&T1_Pk);                
00142                     pc.printf("%f\r\n",T1_Pk);                                            
00143                 }
00144                 if(c == 'I' || c == 'i')
00145                 {
00146                     pc.printf("Enter value for Ik\r\n");
00147                     pc.scanf("%f",&T1_Ik);
00148                     pc.printf("%f\r\n",T1_Ik);
00149                     wait(1.0);            
00150                 }                             
00151                 if(c == 'D' || c == 'd')
00152                 {
00153                     pc.printf("Enter value for Dk\r\n");
00154                     pc.scanf("%f",&T1_Dk);
00155                     pc.printf("%f\r\n",T1_Dk);
00156                     wait(1.0);            
00157                 } 
00158                 if(c == 'T' || c == 't')
00159                 {
00160                     pc.printf("Enter value for Tdelay\r\n");        
00161                     pc.scanf("%f",&Tdelay);
00162                     controller.setInterval(Tdelay);
00163                     pc.printf("%f\r\n",Tdelay);                
00164                 }
00165 
00166                 //Just read the sensors
00167                 if(c == 'R' || c == 'r')
00168                 {                
00169                     mot= 0.0;
00170                     sp = 0.0;
00171                     state = 0;
00172                     while(1){
00173                         float dist = get_range();
00174                         float ir_volts = ir_range * 3.3;
00175                                                                     
00176                         pc.printf("%.2f,%.2f\r\n", dist, ir_volts);
00177                         wait(.05); 
00178                         if(pc.readable()){
00179                             char c = pc.getc();
00180                             break;
00181                         }                            
00182                     }             
00183                     char c = pc.getc();  
00184                     c = 0;
00185                 }
00186                 controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
00187                 //T1_P = 0.0;
00188                 //T1_I = 0.0; //reset integral    
00189             }
00190 
00191             dist = get_range();
00192             ir_volts = ir_range * 3.3;
00193             
00194             //Update the process variable.
00195             controller.setProcessValue(dist);
00196             //Set the new output.
00197             co = controller.compute();
00198         
00199             mot.mot_control(-co); //-co?
00200                                                
00201             led2 = 1;
00202             
00203             wait(Tdelay);
00204             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);
00205             //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);         
00206             led2 = 0;       
00207             
00208             if(dist > 23.0){
00209                mot= 0.0;
00210                
00211                pc.printf("Caution: Elevator too high.  Lowering for 2 sec.\r\n");
00212                for(int i=2;i>=0;i--){
00213                     pc.printf("%d sec left   \r", i);
00214                     wait(1);
00215                     if(pc.readable()){
00216                         char c = pc.getc();
00217                         if(c=='r')
00218                             c='r';     
00219                         else
00220                             c=0;  
00221                         break;
00222                     }
00223                 }                 
00224                 state=0;          
00225                 pc.printf("\r\n");
00226             }                                       
00227         } //while(t.read() < timeLapse)   
00228     } //while(1)
00229 }//main