Diff: main.cpp

Revision:

32:3b00eafcf168

Parent:

30:676ecd59467a

--- a/main.cpp	Fri Oct 28 09:31:08 2016 +0000
+++ b/main.cpp	Tue Nov 01 10:13:28 2016 +0000
@@ -41,7 +41,7 @@
 //library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-HIDScope    scope(6);
+//HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -98,7 +98,7 @@
 float EMGgain = 1.0;
 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
-float MotorSpeedGain = 5.0; //to give the motor higher speeds
+float MotorSpeedGain = 1.0; //to give the motor higher speeds
 float t_sample = 0.01; //seconds
 const float maxStampDistance = 1.5;
 
@@ -288,7 +288,7 @@
     // linear feedback control
     float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
     float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
-    float Kp = 1; //potMeter2.read();
+    float Kp = 1.0; //potMeter2.read();
 
     float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
     float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
@@ -334,10 +334,7 @@
     motorValue1Out = MotorSpeedGain*motorValue1Small;
     motorValue2Out = MotorSpeedGain*motorValue2Small;
     
-    TotalError1_prev = TotalError1;
-    TotalError2_prev = TotalError2;
-    
-    
+    /*
     scope.set(0,q1_ref);
     scope.set(1,q1);
     scope.set(2,q2_ref);
@@ -345,28 +342,25 @@
     scope.set(4,motorValue1Out);
     scope.set(5,motorValue2Out);
     scope.send();
-    
+    */
+    /*
     pc.printf("error1: %f \r\n", q1_error);
     pc.printf("IntError1: %f \r\n", q1IntError);
     pc.printf("DerError1: %f \r\n", q1DerivativeError);
     pc.printf("error2: %f \r\n", q2_error);
     pc.printf("IntError2: %f \r\n", q2IntError);
     pc.printf("DerError2: %f \r\n", q2DerivativeError);
-    
-    /*    
+    */
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
     TotalError1_prev = TotalError1;
     TotalError1_prev = TotalError1;
-    */
-     
-    /*
-    scope.set(0,q1);
-    scope.set(1,q2);
-    scope.set(2,biceps_l);
-    scope.set(3,biceps_r);
-    scope.send();
-    */ 
+    
+    pc.printf("TE1: %f  ", TotalError1);
+    pc.printf("TE2: %f  ", TotalError2);
+    pc.printf("M1: %f   ", motorValue1Out);
+    pc.printf("M2: %f   ", motorValue2Out);
+      
 }
 
 void SetMotor1(float motorValue1, float motorValue2)