Diff: main.cpp

Revision:

4:19e376d31380

Parent:

3:8caef4872b0c

Child:

5:37e230689418

--- a/main.cpp	Mon Oct 17 10:39:56 2016 +0000
+++ b/main.cpp	Mon Oct 17 12:52:16 2016 +0000
@@ -24,6 +24,11 @@
 //set datatypes
 int counts = 0;
 double DerivativeCounts;
+
+float error_prev = 0;
+float IntError = 0;
+float t_sample = 0.01; //seconds
+
 int countsPrev = 0;
 float referenceVelocity = 0;
 float bqcDerivativeCounts = 0;
@@ -48,9 +53,10 @@
 
 //define encoder counts and degrees
 QEI Encoder(D12, D13, NC, 32); // turns on encoder
-const int counts_per_revolution = 4200;
-const int gear_ratio = 131;
-const float resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian
+const int counts_per_revolution = 4200; //counts per motor axis revolution
+const int inverse_gear_ratio = 131;
+//const float motor_axial_resolution = counts_per_revolution/(2*PI);
+const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis
 
 float GetReferencePosition()
 {
@@ -60,24 +66,32 @@
     float Potmeter1 = potMeter1.read();
     float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians 
     pc.printf("Max Position: %f rad \r\n", maxPosition);
-    //pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
-    pc.printf("Ref Position: %f rad \r\n", referencePosition);
+    pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
+    pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition);
     return referencePosition;
 }
 
 float FeedForwardControl(float referencePosition)
 {
-    //QEI Encoder(D13, D12, NC, 32); // turns on encoder
-    //int counts = Encoder.getPulses();           // gives position of encoder 
-    float Position = counts/resolution;         //position in radians
+    float EncoderPosition = counts/resolution;         //position in radians, encoder axis
+    float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
     // linear feedback control
-    float ControllerInput = referencePosition - Position; // 'error' in radians
-    float Kp = potMeter2.read(); //Potmeter2;
-    float motorValue = ControllerInput * Kp;
-    pc.printf("Position: %f rad \r\n", Position);
-    pc.printf("Counts: %i rad \r\n", counts);
+    float error = referencePosition - Position; // 'error' in radians
+    float Kp = 1; //potMeter2.read();
+    
+    float IntError = IntError + error*t_sample;
+    float Ki = potMeter2.read();
+    
+    //float DerivativeError = (error_prev + error)/t_sample;
+    //float Kd = 1;
+    
+    float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
+    pc.printf("Motor Axis Position: %f rad \r\n", Position);
+    pc.printf("Counts encoder: %i rad \r\n", counts);
     pc.printf("Kp: %f \r\n", Kp);
     pc.printf("MotorValue: %f \r\n", motorValue);
+    
+    error_prev = error;
     return motorValue;
 }
 
@@ -175,7 +189,7 @@
  pc.baud(115200);
  pc.printf("Test putty");
  //float Potmeter = potMeterIn.read();
- MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
+ MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
  bqc.add(&bq1).add(&bq2);
  //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz
  //TimeTracker.attach(&TimeTracker_act, 1.0f);