Revision 17:98e298577f09, committed 2020-09-25

Comitter:

sangbae

Date:

Fri Sep 25 01:07:22 2020 +0000

Parent:

16:bbe4ac38c535

Child:

18:54195aa5e534

Commit message:

This is 2.74 Lab3 example code for impedance control.

Changed in this revision

--- a/main.cpp	Sat Sep 12 16:10:06 2020 +0000
+++ b/main.cpp	Fri Sep 25 01:07:22 2020 +0000
@@ -6,13 +6,24 @@
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
 
-#define NUM_INPUTS 2
-#define NUM_OUTPUTS 4
-
+#define NUM_INPUTS 8
+#define NUM_OUTPUTS 6
+float velocity = 0;
+float current = 0;
+float current_d = 0;
+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;
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
 ExperimentServer server;    // Object that lets us communicate with MATLAB
 Timer t;                    // Timer to measure elapsed time of experiment
+Ticker ControlLoop;
 
 QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
@@ -21,6 +32,36 @@
 
 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ
 
+void current_control()
+{
+    float error = 0;
+    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);
+    duty  = volt/12.0;
+    if (duty > 1){
+        duty = 1;
+    }
+    if (duty <-1){
+        duty = -1;   
+    }
+    if (duty >= 0){
+        motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction 
+    }
+    else if (duty < 0){
+        motorShield.motorAWrite(abs(duty), 1);
+    }
+}
+
+
 int main (void)
 {
     // Link the terminal with our server and start it up
@@ -33,12 +74,28 @@
     
     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 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];
+            
+            ControlLoop.attach(&current_control,0.0001); //start current loop
+            
             // Setup experiment
             t.reset();
             t.start();
+            float th= 0;
+            float error = 0;
             encoderA.reset();
             encoderB.reset();
             encoderC.reset();
@@ -50,27 +107,29 @@
             //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
              
             // Run experiment
-            while( t.read() < 2 ) {
+            while( t.read() < 5 ) {
                 // Perform control loop logic
-                if (t.read() < 1)
-                    motorShield.motorAWrite(d1, 0); //run motor A at "v1" duty cycle and in the forward direction 
-                else
-                    motorShield.motorAWrite(d2, 0); //run motor A at "v2" duty cycle and in the forward direction 
+                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 = k*(th_d-th) - D*velocity + b*velocity;
 
-                // Form output to send to MATLAB
                 float output_data[NUM_OUTPUTS];
-                
                 output_data[0] = t.read();
-                output_data[1] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
-                output_data[2] = 0; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
-                output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
+                output_data[1] = th;
+                output_data[2] = velocity;
+                output_data[3] = current;
+                output_data[4] = current_d;
+                output_data[5] = volt;
+                
                 
                 // Send data to MATLAB
-                server.sendData(output_data,NUM_OUTPUTS);
-                
+                server.sendData(output_data,NUM_OUTPUTS);              
                 wait(.001); //run control loop at 1kHz
             }
             // Cleanup after experiment
+            ControlLoop.detach();
             server.setExperimentComplete();
             motorShield.motorAWrite(0, 0); //turn motor A off
         } // end if