Revision 20:8f4838b0d94d, committed 2016-04-20

Comitter:

wretrop

Date:

Wed Apr 20 21:28:16 2016 +0000

Parent:

19:296fb2f473a3

Child:

21:f8b6135b91f9

Commit message:

Latest revision.

Changed in this revision

--- a/PID.lib	Tue Apr 19 18:30:46 2016 +0000
+++ b/PID.lib	Wed Apr 20 21:28:16 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
+http://mbed.org/users/aberk/code/PID/#f61c47297c2c

--- a/main.cpp	Tue Apr 19 18:30:46 2016 +0000
+++ b/main.cpp	Wed Apr 20 21:28:16 2016 +0000
@@ -86,6 +86,11 @@
 float Ksi = 0;
 float Ksd = 0;
 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
+float Ksp1 = 0.9;
+float Ksi1 = 0;
+float Ksd1 = 0;
+PID servo_ctrl1(Ksp1, Ksi1, Ksd1, interrupt_T);
+int ctrl_switch = 15;
 Servo servo(PTA12);                                     // Signal pin (PWM)
 
 // ======
@@ -166,8 +171,9 @@
         bt.printf("  [2] Change Ksp\r\n");
         bt.printf("  [3] Change Ksi\r\n");
         bt.printf("  [4] Change Ksd\r\n");
-        bt.printf("  [5] Change Ksp\r\n");
-        bt.printf("  [6] Change reference velocity\r\n");
+        bt.printf("  [5] Change Ksp1\r\n");
+//        bt.printf("  [6] Change reference velocity\r\n");
+        bt.printf("  [6] Change switching pixel threshold\r\n");
         bt.printf("  [7] EMERGENCY STOP\r\n");
 //        bt.printf("  [8] Timing\r\n");
         bt.printf("  [8] duty += 0.05\r\n");
@@ -192,6 +198,7 @@
                     bt.printf("\r\n");
                     Ksp = atof(comm_in);
                     servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+                    servo_ctrl.reset();
                     comm_cmd = 'q';
                     break;
                 case 3:
@@ -202,7 +209,8 @@
                     }
                     bt.printf("\r\n");
                     Ksi = atof(comm_in);
-                    motor_ctrl.setTunings(Ksp, Ksi, Ksd);
+                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+                    servo_ctrl.reset();
                     comm_cmd = 'q';
                     break;
                 case 4:
@@ -213,7 +221,8 @@
                     }
                     bt.printf("\r\n");
                     Ksd = atof(comm_in);
-                    motor_ctrl.setTunings(Ksp, Ksi, Ksd);
+                    servo_ctrl.setTunings(Ksp, Ksi, Ksd);
+                    servo_ctrl.reset();
                     comm_cmd = 'q';
                     break;
 //                case 2:
@@ -250,26 +259,37 @@
 //                    comm_cmd = 'q';
 //                    break;
                 case 5:
-                    bt.printf("Current: %f, New (8 digits): ", Ksp);
+                    bt.printf("Current: %f, New (8 digits): ", Ksp1);
                     for (int i = 0; i < 8; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
                     bt.printf("\r\n");
-                    Ksp = atof(comm_in);
+                    Ksp1 = atof(comm_in);
+                    servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
                     comm_cmd = 'q';
                     break;
                 case 6:
-                    bt.printf("Current: %f, New (8 digits): ", ref_v);
+                    bt.printf("Current: %d, New (8 digits): ", ctrl_switch);
                     for (int i = 0; i < 8; i++) {
                         comm_in[i] = bt.getc();
                         bt.putc(comm_in[i]);
                     }
                     bt.printf("\r\n");
-                    ref_v = atof(comm_in);
-                    motor_ctrl.setSetPoint(ref_v);
+                    ctrl_switch = atoi(comm_in);
                     comm_cmd = 'q';
                     break;
+//                case 6:
+//                    bt.printf("Current: %f, New (8 digits): ", ref_v);
+//                    for (int i = 0; i < 8; i++) {
+//                        comm_in[i] = bt.getc();
+//                        bt.putc(comm_in[i]);
+//                    }
+//                    bt.printf("\r\n");
+//                    ref_v = atof(comm_in);
+//                    motor_ctrl.setSetPoint(ref_v);
+//                    comm_cmd = 'q';
+//                    break;
                 case 7:
 //                    e_stop = true;
                     motor = 1.0;
@@ -392,8 +412,16 @@
     //        angle = 63;
     //    }
     //    servo = angle / 180;
-        servo_ctrl.setProcessValue(center);
-        angle = 88 + servo_ctrl.compute();
+    
+     //   if (abs(center - 64) > ctrl_switch) {
+//            servo_ctrl1.setProcessValue(center);
+//            angle = 88+servo_ctrl1.compute();
+//            servo_ctrl.reset();
+//        } else {
+            servo_ctrl.setProcessValue(center);
+            angle = 88 + servo_ctrl.compute();
+//            servo_ctrl1.reset();
+       // }
         servo = angle / 180;
         
         // AGC
@@ -416,6 +444,7 @@
             t_int = T_INT_MAX;
         }
         tele_exposure = t_int;
+        servo_ctrl.setInterval(t_int);
         rdyFlag = 1;
         dataFlag = 0;
         //t_main = t.read_us();
@@ -501,6 +530,13 @@
     servo_ctrl.setBias(0.0);
     servo_ctrl.setMode(1);
     
+    servo_ctrl1.setInputLimits(10, 107);
+    //servo_ctrl1.setOutputLimits(-25, 25);
+    servo_ctrl1.setOutputLimits(-30, 30);
+    servo_ctrl1.setSetPoint(63.5);
+    servo_ctrl1.setBias(0.0);
+    servo_ctrl1.setMode(1);
+    
     // Initialize communications thread
     Thread communication_thread(communication);