Diff: main.cpp

Revision:

25:f5eaea12412f

Parent:

24:4d2f1b7746de

--- a/main.cpp	Mon Sep 27 19:55:59 2021 +0000
+++ b/main.cpp	Thu Dec 09 23:53:19 2021 +0000
@@ -6,7 +6,7 @@
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
 
-#define NUM_INPUTS 3
+#define NUM_INPUTS 6
 #define NUM_OUTPUTS 5
 
 //Measured values
@@ -18,6 +18,10 @@
 float current_d = 0;
 float kp = 0;
 float ki = 0;
+float K = 0;
+float b = 0;
+float kb = .2;
+float D = 0;
 
 //Controller values
 float volt = 0;
@@ -37,13 +41,30 @@
 
 void current_control()   //Current control function
 {
+    float input_params[NUM_INPUTS];
+    
     float error = 0;
+    float sum_error = 0;
+    float limit = 100;
     theta = encoderA.getPulses()*(6.2831/1200.0);
     velocity = encoderA.getVelocity()*(6.2831/1200.0);
     current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.0); //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab.            
     error = current_d - current;
+    float R = 6.0/2.777;
+    float kb = .2;
+    
 
-    volt = 0; //CHANGE THIS TO SET CURRENT CONTROLLER VALUES
+    if (sum_error > limit) {
+        sum_error = limit;
+    }
+    else if (sum_error < -limit) {
+        sum_error = limit;
+    }
+    else {
+      sum_error = sum_error + error;
+    }  
+
+    volt = kp*error + ki*sum_error + current_d*R + kb*velocity; //CHANGE THIS TO SET CURRENT CONTROLLER VALUES
     
     duty  = volt/12.0;
     if (duty > 1){
@@ -75,13 +96,17 @@
         if (server.getParams(input_params,NUM_INPUTS)) {
             kp = input_params[0];
             ki = input_params[1];
-            current_d = input_params[2];
+            K = input_params[2];
+            b = input_params[3];
+            D = input_params[4];
+            current_d = input_params[5];
 
             ControlLoop.attach(&current_control,0.0001); //Run current controller at 10khz
             
             // Setup experiment
             t.reset();
             t.start();
+            
             encoderA.reset();
             encoderB.reset();
             encoderC.reset();
@@ -95,7 +120,8 @@
             // Run experiment
             while( t.read() < 5 ) {
                 // Perform control loop logic
-                //current_d = 0; // Set commanded current from impedance controller here.
+                current_d = (-K*theta + b*velocity - D*velocity)/kb; // Set commanded current from impedance controller here.
+                //current_d = sin(3*t.read());
                 
                 // Send data to MATLAB
                 float output_data[NUM_OUTPUTS];
@@ -105,7 +131,8 @@
                 output_data[3] = current;
                 output_data[4] = volt;
 
-                server.sendData(output_data,NUM_OUTPUTS);              
+                server.sendData(output_data,NUM_OUTPUTS);      
+                        
                 wait(.001); //run outer control loop at 1kHz
             }
             // Cleanup after experiment