First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_pract3_3_PI_controller by Gerhard Berman

Revision:
5:37e230689418
Parent:
4:19e376d31380
Child:
6:3c4f3f2ce54f
--- a/main.cpp	Mon Oct 17 12:52:16 2016 +0000
+++ b/main.cpp	Mon Oct 17 13:10:20 2016 +0000
@@ -19,7 +19,7 @@
 //set settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-//HIDScope    scope(2);
+HIDScope    scope(2);
 
 //set datatypes
 int counts = 0;
@@ -76,14 +76,19 @@
     float EncoderPosition = counts/resolution;         //position in radians, encoder axis
     float Position = EncoderPosition*inverse_gear_ratio;        //position in radians, motor axis
     // linear feedback control
+    scope.set(0,referencePosition);
+    scope.set(1,Position);
+    scope.send();
     float error = referencePosition - Position; // 'error' in radians
     float Kp = 1; //potMeter2.read();
     
     float IntError = IntError + error*t_sample;
-    float Ki = potMeter2.read();
+    float maxKi = 0.2;
+    float Ki = potMeter2.read()*maxKi;
     
     //float DerivativeError = (error_prev + error)/t_sample;
-    //float Kd = 1;
+    float maxKd = 0.2;
+    //float Kd = potMeter2.read()*maxKd;
     
     float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd;
     pc.printf("Motor Axis Position: %f rad \r\n", Position);