Revision 2:36cffabd0c01, committed 2018-09-20

Comitter:

jebradshaw

Date:

Thu Sep 20 18:16:16 2018 +0000

Parent:

1:d0f5e3da12cd

Commit message:

ES20X Board Example with the WSE Elevator and the GP2D12 range finder for feedback. Crudely configured gain values.

Changed in this revision

diff -r d0f5e3da12cd -r 36cffabd0c01 main.cpp
--- a/main.cpp	Fri Mar 16 15:49:02 2018 +0000
+++ b/main.cpp	Thu Sep 20 18:16:16 2018 +0000
@@ -1,17 +1,20 @@
 //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, .1);
+PID controller(0.0, 0.0, 0.0, .01);
 Serial pc(USBTX, USBRX);
-AnalogIn ir_range(p19);
+AnalogIn ir_range(p20);
 AnalogIn sonar_range(p16);
 InterruptIn sonar_pulse(p17);
 DigitalIn sonar_pulse_pin(p17);
-MotCon mot(p25, p27);
+MotCon mot(p25, p27, p28);
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 
@@ -20,9 +23,9 @@
 volatile float pulse_dist=0.0;
 
 volatile float co;
-volatile float Tdelay = .1;
-volatile float T1_Pk =2.3;
-volatile float T1_Ik =0.4;
+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;
@@ -52,9 +55,11 @@
         sonar_pulse.rise(&pulse_measure);   //reset interrupt for rising edge
     }
 }
-    
+
 int main() {        
+    wait(.2);
     pc.baud(921600);
+      
     sonar_pulse.rise(&pulse_measure);
     t.start();
     
@@ -65,7 +70,7 @@
     //Distance sensor limits
     controller.setInputLimits(6.0, 24.0);   //0 inches to 26 inches
     //Pwm output from -1.0 to 1.0
-    controller.setOutputLimits(-0.3, 0.4);
+    controller.setOutputLimits(-.72, 0.72);
     //If there's a bias.
     controller.setBias(0.0);
     controller.setMode(AUTO_MODE);
@@ -76,10 +81,11 @@
     controller.setSetPoint(T1_set_point1);
     controller.setTunings(T1_Pk, T1_Ik, T1_Dk);
     
-    int state = 1;
-    float sp = 0.0;
+    int state = 0;
+    float sp = 10.0;
         
     while(1){
+       
         switch(state){
             case 0:
                 sp = 10.0; //10 inches
@@ -94,18 +100,18 @@
                 state = 3;
                 break;
             case 3:
-                sp = 20.0;  //20 inches
+                sp = 20.0;  //22 inches
                 state = 4;
                 break;
             case 4:
-                sp = 23.0;  //23 inches
+                sp = 13.0;  //13 inches
                 state = 0;
                 break;
             default:
                 sp = 0.0;
                 state = 0;
                 break;
-        } //switch
+        } //switch                
         
         //set the set point
         controller.setSetPoint(sp);
@@ -142,6 +148,13 @@
                     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");        
@@ -162,8 +175,10 @@
                                                                     
                         pc.printf("%.2f,%.2f\r\n", dist, ir_volts);
                         wait(.05); 
-                        if(pc.readable())
+                        if(pc.readable()){
+                            char c = pc.getc();
                             break;
+                        }                            
                     }             
                     char c = pc.getc();  
                     c = 0;
@@ -172,7 +187,7 @@
                 //T1_P = 0.0;
                 //T1_I = 0.0; //reset integral    
             }
-            
+
             dist = get_range();
             ir_volts = ir_range * 3.3;
             
@@ -181,12 +196,12 @@
             //Set the new output.
             co = controller.compute();
         
-            mot=-co; //-co?
+            mot.mot_control(-co); //-co?
                                                
             led2 = 1;
             
             wait(Tdelay);
-            pc.printf("T=%.3f SP=%.3f Dist=%.2f co=%.4f Pk=%.3f Ik=%13.10f\r\n", t.read(), sp, dist, co, T1_Pk, T1_Ik);
+            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;       
             
@@ -205,7 +220,8 @@
                             c=0;  
                         break;
                     }
-                }                           
+                }                 
+                state=0;          
                 pc.printf("\r\n");
             }                                       
         } //while(t.read() < timeLapse)