Diff: main.cpp

Revision:

19:d79692cef6c7

Parent:

18:19d35daed140

Child:

20:d796667e0c4d

--- a/main.cpp	Fri Mar 24 19:31:07 2017 +0000
+++ b/main.cpp	Fri Mar 24 19:47:50 2017 +0000
@@ -64,7 +64,7 @@
 
 //PID Controller 
 PID velocity_pid(0.75, 0.025, 0.2, 0.1);  // (P, I, D, WAIT)
-PID dist_pid(10, 0.0, 0.01, 0.1);        // (P, I, D, WAIT)
+PID dist_pid(2, 0.0, 0.01, 0.1);        // (P, I, D, WAIT)
 
 //Initialize Threads 
 Thread pid_thread(osPriorityNormal, 512, NULL);
@@ -170,7 +170,7 @@
 void calculatePID(){
     TIME = 1;
     //Calculate new PID Control Point
-    if(0 && (total_rev/rev_target) > 0.75f){
+    if((total_rev/rev_target) > 0.75f){
         dist_pid.setProcessValue(total_rev);
         dutyout = dist_pid.compute();
     }
@@ -228,18 +228,20 @@
     
     pc.printf("Waiting for stabilize... %d\r\n", direction); 
     dutyout = 0.0f;
+    velocity_pid.reset();
+    dist_pid.reset();
     wait(3);
     motorHome();
     pc.printf("Restarting... %d\r\n", direction); 
 
     velocity_pid.reset();
-    velocity_pid.setInputLimits(0.0, 50);
+    //velocity_pid.setInputLimits(0.0, 50);
     velocity_pid.setOutputLimits(0.0, 1.0);
     velocity_pid.setMode(1);
     velocity_pid.setSetPoint(vel_target);
     
     dist_pid.reset();
-    dist_pid.setInputLimits(0.0, rev_target);
+    //dist_pid.setInputLimits(0.0, rev_target);
     dist_pid.setOutputLimits(0.0, 1.0);
     dist_pid.setMode(1);
     dist_pid.setSetPoint(rev_target);
@@ -249,7 +251,7 @@
     driveto = (intState-orState+(direction*lead)+6)%6; 
     motorOut(driveto);
     
-    calcPID_timer.attach(&calculatePID, 0.1);
+    calcPID_timer.attach(&calculatePID, 0.05);
 }
 
 void changestate_isr(){
@@ -473,10 +475,6 @@
                     v=atof(input.substr(1).c_str());
                     pc.printf("Spin at %.3f speed\r\n",v);
                     position_control(std::numeric_limits<float>::max(),(float)v);
-                    while(!completed){
-                        pc.printf("Duty Cycle: %.3f\r\n partial_val: %.3f\r\n", dutyout, partial_vel);    
-                        wait(1);
-                    }
                 }
                 else{
                     pc.printf("Invalid input\r\n");
@@ -498,7 +496,7 @@
     
     //Run the motor synchronisation: orState is subtracted from future rotor state inputs
     orState = motorHome();
-    pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %x\n\r",orState);
+    pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %d \r\n",orState);
     
     //Interrupts (Optical Disk State Change): Drives to next state, Measures whole revolution count, Measures angular velocity over a whole revolution 
     InterruptI1.rise(&changestate_isr);