Diff: main.cpp

Revision:

3:c8e53b5762bd

Parent:

1:95a7fddd25a9

Child:

4:300ced917633

--- a/main.cpp	Tue Sep 15 15:40:39 2015 +0000
+++ b/main.cpp	Sat Oct 03 19:42:23 2015 +0000
@@ -4,17 +4,27 @@
 #include "ExperimentServer.h"
 #include "QEI.h"
 
-#define NUM_INPUTS 2
-#define NUM_OUTPUTS 3
+#define NUM_INPUTS 7
+#define NUM_OUTPUTS 6
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
 ExperimentServer server;    // Object that lets us communicate with MATLAB
-PwmOut motorPWM(D5);        // Motor PWM output
-DigitalOut motorFwd(D6);    // Motor forward enable
-DigitalOut motorRev(D7);    // Motor backward enable
+PwmOut motorPWM(D7);        // Motor PWM output
+DigitalOut motorFwd(D5);    // Motor forward enable
+DigitalOut motorRev(D6);    // Motor backward enable
 Timer t;                    // Timer to measure elapsed time of experiment
+Ticker k;                   // Time interval
+AnalogIn Anal(A5);          // AnalogIn port for current sensing
+QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+float Volt = 0;
+float emf  = 0;
+float Current =0;
+float i = 0;
+float  P = 0;
+float vel=0;
+float pot=0;
+float i_d=0;
 
-QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
 
 int main (void) {
     // Link the terminal with our server and start it up
@@ -22,37 +32,96 @@
     server.init();
 
     // PWM period should nominally be a multiple of our control loop
-    motorPWM.period_us(500);
+    motorPWM.period_us(200);
     
     // Continually get input from MATLAB and run experiments
     float input_params[NUM_INPUTS];
     
     while(1) {
         if (server.getParams(input_params,NUM_INPUTS)) {
-            float v1   = input_params[0]; // Voltage for first second
-            float v2   = input_params[1]; // Voltage for second second
-        
+           float tf   = input_params[0]; // Voltage for first second
+           float i_d  = input_params[1]; // Voltage for second second
+           float Kp   = input_params[2];
+           float R   = input_params[3];
+           float Kb   = input_params[4];
+           float Kth   = input_params[5];
+           float b   = input_params[6];
+            //float .0thd  = input_params[3];
+            Volt = 0.0;
+            emf  = 0.0;
+            Current =0.0;
+             i = 0.0;
+              P = 0.0;
+             vel=0.0;
+             pot=0.0;
+             i_d=0.0;
             // Setup experiment
             t.reset();
             t.start();
             encoder.reset();
+            //float thj = 0;
             motorFwd = 1;
             motorRev = 0;
             motorPWM.write(0);
 
             // Run experiment
-            while( t.read() < 2 ) { 
+            while( t.read() < tf ) { 
                 // Perform control loop logic
-                if (t.read() < 1) 
-                    motorPWM.write(v1);
-                else 
-                    motorPWM.write(v2);
+        //void attime(){ 
+            //motorPWM.write(v1);
+                         
+            
+            //k.attach_us(0, 2000);
+            // This is for position based PID
+               //thj = thj + (thd - encoder.getPulses());
+                //float P= Kp*(thd-encoder.getPulses()); 
+                //float I= Ki*(0-encoder.getVelocity());
+                //float D= Kd*thj;
+                // float Volt = P+I+D;
+                // float pulse = encoder.getPulses();
+              
+                
+                //below is for Current Based PID
+                 Current = Anal.read();
+                 vel=encoder.getVelocity()/1200.0*6.28;
+                 pot=encoder.getPulses()/1200.0*6.28;
+                 i_d=(Kth*pot+b*vel)/Kb;
+                 //i_d = 1.5;
+                 i = 3.3f*Current/0.140f;
+                 if (i_d<0){
+                  i=i*-1;
+                  }
+                  P = Kp*(i_d-i);
+                emf = Kb*vel;
+                Volt = (R*i_d+P+emf)/12.0;
+               //float Kb=(Volt-R*i_d)/vel;
+            
+                //float Volt = 0;
+                //pc.printf("  %f  /n",P);
+                if (Volt>0){
+                motorRev.write(1);
+                motorFwd.write(0);
+                motorPWM.write(Volt);
+                }
+                else{
+                motorFwd.write(1); 
+                motorRev.write(0); 
+                motorPWM.write(-1*Volt); 
+                } 
+                
+                //if (t.read() < 1) 
+                   //motorPWM.write(v1);
+                //else 
+                  //  motorPWM.write(v2);
                     
                 // Form output to send to MATLAB    
                 float output_data[NUM_OUTPUTS];
                 output_data[0] = t.read();
-                output_data[1] = encoder.getPulses();
-                output_data[2] = encoder.getVelocity();
+                output_data[1] = encoder.getPulses()/1200.0*6.28;
+                output_data[2] = vel;
+                output_data[3] = Volt;
+                output_data[4] = i;
+                output_data[5] = i_d;
              
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);
@@ -63,4 +132,5 @@
             motorPWM.write(0);
         } // end if
     } // end while
-} // end main
\ No newline at end of file
+
+}// end main
\ No newline at end of file