Diff: main.cpp

Revision:

27:a4228ea8fb8f

Parent:

26:4f84448b4d46

Child:

28:8cd898ff43a2

--- a/main.cpp	Fri Oct 06 13:11:24 2017 +0000
+++ b/main.cpp	Fri Oct 13 11:59:15 2017 +0000
@@ -4,11 +4,20 @@
 #include "QEI.h"
 #include "FastPWM.h"
 
+// Defining relevant constant parameters
+const float gearRatio = 131;
+
+// Controller parameters
+const float k_p = 0.5;
+const float k_i = 0; // Still needs a reasonable value
+const float k_d = 0; // Again, still need to pick a reasonable value
+
 enum robotStates {KILLED, ACTIVE};
 robotStates currentState = KILLED;
 
 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(5);
 
 // Defining outputs
 
@@ -24,6 +33,7 @@
 InterruptIn sw2(SW2);
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
+InterruptIn button2(D4);
 AnalogIn pot1(A0);
 AnalogIn pot2(A1);
 
@@ -31,9 +41,28 @@
 float pwmPeriod = 1.0/5000.0;
 int frequency_pwm = 10000; //10kHz PWM
 
+
+// Setting up HIDscope
 volatile float x;
-volatile float x_prev =0; 
-volatile float y; // filtered 'output' of ReadAnalogInAndFilter 
+volatile float y;  
+volatile float z; 
+volatile float q;
+volatile float k;
+
+void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
+        // Capture data
+        x = data1;
+        y = data2;
+        z = data3;
+        q = data4;
+        k = data5;
+        scope.set(0, x);   
+        scope.set(1, y); 
+        scope.set(2, z);
+        scope.set(3, q);
+        scope.set(4, z);
+        scope.send(); // send what's in scope memory to PC
+}
 
 // Initializing encoder
 Ticker encoderTicker;
@@ -42,53 +71,62 @@
 volatile float revs = 0.00;
 
 // MOTOR CONTROL PART    
-bool m1_direction = false;
-int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
+bool r_direction = false;
+int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
 const float Ts = 0.1;
 
-// Function getReferencePosition returns reference angle within range
+// Function getReferencePosition returns reference angle based on potmeter 1
 float getReferencePosition(){
-    double r;
-    if(m1_direction == false){
+    float r;
+    if(r_direction == false){
         // Clockwise rotation yields positive reference
         r = maxAngle*pot1.read();
         }
-    if(m1_direction == true){
+    if(r_direction == true){
         // Counterclockwise rotation yields negative reference
         r = -1*maxAngle*pot1.read();
         }
     return r;
     }
 
-// Initializing position and integral errors
+// Initializing position and integral errors to zero
 float e_pos = 0;
 float e_int = 0;
 float e_der = 0;
 float e_prev = 0;
-float e = e_pos + e_int + e_der;
 
 // readEncoder reads counts and revs and logs results to serial window
 void getError(float &e_pos, float &e_int, float &e_der){
+    // Getting encoder counts and calculating motor position
     counts = Encoder.getPulses();
-    double motor1Position = 2*3.14*(counts/(131*64.0f));
-    pc.printf("%0.2f revolutions \r\n", motor1Position);
+    double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));
+
+    // Computing position error
     e_pos = getReferencePosition() - motor1Position;
-    e_int = e_int + Ts*e_pos;
+    
+    // Limiting the integral error to prevent integrator saturation
+    if(fabs(e_int) <= 5){
+        e_int = e_int + Ts*e_pos;    
+        }
+    
+    // Derivative error   
     e_der = (e_pos - e_prev)/Ts;
-    e = e_pos + e_int + e_der; // not sure if this is really even all that useful
-    e_prev = e_pos; // Store current error as we'll need it to compute the next derivative error
-    pc.printf("Error is %0.2f \r \n", e);
+    e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
     }
 
-const float k_p = 0.1; // use potmeter 2 to adjust k_p within range 0 to 1
-const float k_i = 0.05; // Still needs a reasonable value
-const float k_d = 0.01; // Again, still need to pick a reasonable value
-
 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
-float motorController(float e_pos, float e_int, float e_der){    
-    // NOTE: do I still need the maxangle bit here?
-    double motorValue = (e_pos*k_p) + 0.35 + k_i*e_int + k_d*e_der; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
+float motorController(float e_pos, float e_int, float e_der){
+    float motorValue;
+    if(e_pos >= 0){
+        // Positive error yields positive motor value = counterclockwise rotation
+        motorValue = -1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
+        }    
+    else {
+        // Negative error yields negative motor value = clockwise rotation
+        motorValue = 1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
+     }
+     
     return motorValue;
     }
     
@@ -126,12 +164,8 @@
 void measureAndControl(){
     getError(e_pos, e_int, e_der);
     float motorValue = motorController(e_pos, e_int, e_der);
-    pc.printf("Position action is %.2f \r \n", k_p*e_pos);
-    pc.printf("Derivative action is %.2f \r \n", k_d*e_der);
-    pc.printf("Integral action is %.2f", k_i*e_int);
-    pc.printf("Total action is %.2f", k_p*e_pos + k_d*e_der + k_i*e_int);
-    pc.printf("Motorvalue is %.2f \r \n", motorValue);
-    pc.printf("Actual error is %.2f \r \n", e_pos);
+    float r = getReferencePosition();
+    sendDataToPc(r, e_pos, e_int, e_der, motorValue);
     setMotor1(motorValue);
     }
 
@@ -145,14 +179,12 @@
     currentState = ACTIVE; 
     }
 
-    
-void M1switchDirection(){
-    m1_direction = !m1_direction;
-    pc.printf("Switched direction!");
+void rSwitchDirection(){
+    r_direction = !r_direction;
+    pc.printf("Switched reference direction! \r\n");
     }
 
 
-
 int main()
     {
     pc.printf("Main function");
@@ -160,7 +192,7 @@
     motor1_pwm.period(pwmPeriod);//T=1/f 
     sw2.fall(&killSwitch);
     sw3.fall(&turnMotorsOn);
-    button1.rise(&M1switchDirection);
+    button2.rise(&rSwitchDirection);
     pc.baud(115200);
     controllerTicker.attach(measureAndControl, Ts);