Revision 21:94aeb8abe04c, committed 2021-09-25

Comitter:

elijahsj

Date:

Sat Sep 25 21:05:54 2021 +0000

Parent:

20:4c172a0737c6

Child:

22:c010476ffeb8

Commit message:

Fixed formatting

Changed in this revision

--- a/main.cpp	Sat Sep 26 00:19:16 2020 +0000
+++ b/main.cpp	Sat Sep 25 21:05:54 2021 +0000
@@ -6,20 +6,22 @@
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
 
-#define NUM_INPUTS 9
-#define NUM_OUTPUTS 6
+#define NUM_INPUTS 3
+#define NUM_OUTPUTS 5
+
+//Measured values
 float velocity = 0;
 float current = 0;
+float theta = 0;
+
+//Set values
 float current_d = 0;
+float kp = 0;
+float ki = 0;
+
+//Controller values
 float volt = 0;
 float duty = 0;
-float cum_error = 0;
-float kp = 0;
-float ki = 0;
-float R =0;
-float emf =0;
-float th_d= 0;
-float period=0;
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
 ExperimentServer server;    // Object that lets us communicate with MATLAB
@@ -39,14 +41,9 @@
     velocity = encoderA.getVelocity()*(6.2831/1200.0);
     current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.15); //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;
-    cum_error = cum_error + error;
-    if (cum_error < -3){
-       cum_error = -3;
-    }
-    else if (cum_error > 3){
-        cum_error = 3;       
-    }
-    volt = R*current_d + emf*velocity + kp * error + ki * (cum_error);
+
+    volt = 0; //CHANGE THIS TO SET CURRENT CONTROLLER VALUES
+    
     duty  = volt/12.0;
     if (duty > 1){
         duty = 1;
@@ -75,29 +72,15 @@
     
     while(1) {
         if (server.getParams(input_params,NUM_INPUTS)) {
-            //float d1   = input_params[0]; // Duty cycle for first second
-            //float d2   = input_params[1]; // Duty cycle for second second
-            //float current_d = input_params[0];
-            //float volt_in = input_params[0];
-            current_d = 0;
-            cum_error = 0;
-            th_d =input_params[0];
-            R = input_params[1];
-            emf = input_params[2];
-            kp = input_params[3];
-            ki = input_params[4];
-            float k = input_params[5];
-            float b = input_params[6];
-            float D = input_params[7];
-            period = input_params[8];
-            
-            ControlLoop.attach(&current_control,period); //start current loop using Ticker function
+            kp = input_params[0];
+            ki = input_params[1];
+            current_d = input_params[2];
+
+            ControlLoop.attach(&current_control,0.0001); //Run current controller at 10khz
             
             // Setup experiment
             t.reset();
             t.start();
-            float th= 0;
-            float error = 0;
             encoderA.reset();
             encoderB.reset();
             encoderC.reset();
@@ -111,24 +94,18 @@
             // Run experiment
             while( t.read() < 5 ) {
                 // Perform control loop logic
-                th = encoderA.getPulses()*(6.2831/1200.0); 
-                //velocity = encoderA.getVelocity()*(6.2831/1200.0);
-                //current = motorShield.readCurrentA()*(30.0/65536.0)-15.1; //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab.
+                // current_d = 0; // Set commanded current from impedance controller here.
                 
-                current_d = k*(th_d-th) - D*velocity + b*velocity;
-
+                // Send data to MATLAB
                 float output_data[NUM_OUTPUTS];
                 output_data[0] = t.read();
-                output_data[1] = th;
+                output_data[1] = theta;
                 output_data[2] = velocity;
                 output_data[3] = current;
-                output_data[4] = current_d;
-                output_data[5] = volt;
-                
-                
-                // Send data to MATLAB
+                output_data[4] = volt;
+
                 server.sendData(output_data,NUM_OUTPUTS);              
-                wait(.001); //run control loop at 1kHz
+                wait(.001); //run outer control loop at 1kHz
             }
             // Cleanup after experiment
             ControlLoop.detach();