Diff: main.cpp

Revision:

17:e10e5ad24842

Parent:

16:bbe4ac38c535

--- a/main.cpp	Sat Sep 12 16:10:06 2020 +0000
+++ b/main.cpp	Wed Sep 16 20:04:38 2020 +0000
@@ -5,9 +5,20 @@
 #include "QEI.h"
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
+//#include<iostream> // to use min function
+//#include<algorithm>// to use min function 
+//using namespace std; 
 
-#define NUM_INPUTS 2
-#define NUM_OUTPUTS 4
+#define NUM_INPUTS  5
+#define NUM_OUTPUTS 5
+#define DT 0.001
+
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846
+#endif
+
+#define Vnom           12.0
 
 
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
@@ -26,15 +37,25 @@
     // Link the terminal with our server and start it up
     server.attachTerminal(pc);
     server.init();
-    
+    float V; //input voltage
+    float d; // duty cycle
+    float error;
+    float last_error=0;
+    float sum_error=0;
+    float duration =3;
+    int cst;
     // Continually get input from MATLAB and run experiments
     float input_params[NUM_INPUTS];
     pc.printf("%f",input_params[0]);
     
     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 theta_d   = input_params[0]; // desired angle degrees
+            float Kp   = input_params[1]; // Kp
+            float Kd   = input_params[2]; // Kp
+            float Ki   = input_params[3]; // Kp
+            float duration = input_params[4]; //
+            int dir;
 
             // Setup experiment
             t.reset();
@@ -50,25 +71,74 @@
             //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
              
             // Run experiment
-            while( t.read() < 2 ) {
+            
+            
+            /*while( t.read() < 2 ) {
                 // Perform control loop logic
                 if (t.read() < 1)
-                    motorShield.motorAWrite(d1, 0); //run motor A at "v1" duty cycle and in the forward direction 
+                    motorShield.motorAWrite(0.5, 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 
+                    motorShield.motorAWrite(0.5, 1); //run motor A at "v2" duty cycle and in the forward direction */
 
                 // Form output to send to MATLAB
                 float output_data[NUM_OUTPUTS];
+    /*            output_data[0] = t.read();
+                output_data[1] = encoderA.getPulses()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
+                output_data[2] = encoderA.getVelocity()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
+                output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
+                output_data[4] = V_nom*0.5;*/
                 
-                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;
+                
+                
+                
+                  while( t.read() < duration ) {
+                    output_data[0] = t.read();
+                    output_data[1] = encoderA.getPulses()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
+                    output_data[2] = encoderA.getVelocity()*2*M_PI/1200; //MODIFY: COPY IN YOUR ANSWER FROM PART 1
+                    output_data[3] = motorShield.readCurrentA()*(30.0/65536.0)-15;
+                    
+                    
+                    error = (theta_d-output_data[1]);
+                    
+                    
+                    V = Kp*error + Kd*(error-last_error)/DT + Ki*sum_error; //(-output_data[2]); //PID controller gives voltage to apply
+                    //output_data[4] = V;
+                    last_error = error;
+                    sum_error += error;
+                    // convert to duty cycle
+                    if (V>0){
+                        dir = 0;
+                        d = V/Vnom;
+                        cst=1;
+                        }
+                    else { // V<=0;
+                        dir = 1;
+                        d = -V/Vnom;// make sure this is positive
+                        cst = -1;
+                        //
+                        }
+                    
+                    if (d>=1){
+                        d=1;
+                        }
+                        
+                  
+                    output_data[4]=d*Vnom*cst;
+                    //d = std::min(d,1) // ceil the value of the duty cycle;
+                    
+                // Perform control loop logic
+                     //if (t.read() < 1)
+                        motorShield.motorAWrite(d, dir); //run motor A at "v1" duty cycle and in the forward direction 
+                    // else
+                        
+
+                
+                
                 
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);
                 
-                wait(.001); //run control loop at 1kHz
+                wait(DT); //run control loop at 1kHz
             }
             // Cleanup after experiment
             server.setExperimentComplete();