Diff: main.cpp

Revision:

3:3c00f1332d9b

Parent:

1:95a7fddd25a9

--- a/main.cpp	Tue Sep 15 15:40:39 2015 +0000
+++ b/main.cpp	Tue Dec 05 02:37:28 2017 +0000
@@ -4,17 +4,153 @@
 #include "ExperimentServer.h"
 #include "QEI.h"
 
-#define NUM_INPUTS 2
-#define NUM_OUTPUTS 3
+#define NUM_INPUTS 10
+#define NUM_OUTPUTS 9
 
+//leg 50 deg
+//tail 45 deg
+
+Ticker control;
 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
-Timer t;                    // Timer to measure elapsed time of experiment
+
+// Tail Motor Control Pins
+PwmOut tailPWM(D5);        // Tail motor PWM output
+DigitalOut tailFwd(D6);    // Tail motor forward enable
+DigitalOut tailRev(D7);    // Tail motor backward enable
+
+// Leg Motor Control Pins
+PwmOut legPWM(D11);         // Leg motor PWM output
+DigitalOut legFwd(D10);    // Leg motor forward enable
+DigitalOut legRev(D9);     // Leg motor backward enable
+
+Timer t;                   // Timer to measure elapsed time of experiment
+float time_ = 0.0;               // time variable
+float tfinal = 20;
+
+// Motor current sensors
+AnalogIn tailCurrent(A0);  // Tail current sensor
+AnalogIn legCurrent(A1);   // Leg current sensor 
+
+// Motor Encoders
+QEI tailEncoder(D1 ,D2 ,NC ,1200, QEI::X4_ENCODING); // Tail encoder 
+QEI legEncoder(D3 ,D4 ,NC ,1200, QEI::X4_ENCODING);  // Leg encoder
+
+// Physical motor parameters
+float R = 2.79;                     // Estimated motor resistance from lab
+float Kb = .1603;                   // Estimated motor torque constant measured from lab
+float Kcurrent = 1.5;                 // Proportional gain of the current control 
+float v = 0.01;                     // BackEMF constant
+
+
+// Matlab inputs/Control variables
+float tailCmd = 0.0;                  // Commanded angle (Rads) for the tail
+float legCmd = 0.0;                   // Commanded angle (Rads) for the leg
+float tailt0 = 0.0;                   // Initial angle (Rads)
+float legt0 = 0.0;                    // Initial angle (Rads)
+float Kptail = 0.0;                   // Tail proportional gain of position control
+float Kpleg = 0.0;                    // Leg proportional gain of position control
+float Kdtail = 0.0;                   // Tail derivative gain of position control
+float Kdleg = 0.0;                    // Leg derivative gain for position control
+
+// State varibles/sensed parameters
+float idtail = 0.0;                   // Tail desired current
+float idleg = 0.0;                    // Leg desired current
+float taild = 0.0;                    // Desired angle (Rads) for the tail
+float legd = 0.0;                     // Desired angle (Rads) for the leg
+float wtail = 0.0;                    // Measured angular velocity of the tail
+float wleg = 0.0;                     // Measured angular velocity of the leg
+float ttail = 0.0;                    // Measured angular position of the tail
+float tleg = 0.0;                     // Measured angular position of the leg
+float itail = 0.0;                    // Current sensed in the tail motor
+float ileg = 0.0;                     // Current sensed in the leg motor
+float vctail = 0.0;                   // Tail voltage command
+float vcleg = 0.0;                    // Leg voltage command
+
+// Error terms
+float etail = 0.0;                    // Error term for tail
+float eleg = 0.0;                     // Error term for leg
+float eptail = 0.0;                   // Previous error term for tail
+float epleg = 0.0;                    // Previous error term for leg
+float edtail = 0.0;                   // Derivative error term for tail
+float edleg = 0.0;                    // Derivative error term for leg
+
+float max_cmd = 1.0;
+
+bool new_data = false;
+bool end = false;
+
 
-QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+void control_loop() {
+    if (end){
+        tailPWM.write(0);
+        legPWM.write(0);
+        }    
+    else{
+        //Sensing
+        itail = (tailCurrent.read()-.5)*36.7;                                 // Tail current
+        ileg = (legCurrent.read()-.5)*36.7;                                   // Leg current
+        ttail = tailEncoder.getPulses()/1200.0*2*3.14159 + tailt0;            // Leg angle
+        tleg = legEncoder.getPulses()/1200.0*2*3.14159 + legt0;               // Leg angle
+        wtail = tailEncoder.getVelocity()*2*3.14159/1200.0;                   // Tail angular velocity
+        wleg = legEncoder.getVelocity()*2*3.14159/1200.0;                     // Leg angular velocity
+        time_ = t.read();
+        
+        // Error update
+        etail = taild - ttail;                                  // Error in tail's angle position
+        eleg = legd - tleg;                                     // Error in leg's angle position
+        edtail = (etail - eptail)/.001;                         // Rate of change in tail's error
+        edleg = (eleg - epleg)/.001;                            // Rate of change in leg's error
+        eptail = etail;                                         // Update previous tail error
+        epleg = eleg;                                           // Update previous leg error
+        
+        // Perform control loop logic
+        idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb;         // Tail position control
+        idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb;              // Leg position control
+        
+        vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0;  // Tail current control
+        vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0;      // Leg current control
+        
+        // Tail set command
+        if (vctail < 0){
+            tailFwd = 1;
+            tailRev = 0;
+            if (abs(vctail) > 1){
+                vctail = -1;
+            }
+            tailPWM.write(max_cmd*abs(vctail));
+        }else if (vctail > 0){
+            tailFwd = 0;
+            tailRev = 1;
+            if (abs(vctail) > 1){
+                vctail = 1;
+            }
+            tailPWM.write(max_cmd*abs(vctail));
+        } else {
+            tailPWM.write(0);
+        }      
+        
+        // Leg set command
+        if (vcleg < 0){
+            legFwd = 1;
+            legRev = 0;
+            if (abs(vcleg) > 1){
+                vcleg = -1;
+            }
+            legPWM.write(max_cmd*abs(vcleg));
+        }else if (vcleg > 0){
+            legFwd = 0;
+            legRev = 1;
+            if (abs(vcleg) > 1){
+                vcleg = 1;
+            }
+            legPWM.write(max_cmd*abs(vcleg));
+        } else {
+            legPWM.write(0);
+        }   
+        new_data = true;         
+        }
+    }
 
 int main (void) {
     // Link the terminal with our server and start it up
@@ -22,45 +158,82 @@
     server.init();
 
     // PWM period should nominally be a multiple of our control loop
-    motorPWM.period_us(500);
+    tailPWM.period_us(500);
+    legPWM.period_us(500);
     
     // Continually get input from MATLAB and run experiments
     float input_params[NUM_INPUTS];
+    control.attach(&control_loop, .001);
+    
     
     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
-        
+            tailCmd = input_params[0];                   // Desired angle (Rads) for the tail
+            legCmd = input_params[1];                    // Desired angle (Rads) for the leg
+            tailt0 = input_params[2];                    // Initial angle (Rads)
+            legt0 = input_params[3];                     // Initial angle (Rads)
+            Kptail = input_params[4];                    // Tail proportional gain of position control
+            Kpleg = input_params[5];                     // Leg proportional gain of position control
+            Kdtail = input_params[6];                    // Tail derivative gain of position control
+            Kdleg = input_params[7];                     // Leg derivative gain for position control
+            max_cmd = input_params[8];                   // cmd thresholder
+            tfinal = input_params[9];
+            
+            if (max_cmd > 1.0 || max_cmd < 0){
+                max_cmd = 1.0;
+                }
             // Setup experiment
             t.reset();
             t.start();
-            encoder.reset();
-            motorFwd = 1;
-            motorRev = 0;
-            motorPWM.write(0);
-
+            end = false;
+            
+            // Reset the encoders
+            tailEncoder.reset();
+            legEncoder.reset();
+            
+            // Default forward rotation
+            tailFwd = 1;
+            tailRev = 0;
+            legFwd = 1;
+            legRev = 0;
+            
+            // Stop the motors
+            tailPWM.write(0);
+            legPWM.write(0);
+            
             // Run experiment
-            while( t.read() < 2 ) { 
-                // Perform control loop logic
-                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();
-             
+            while( t.read() < tfinal) { 
+                // Set desired parameters at specific times
+                if(t.read()<5){
+                    taild = tailt0;
+                    legd = legt0;
+                }else{
+                    taild = tailCmd;
+                    legd = legCmd;
+                }
+                
                 // Send data to MATLAB
-                server.sendData(output_data,NUM_OUTPUTS);
-                wait(.001); 
+                if(new_data) {
+                    // Form output to send to MATLAB    
+                    float output_data[NUM_OUTPUTS];
+                    output_data[0] = time_;
+                    output_data[1] = vctail;
+                    output_data[2] = vcleg;
+                    output_data[3] = itail;
+                    output_data[4] = ileg;
+                    output_data[5] = ttail;
+                    output_data[6] = tleg;
+                    output_data[7] = wtail;
+                    output_data[8] = wleg;
+                    server.sendData(output_data,NUM_OUTPUTS);
+                    new_data = false;
+                }
             }     
+            end = true;  
             // Cleanup after experiment
             server.setExperimentComplete();
-            motorPWM.write(0);
+            tailPWM.write(0);
+            legPWM.write(0);
         } // end if
     } // end while
 } // end main
\ No newline at end of file