Diff: main.cpp

Revision:

21:dd51d78c0732

Parent:

20:201175fa8a2a

Child:

22:c679f753a8bd

--- a/main.cpp	Tue Oct 25 14:39:47 2016 +0000
+++ b/main.cpp	Tue Oct 25 15:12:28 2016 +0000
@@ -52,8 +52,7 @@
 float dx;
 float dy;
 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
-float motorMaxGain1 = 0.1;
-float motorMaxGain2 = 0.1;
+float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
 float t_sample = 0.01; //seconds
 //float referenceVelocity = 0;
 //float bqcDerivativeCounts = 0;
@@ -90,6 +89,9 @@
 float q2_error_prev = 0;
 float q1IntError = 0;
 float q2IntError = 0;
+float TotalError1= 0;
+float TotalError2= 0;
+float TotalErrorMin= 0;
 
 float motorValue1 = 0.0;
 float motorValue2 = 0.0;
@@ -292,8 +294,25 @@
     //scope.set(2,Ki);
     //scope.send();
     
-    motorValue1Out = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;    //total controller output = motor input
-    motorValue2Out = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;    //total controller output = motor input
+    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output = motor input
+    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output = motor input
+    
+    if (TotalError1 < TotalErrorMin) {
+        TotalError1=0;
+        }
+    else {
+        TotalError1=TotalError1;
+        }
+    if (TotalError2 < TotalErrorMin) {
+        TotalError2=0;
+        }
+    else {
+        TotalError2=TotalError2;
+        }
+    
+    motorValue1Out = TotalError1/MotorGain;       
+    motorValue2Out = TotalError2/MotorGain;       
+    
     //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
     //pc.printf("Counts encoder1: %i rad \r\n", counts1);
     //pc.printf("Kp: %f \r\n", Kp);
@@ -337,8 +356,8 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue1)>1) motor1MagnitudePin = motorMaxGain1;
-        else motor1MagnitudePin = motorMaxGain1*fabs(motorValue1);  //fabs(motorValue1);
+    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue1);  //fabs(motorValue1);
     //control motor 2
     if (motorValue2 >=0)  //clockwise rotation
         {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
@@ -350,8 +369,8 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue2)>1) motor2MagnitudePin = motorMaxGain2;
-        else motor2MagnitudePin = motorMaxGain2*fabs(motorValue2);  //fabs(motorValue1);
+    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
+        else motor2MagnitudePin = fabs(motorValue2);  //fabs(motorValue1);
         float ReadMagn1 = motor1MagnitudePin.read();
         float ReadMagn2 = motor2MagnitudePin.read();
         pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);